Multi-robotic arm collaborative obstacle avoidance, rearrangement, and grasping method and system for crowded scenarios

By constructing a hierarchical graph structure and using a heuristic scoring method, the coordination problem of obstacle removal and target grasping in multi-robotic arm collaborative rearrangement grasping was solved, achieving efficient obstacle removal and target grasping and improving task execution efficiency.

CN123103644APending Publication Date: 2026-11-17TIANJIN UNIVERSITY OF TECHNOLOGY +2
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611306515.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-08-26
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

In multi-robotic arm collaborative rearrangement grasping tasks, existing technologies struggle to effectively coordinate multiple robotic arms to remove obstacles and grasp targets in crowded scenarios. Furthermore, high-level task decisions and low-level motion planning are difficult to connect effectively, resulting in low execution efficiency.

Method used

By constructing a hierarchical graph structure, including an object graph and an action graph, and combining the expansion interaction radius, VFH neighborhood influence radius, and symbolic distance field for filtering, a unified grasping utility function and obstacle removal utility function are used for heuristic scoring. Action requests are generated by combining action feasibility masks and policy networks, joint action allocation is performed, and motion feasibility is verified.

Benefits of technology

It effectively compresses the candidate action space, improves decision response speed, avoids repeated removal and path conflicts, ensures the executability and collaborative rationality of actions, and improves the efficiency of multi-robotic arm collaborative rearrangement grasping.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123103644A_ABST
    Figure CN123103644A_ABST
Patent Text Reader

Abstract

This invention relates to a multi-robotic arm collaborative obstacle avoidance, rearrangement, and grasping method and system for congested scenarios, belonging to the field of robotic arm control technology. The method includes the following steps: acquiring scene state information within the current workspace; constructing a hierarchical graph structure composed of an object graph and an action graph, and generating three candidate actions—direct grasping, angle grasping, and obstacle removal—through progressive filtering using the expansion interaction radius, VFH neighborhood influence radius, and SDF safety gap; using a unified grasping utility function and obstacle removal utility function to heuristically score the candidate actions, converting the scored candidate actions into structured observations and combining them with an action feasibility mask input to a policy network, obtaining the optimal joint action through joint action allocation; generating a collision-free execution trajectory through motion feasibility verification, with the robotic arm synchronously executing the grasping action and updating the scene state. This invention can improve the task completion rate in highly congested scenarios and reduce rearrangement costs and motion expenses.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention belongs to the field of robotic arm control technology, specifically relating to a multi-robotic arm collaborative obstacle avoidance, rearrangement, and grasping method and system for crowded scenarios. Background Technology

[0002] In applications such as warehouse sorting, industrial assembly, laboratory automation, and service robot operation, the target object to be grasped may be obstructed or blocked by multiple surrounding objects, making it difficult for the robotic arm to complete the grasp through a single direct approach. For such congested scenarios, it is usually necessary to first remove or rearrange some of the obstacles that are hindering the grasp, gradually releasing the robotic arm's approach space and grasping channel, and then complete the grasp of the target object. This type of task can be called a rearrangement grasping task in a congested scenario.

[0003] Existing technical solutions for rearrangement and grasping in congested scenes are mostly geared towards single-arm systems. They typically rely on geometric relationships between objects, heuristic evaluation, graph search, tree search, or task and motion planning methods to determine the obstacles to be removed and their operational sequence, and further generate the grasping or removal trajectory of the robotic arm. While these methods can address the problem of progressive rearrangement when the target object is occluded under single-arm conditions, when multiple robotic arms collaborate to perform rearrangement and grasping tasks in a shared workspace, further consideration is needed regarding task allocation between different robotic arms, object selection, execution conflicts, and motion collisions. Existing planning methods for single-arm systems are difficult to directly apply to such multi-arm collaborative scenarios. Furthermore, during the progressive rearrangement process in congested scenes, the composition and spatial relationships of objects in the scene may change with each obstacle removal action. Consequently, the accessibility of the target object, available grasping directions, and the candidate actions that each robotic arm can execute will also change. Therefore, multi-arm systems need to repeatedly perform action generation, action allocation, and feasibility assessment based on the current scene state. If independent strategies are used to select actions for each robotic arm, issues may arise such as multiple arms selecting the same or conflicting targets, some arms having no effective tasks, and interference between arm movement paths. If a joint search is performed directly among all robotic arms and all candidate actions, the number of combined action combinations to be searched increases with the number of robotic arms, target objects, and obstacles, thus increasing the computational load. Furthermore, in existing rearrangement grasping schemes, high-level task decision-making and low-level robotic arm motion planning are typically completed at different stages. High-level task planning mainly determines the target and type of operation the robotic arm needs to perform, while specific execution also needs to meet conditions such as the robotic arm's workspace, inverse kinematics, joint range of motion, and collision constraints. If the above-mentioned motion feasibility is not fully considered during the high-level decision-making process, the determined candidate actions may be unable to be executed in the subsequent motion planning stage due to the lack of joint configurations that meet the constraints or collision-free motion trajectories, thus requiring re-selection or replanning of actions, affecting the execution efficiency of multi-robotic arm rearrangement grasping tasks.

[0004] Therefore, how to address the characteristic that object relationships continuously change during the rearrangement process in crowded scenarios, while controlling the scale of joint decision-making, coordinate multiple robotic arms to remove obstacles and grasp targets, and effectively connect task-level action decisions with the actual feasibility of robotic arm movements, is a problem that needs to be solved in multi-robotic arm collaborative rearrangement and grasping. Summary of the Invention

[0005] To achieve the above objectives, the present invention employs the following technical solution: This invention provides a multi-robotic arm collaborative obstacle avoidance and rearrangement grasping method for crowded scenarios, comprising the following steps: S1. Obtain scene status information in the current workspace; the scene status information includes robotic arm status information, target object status information, obstacle status information, and geometric relationship information; S2. Construct a hierarchical graph structure based on the current scene state, including an object graph and an action graph; during the object graph construction process, perform step-by-step filtering by expanding the interaction radius, the VFH neighborhood influence radius, and the SDF safety gap; S3. Perform heuristic scoring on executable candidate actions. A unified grasping utility function is used for direct grasping actions and angle grasping actions, and a removal utility function is used for obstacle removal actions to obtain heuristic score values ​​for each candidate action. S4. Convert the scored executable candidate actions into candidate center structured observations, and combine them with the action feasibility mask input to the policy network to obtain action request vectors for multiple robotic arms; perform joint action allocation on the action request vectors to obtain the final joint action; S5. Verify the feasibility of the final combined action to obtain the final collision-free trajectory that each robotic arm can execute. S6. The robotic arm performs a grasping action based on the generated trajectory. After the execution is completed, the execution result is determined and the scene state is updated.

[0006] Step S1 specifically includes: Assume there are a total of A robotic arm, a collection of robotic arms Represented as: ,in, Indicates the first The robotic arm; the first Status information of each robotic arm Represented as: ,in, , , , , , They represent the first The base position, current joint angle vector, end effector position, equivalent size of the end effector, reachable workspace, and current execution state of each robotic arm; Let the set of current target objects be: ,in, Represents the set of objects currently being targeted. Indicates the first The target object; the first State information of the target object Represented as: ,in, , , , They represent the first The center position, equivalent radius, priority, and activity status of each target object; Let the current set of obstacles be: ,in, Represents the set of obstacles currently in use. Indicates the first The first obstacle; the second Status information of each obstacle Represented as: ,in, , , They represent the first The center position, equivalent radius, and state of each obstacle.

[0007] In step S2, constructing the object graph specifically includes: An object graph is constructed using a two-dimensional node-edge relationship graph method, where nodes represent scene entities and edges represent reachability, adjacency, occlusion, or interaction relationships between entities; the object graph is represented as follows. ,in, Represents a collection of nodes in an object graph. The object graph represents the set of edges; the object graph nodes include three categories: robotic arm nodes, target object nodes, and obstacle nodes; robotic arm nodes Corresponding robotic arm The characteristics of the robotic arm node include the position of the robotic arm base, the position of the end effector, the current joint state, the reachability, and the idle state; the target object node... Corresponding target object The target object node features include the target center location, equivalent radius, activity status, and priority; obstacle nodes... Corresponding obstacles The obstacle node features include the obstacle center position, equivalent radius, and obstacle state; the object graph edges include robot arm-target object edges, robot arm-obstacle edges, target object-obstacle edges, obstacle-obstacle edges, and robot arm-robot arm edges.

[0008] In step S2, constructing the action graph specifically includes: Action GIF Represented as: ,in, Represents a set of action graph nodes. Represents the set of edges in the action graph; each candidate action is uniformly represented as: , in, Indicates the first One candidate action; This refers to the robotic arm that performs the action; Indicates the object being operated on; Indicates the action type, including direct grab, angled grab, and obstacle removal; Indicates the prerequisite for the action to be performed; This indicates the state change after the action is successfully executed; Indicates the cost of performing the action; Indicates the expected duration of the action; generated candidate actions include grab action, angle grab action, and obstacle removal action.

[0009] Furthermore, a direct grasping action means that the robotic arm directly approaches the target object along the nominal approach direction and completes the grasping; for the robotic arm and target object For robotic arm-target object pairs that meet the conditions for generating direct grasping actions, corresponding direct grasping action edges are generated in the motion graph, forming a set of direct grasping action edges. The conditions for generating a direct grabbing action are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment The minimum distance; if the following condition is met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. This represents the minimum distance from the center of the obstacle to the line segment that approaches it. Indicates the radius of the obstacle; This represents the equivalent radius of action of the robotic arm's end effector or gripper on a two-dimensional plane. Indicates a safety margin; Condition 3: In the local neighborhood of the target, the symbolic distance field SDF safety gap is greater than a preset safety threshold. .

[0010] Furthermore, when the target object is blocked from direct approach along the nominal approach direction and there is at least one feasible approach sector, an angle grabbing action is generated, ultimately resulting in a set of angle grabbing action edges. The conditions for generating the angle grabbing action are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment minimum distance If the following conditions are met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. Indicates the radius of the obstacle; Condition 3: In the local neighborhood of the target, the symbolic distance field SDF safety gap is greater than a preset safety threshold. ; Condition 4: There is a feasible access sector around the target that meets the safety clearance requirements.

[0011] Furthermore, for robotic arms Target object and obstacles An obstacle removal action will be generated if the following conditions are met simultaneously: Condition 1: Obstacles Located in the target object Within the local neighborhood, that is, the distance from the obstacle to the target object is less than or equal to the VFH neighborhood influence radius. Among them, the influence radius of the VFH neighborhood is: ,in, This represents the neighborhood influence radius of the vector field histogram. This represents the minimum angular resolution when discretizing the VFH direction. Indicates the radius of the expansion interaction; Condition 2: Obstacles In VFH direction analysis, it occupies one or more candidate approach directions of the target object; Condition 3: Obstacles Located in the robotic arm Within the accessible workspace.

[0012] Step S3 specifically includes: For both direct grabbing and angle-based grabbing actions, a unified grabbing utility function is used: , in, This indicates the score for the grabbing action. This represents a candidate grab action edge in the action graph; This represents the set of edges that are directly captured. This represents the set of edges representing the angle-grabbing action. Indicates the target to be captured Basic returns; Indicates the target to be captured The scarcity of opportunities to seize; This represents a fixed reward for a direct grabbing action; This indicates the angle quality item for the angle-grabbing action; Indicates the cost of movement in an action; Indicates the target scarcity weight; Indicates the weight of the motion cost; For obstacle removal actions, the obstacle removal utility function is used: in, This indicates the score for the obstacle removal action. This indicates the expected unlocking rewards for the obstacle removal candidate action; Indicates the execution of the robotic arm The relative level of free time; This represents the motion cost of performing a candidate action to remove an obstacle; This indicates that the penalty for recursively removing the penalty is being applied. Indicates the utilization weight of the idle robotic arm; This represents the weight of the motion cost.

[0013] Step S4 specifically includes: Assume each robotic arm retains If there are 10 visible candidate actions, then the structured observation is defined as: , in, This represents the structured observations input to the policy network at the current time. Represents global scene features; Indicates a joint context across robotic arms; Indicates the first Local motion availability characteristics of a robotic arm; Indicates the first The first robotic arm Each candidate action feature; Indicates the number of robotic arms; This indicates the number of candidate actions retained by each robotic arm; The final joint action was determined in the following way: in, The joint action representing the final choice of the policy network output; Represents the set of feasible joint actions; Indicates the first The single-arm movement of a robotic arm in a combined motion; This illustrates the heuristic effect of single-arm movements. Indicates the synergistic benefits of joint actions; This indicates a penalty for redundant actions.

[0014] Step S5 specifically includes: In the joint motion, the high-level motions corresponding to each robotic arm are solved by inverse kinematics and target pose collision detection to obtain the target joint configuration that satisfies the joint constraints. A linear interpolation trajectory in joint space is constructed between the current joint configuration and the target joint configuration, and local path collision detection is performed on each interpolation state. If the linear interpolation trajectory satisfies the collision-free constraint, it is used as a candidate execution trajectory. If the linear interpolation trajectory has a collision, the RRT-Connect sampling motion planning algorithm is called to generate an obstacle avoidance trajectory. Environmental collision detection and inter-robotic arm collision detection are performed on the trajectory of each robotic arm. The motion feasibility of the joint motion during the execution process is comprehensively judged to obtain the final collision-free trajectory that each robotic arm can execute.

[0015] This invention also provides a multi-robotic arm cooperative obstacle avoidance and rearrangement grasping system for congested scenarios, which executes the aforementioned multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios, including: Scene perception module: used to acquire scene state information in the current workspace; the scene state information includes robotic arm state information, target object state information, obstacle state information, and geometric relationship information; Hierarchical Graph Construction Module: Used to construct a hierarchical graph structure based on the current scene state, including object graphs and action graphs; Heuristic scoring module: Used to perform heuristic scoring on executable candidate actions. Direct grabbing and angle grabbing actions use a unified grabbing utility function, while obstacle removal actions use a removal utility function to obtain heuristic score values ​​for each candidate action. Collaborative decision-making module: It is used to convert the scored executable candidate actions into candidate center structured observations, and combine them with action feasibility masks to input the policy network to obtain action request vectors of multiple robotic arms; it performs joint action allocation on the action request vectors to obtain the final joint action; Motion verification module: used to verify the motion feasibility of the final combined action and obtain the final collision-free trajectory that each robotic arm can execute; Execution control module: used to synchronously execute the motion trajectories of multiple robotic arms. The robotic arms perform grasping actions according to the generated trajectories. After execution, the module determines the execution result and updates the scene status.

[0016] The advantages of this invention are: This invention constructs a hierarchical graph structure composed of an object graph and an action graph, transforming the spatial dependencies of scene entities into executable candidate actions. It combines expanded interaction radius, VFH neighborhood pruning, and symbolic distance field to achieve hierarchical local geometric filtering, effectively compressing the candidate action space and improving decision-making response speed. Heuristic scoring is performed using a unified grasping utility function and an obstacle removal utility function. Action requests are generated by combining action feasibility masks and a policy network, and conflict filtering and collaborative benefit evaluation are completed through joint action allocation, balancing decision-making efficiency and the rationality of multi-arm collaboration, thus avoiding duplicate removal and path conflict issues. A motion feasibility verification step is introduced to achieve closed-loop verification between high-level decision-making and low-level motion, ensuring action executability. Attached Figure Description

[0017] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof.

[0018] Figure 1 This is a flowchart of the steps of the method of the present invention; Figure 2 This is a comparison chart of the task completion rates of the method of this invention and existing methods in highly congested scenarios; Figure 3 This is a comparison chart showing the average number of obstacles to be removed required to complete a task in a highly congested scenario, between the method of this invention and existing methods. Figure 4 This figure shows a comparison of the motion costs of the method of the present invention and existing methods in highly congested scenarios. Detailed Implementation

[0019] 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, and 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.

[0020] Example 1 In this embodiment, as Figure 1 As shown, this invention provides a multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for crowded scenarios, the specific steps of which include: S1. Obtain scene status information in the current workspace; Specifically, the scene state information includes robotic arm state information, target object state information, obstacle state information, and geometric relationship information; The robotic arm status information includes the robotic arm base position, current joint angle vector, end effector position, end effector equivalent radius, reachable workspace, and current execution status; The target object status information includes the target object's center position, the target object's equivalent radius, the target object's priority, and the target object's activity status (whether it still needs to be captured). The obstacle status information includes the obstacle's center position, the obstacle's equivalent radius, and the obstacle's status (whether it still exists in the crowded scene). The geometric relationship information is calculated from basic information such as the state information of the robotic arm, the state information of the target object, and the state information of the obstacle; Assume there are a total of A robotic arm, a collection of robotic arms Represented as: ,in, Indicates the first The robotic arm; the first Status information of each robotic arm Represented as: ,in, , , , , , They represent the first The base position, current joint angle vector, end effector position (projection position of the end effector in the two-dimensional workspace), equivalent size of the end effector (equivalent radius of action of the end effector or gripper on the two-dimensional plane), reachable workspace, and current execution status (idle, executing, or action failed) of each robotic arm. The robot arm's base position, current joint angles, and end effector position can be obtained from the robot control system, simulation environment, or forward kinematics calculations. The robot arm's reachable workspace can be pre-calculated based on the robot arm's base position, link length, joint angle range, and end effector's working range, or it can be determined online through inverse kinematics queries.

[0021] Let the set of current target objects be: ,in, Represents the set of objects currently being targeted. Indicates the first The first target object that was not successfully captured; Status information of target objects that were not successfully captured. Represented as: ,in, , , , They represent the first The center position, equivalent radius, priority (if all target objects have the same priority, the same constant can be used; if different targets have different task importance, it can be preset by task requirements), and activity status (used to indicate whether the target object is still in the state of waiting to be grabbed) of each ungrabbed target object. Let the current set of obstacles be: ,in, Represents the set of obstacles currently in use. Indicates the first One obstacle still exists in crowded scenes; the first Status information of obstacles that still exist in crowded scenes. Represented as: ,in, , , They represent the first The center position, equivalent radius, and obstacle status (indicating whether the obstacle is still in the set of active obstacles) of each obstacle still existing in the crowded scene; if the obstacle has been successfully removed, then remove it from the list of active obstacles. Deleted from the list, it will no longer participate in the subsequent construction of object graphs and action graphs; Geometric relationship information includes the reachability between the robotic arm and the object, the distribution of obstacles around the target object, whether the straight path for the robotic arm to approach the target is blocked, whether there are feasible approach directions in the local neighborhood of the target object, whether obstacles will affect target grasping, and whether there is shared workspace and potential conflicts among multiple robotic arms. These relationships serve as the basis for subsequent object graph construction and motion graph generation.

[0022] This invention primarily focuses on geometric modeling of two-dimensional congested scenarios, thus allowing target objects and obstacles to be approximated as two-dimensional disks. For actual non-circular objects, approximations can be made using the circumcircle, minimum bounding circle, collision bounding circle, or equivalent disk radius. The object's center position and radius can be derived from simulators, visual inspection, pose estimation, or manual calibration. In a simulation environment, the object's geometric parameters can be directly read; in a real robot system, they can be obtained through camera detection, depth estimation, or model calibration.

[0023] S2. Construct a hierarchical graph structure based on the current scene state, including an object graph and an action graph; during the object graph construction process, perform step-by-step filtering by expanding the interaction radius, the VFH neighborhood influence radius, and the SDF safety gap; In one embodiment, during object graph construction, the expansion interaction radius is used to determine whether obstacles satisfy the geometric conditions that affect the approach direction of the currently interested object; during object graph construction, the following rules are used for step-by-step filtering: First, perform reachability screening for the robotic arm. If the target object or obstacle is not within the reachable workspace of the robotic arm, then a robotic arm-object edge is not established.

[0024] Second, perform expansion interaction radius filtering. For the currently interested object, if the distance from the center of the obstacle to the center of the currently interested object is greater than the expansion interaction radius, then the obstacle will not participate in the local collision analysis of the interested object.

[0025] Third, perform VFH neighborhood influence radius filtering. If the distance from an obstacle to the currently interested object is greater than the VFH neighborhood influence radius, then the obstacle will not participate in the approach direction occupancy determination at the current angular resolution.

[0026] Fourth, perform SDF (Safety Gap) screening. If all sampling points in a candidate approach direction satisfy the condition that the SDF value is greater than the minimum safety threshold, then that direction is retained as a feasible approach direction; otherwise, it is discarded.

[0027] The local safety gap is calculated using the symbolic distance field. The local safety gap is the minimum collision-free distance between a sampling point in a candidate approach path or candidate approach sector and the nearest obstacle boundary within the local neighborhood around the object of interest.

[0028] Local safety gaps are mainly used to determine the feasibility of candidate approach directions and for scoring angle-grabbing actions.

[0029] The object graph represents the spatial relationships, reachability relationships, occlusion relationships, local neighborhood relationships, and interaction relationships between robotic arms, target objects, and obstacles. An action graph is constructed based on the object graph and local geometric analysis results. The action graph converts the spatial relationships in the object graph into three types of executable candidate actions: direct grasping, angle grasping, and obstacle removal. The action graph converts object-level relationships into executable candidate actions. Object graphs focus on representing scene relationships, answering questions like "who is related to whom, who can reach whom, and who blocks whom?" Action graphs focus on representing actions, answering questions like "which robotic arm can perform what action on which object?" The two are interconnected: the system first constructs an object graph based on the current scene state, and then generates an action graph based on the edge relationships in the object graph and the results of local geometric analysis.

[0030] S21. Object Graph Construction: An object graph is constructed using a two-dimensional node-edge relationship graph construction method. This method is a commonly used graph structure modeling method in robot scene relationship modeling, where nodes represent scene entities and edges represent reachability, adjacency, occlusion, or interaction relationships between entities. The object graph is represented as follows: ,in, Represents a collection of nodes in an object graph. The object graph represents the set of edges; the object graph nodes include three categories: robotic arm nodes, target object nodes, and obstacle nodes; robotic arm nodes Corresponding robotic arm The characteristics of the robotic arm node include the position of the robotic arm base, the position of the end effector, the current joint state, the reachability, and the idle state; the target object node... Corresponding target object The target object node features include the target center location, equivalent radius, activity status, and priority; obstacle nodes... Corresponding obstacles The features of obstacle nodes include the obstacle center location, equivalent radius, and obstacle state; The object graph edges include robot arm-target object edges, robot arm-obstacle edges, target object-obstacle edges, obstacle-obstacle edges, and robot arm-robot arm edges; Robotic arm – The target object side indicates whether the robotic arm can approach and attempt to grasp a target. If the target object… Located in the robotic arm Within the reachable workspace, that is, satisfying: , Then at the robotic arm node and target node An edge is established between the two, and the characteristics of the edge include the Euclidean distance between them, whether the target is within the reach of the robotic arm, whether the straight approach channel is unobstructed, whether there is sufficient safety clearance in the local neighborhood around the target, and whether the robotic arm can generate a direct grasping or angle grasping action on the target. The "robotic arm - obstacle" side indicates whether a robotic arm can reach and remove a certain obstacle. If the obstacle... central position Located in the robotic arm Within the reachable workspace, that is, satisfying: , Then at the robotic arm node and obstacle nodes An edge is established between the robotic arm and the obstacle. The characteristics of this edge include the distance from the robotic arm to the obstacle, whether the obstacle is reachable, whether the obstacle can be removed, and the coarse motion cost of performing the removal action.

[0031] The target object-obstacle edge indicates whether an obstacle is located within the local neighborhood of the target object and may affect the target object's approach direction. For the target object... and obstacles The system determines whether to establish an edge based on the center distance between the two objects and the results of local geometric analysis. If the obstacle is close to the target and may block the target's grabbing direction, then a target-obstacle edge is established. The characteristics of this edge include the distance from the obstacle to the target, the direction angle of the obstacle relative to the target, the proportion of the obstacle in the target's approach sector, and whether the obstacle is a potential obstruction to the target.

[0032] An obstacle-obstacle edge indicates whether two obstacles are within a local area of ​​mutual influence. For example, when a robotic arm removes an obstacle, nearby obstacles may affect the gripper's entry, grasping, or moving path, so local neighborhood relationships need to be preserved in the object graph. If two obstacles are close to each other, and one obstacle may affect the removal of the other obstacle, then an obstacle-obstacle edge is created.

[0033] Robotic arm – Robotic arm edge indicates whether two robotic arms share a workspace or have a potential collision relationship. If the reachable workspaces of the two robotic arms overlap, that is: , Then in the robotic arm and Establish a robotic arm-robotic arm edge between them. This edge is used in the subsequent joint motion allocation and motion verification phases to determine whether multiple robotic arms can perform actions in parallel, and whether it is necessary to filter potential conflicting combinations.

[0034] During object graph construction, the system needs to determine which obstacles are related to the current target of interest or intermediate obstacles, which directions can be used as approach directions, and whether local regions have sufficient safety clearances. To this end, this invention employs the dilated interaction radius, the neighborhood influence range of the vector field histogram, and the symbolic distance field for local geometric analysis.

[0035] S22, Action Graph Construction: Constructing action graphs, action graphs Represented as: ,in, Represents a set of action graph nodes. Represents the set of edges in the action graph; Action graphs are used to convert the robot-object relationships, target-obstacle relationships, and local approach directions in the object graph into executable candidate actions. Each action edge in the action graph corresponds to a candidate action, which is performed by a robot arm on a target object or obstacle.

[0036] Each candidate action is uniformly represented as: in, Indicates the first One candidate action; This refers to the robotic arm that performs the action; This represents the object being manipulated, which can be either the target object or an obstacle. Indicates the action type, including direct grab, angled grab, and obstacle removal; Indicates the prerequisite for the action to be performed; This indicates the state change after the action is successfully executed; Indicates the cost of performing the action; Indicates the expected duration of the action.

[0037] in, The data is derived from object graph edge relationships, robotic arm reachability, VFH direction analysis, and SDF safety clearance determination. For example, a direct grasping action is predicated on the target being reachable and a straight approach to the channel without collision; an angled grasping action is predicated on the target being reachable and the existence of a locally feasible approach sector; an obstacle removal action is predicated on the obstacle being reachable and the obstacle blocking the target's approach direction. This indicates a status update after an action is successful. For example, a target is removed from the active target set after it is successfully captured, or an obstacle is removed from the active obstacle set after it is successfully removed. It is estimated from the distance traveled at the end of the robotic arm, the distance in the joint space, the path length, or the return cost of the motion planner. It is estimated based on trajectory length, execution speed, and action type.

[0038] With action representation, subsequent heuristic scoring, policy network selection, joint action assignment, and motion verification can all be performed on a unified set of candidate actions.

[0039] Candidate action generation Three types of candidate actions are generated: direct grab action, angle grab action, and obstacle removal action; details are as follows: 1. Direct grabbing action generation: A direct grasping action means that the robotic arm directly approaches the target object along the nominal approach direction and completes the grasping; for a robotic arm and target object For robotic arm-target object pairs that meet the conditions for direct grasping actions, corresponding direct grasping action edges are generated in the motion graph, forming a set of direct grasping action edges. The conditions for generating a direct grabbing action are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment The minimum distance; if the following condition is met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. This represents the minimum distance from the center of the obstacle to the line segment that approaches it. Indicates the radius of the obstacle; This represents the equivalent radius of action of the robotic arm's end effector or gripper on a two-dimensional plane. Indicates a safety margin; Condition 3: Within the target's local neighborhood, the SDF safety gap is greater than a preset safety threshold. ; If the grabbing action is successful, the state changes to the target object being removed from the active target set; Prerequisites for executing direct grabbing actions Represented as: in, This indicates that the target object is within the reach (working) range of the robotic arm; This indicates that there is no collision when approaching the passage directly; This indicates that the target has not yet been captured. If the action is successful, the state changes to the target object being removed from the active target set.

[0040] 2. Angle Grabbing Action Generation Angle-grabbing motions are used to handle situations where the forward approach direction to a target object is blocked, but other feasible approach directions still exist in the surrounding area. For robotic arms... and target object If the conditions for direct grasping are not met, the center of the target object will be used. VFH direction analysis is performed with the center as the center. arrive The direction is divided into multiple sectors, and the distribution of surrounding obstacles is used to determine whether each direction is occupied; For a certain candidate direction Several local sampling points are generated around the target object along this direction, and the symbolic distance field value of these sampling points is calculated. If the sampling points within a continuous angular range near this direction satisfy: If so, then the direction is considered to belong to a feasible approach sector, where, The minimum safety clearance threshold is used to constrain the local geometric safety of candidate approach directions. It is determined by the equivalent radius of the robotic arm end effector, the system positioning error, and the control error, and is defined as follows: + , in, This represents the equivalent radius of action of the robotic arm's end effector or gripper in a two-dimensional plane. This represents the pose estimation error margin. This represents the discrete error margin for control and collision detection.

[0041] An angle-grabbing action is generated when the direct approach to the target object along the nominal approach direction is blocked and at least one feasible approach sector exists; the final result is a set of angle-grabbing action edges. If multiple feasible sectors exist, a candidate action is generated for each sector. Alternatively, the top few actions with higher scores can be retained based on sector width, safety clearance, and angle deviation. The conditions for generating angle-grabbing actions are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment minimum distance If the following conditions are met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. Indicates the radius of the obstacle; Condition 3: Within the target's local neighborhood, the SDF safety gap is greater than a preset safety threshold. ; Condition 4: There is a feasible access sector around the target that meets the safety clearance requirements (an additional condition compared to the direct grab condition).

[0042] 3. Obstacle removal action generation The obstacle removal action is used to handle situations where the target is blocked by an obstacle and cannot be directly grasped at present. This action does not generate all obstacles, but only key obstacles that are related to the current target's approach direction and can be reached by the robotic arm.

[0043] For robotic arms Target object and obstacles If the following conditions are met simultaneously, an obstacle removal action will be generated.

[0044] Condition 1: Obstacles Located in the target object Within the local neighborhood, that is, the distance from the obstacle to the target object is less than or equal to the VFH neighborhood influence radius. ; To avoid all obstacles participating in orientation analysis, this invention defines the VFH neighborhood influence radius: ,in, This represents the neighborhood influence radius of the vector field histogram. This represents the minimum angular resolution when discretizing the VFH direction; The precision is determined by the discretization accuracy of the direction histogram. For example, [the following is a possible interpretation:] Evenly divided into For each angle sector, then: .

[0045] For the object of current interest ( (This can be the target object or an intermediate obstacle that needs to be removed later). The expansion interaction radius is used to determine whether surrounding obstacles affect the robotic arm's approach to the currently interested object. The expansion interaction radius is defined as: , in, Indicates the radius of the expansion interaction; Represents the equivalent radius of the target object; This represents the equivalent radius of action of the robotic arm's end effector or gripper on a two-dimensional plane. Indicates a safety margin; The geometric dimensions of the target object or obstacle. If the object is a disk, its radius is taken directly; if the object is not circular, its circumcircle radius or equivalent collision radius is taken. Determined by the structure of the end effector of the robotic arm, the radius of the circumscribed circle when the gripper is open can be taken as either the radius of the circumscribed circle or half of the maximum lateral dimension of the end tool on the two-dimensional plane. It is a preset safety margin used to compensate for visual errors, pose errors, collision detection discrete errors, and control errors. Its value can be set according to the system's safety distance requirements.

[0046] Condition 2: Obstacles In VFH direction analysis, the obstacle occupies one or more candidate approach directions of the target object, meaning that the obstacle has an occupancy effect on the feasible approach sector of the target object. Condition 3: Obstacles Located in the robotic arm Within the accessible workspace, that is, satisfying .

[0047] S3. Perform heuristic scoring on executable candidate actions. A unified grasp utility function is used for direct grasp actions and angle grasp actions, and a removal utility function is used for obstacle removal actions to obtain heuristic score values ​​for each candidate action.

[0048] S31, Grasping Action Scoring For both direct grabbing and angle-based grabbing actions, a unified grabbing utility function is used: , in, This indicates the score for the grabbing action. This represents a candidate grab action edge in the action graph; This represents the set of edges that are directly captured. This represents the set of edges representing the angle-grabbing action. Indicates the target to be captured Basic returns; Indicates the target to be captured The scarcity of opportunities to seize; This represents a fixed reward for a direct grabbing action; This indicates the angle quality item for the angle-grabbing action; Indicates the cost of movement in an action; Indicates the target scarcity weight; Indicates the weight of the motion cost; The It is generated from a robotic arm-target object pair that meets the conditions for direct grasping. Specifically, the generation method is as follows: for any robotic arm With the target object When the target object is within the reachable workspace of the robotic arm, and the straight approach path from the end effector of the robotic arm to the center of the target object is not blocked by obstacles, and the SDF safety clearance in the corresponding approach direction meets the threshold condition, a direct grasping action edge is generated in the motion diagram. All edges that satisfy the conditions form a set. .

[0049] The The robot arm and target object pair that meet the angle grasping conditions are generated. Specifically, when the direct approach direction of the target object is blocked by an obstacle, candidate approach direction sectors are obtained through VFH angle discretization, and corresponding angle grasping action edges are generated in the sectors that meet the SDF safety constraints. All edges that satisfy the conditions form a set. .

[0050] This represents the task value of the target object itself. If all targets in the task have the same priority, then... Set to the same constant; if the targets have different priorities, then Based on target priority The setting is that higher priority goals correspond to greater basic benefits.

[0051] Represents the target object Scarcity of grasping opportunities among multiple robotic arms. If there are few robotic arms capable of grasping the target, the opportunity for that target is scarce and should be given higher priority. This can be defined as: , in, Indicates the current ability to target The number of robotic arms generating candidate actions for direct grasping or angled grasping. This value is determined by the relationship between the action graph and the target. The relevant feasible grasping actions were statistically analyzed. The fewer robotic arms that can grasp the target, the better. The larger.

[0052] This is a fixed reward for the direct grabbing action, used to encourage the system to prioritize actions that can immediately complete the target grabbing. Because direct grabbing does not require the removal of obstacles, it is usually more efficient, hence the fixed reward added to the direct grabbing branch. .

[0053] This is the angular quality term for the angular grasping action, used to evaluate the feasibility of alternative approach directions. Its specific definition is given in the next section.

[0054] This represents the cost of motion. It is used to measure the cost of movement for the robotic arm from its current joint configuration. Configuration of candidate target joints The estimated cost of movement. This indicates the joint space configuration of the robotic arm at the current moment. This represents the target joint configuration corresponding to the candidate grab or remove action, which is obtained by inverse kinematics (IK) or sample-based motion planning (such as RRT).

[0055] , It can also be calculated from the end effector path length, trajectory execution time, or path cost returned by the motion planner. If a certain action requires the robotic arm to move a long distance or bypass complex obstacles, then its The larger the value, the lower the score.

[0056] and These are two different weighting coefficients used to balance target scarcity and motion cost. If the system wants to prioritize targets that only a few robotic arms can grasp, then increase the weighting coefficient. If the system aims to reduce the robotic arm's movement distance or execution cost, then increase... These weights can be obtained through simulation experiments, parameter tuning of verification sets, or empirical settings.

[0057] Angle-based quality assessment: The quality of an angle-grabbing motion depends not only on the presence of an approach direction, but also on the width of the approach sector, the safety clearance, and the approach direction deviation. Therefore, an angle quality term is defined. for: , in, Indicates the angular width of the candidate approach sector; Indicates the minimum SDF safety gap within the candidate proximity sector; This indicates the angular deviation between the candidate approach direction and the direct approach direction of the target object; , and These represent the corresponding normalized reference values.

[0058] This is derived from VFH direction analysis. Continuous feasible directions are merged into approach sectors. The wider the sector angle range, the greater the fault tolerance of the robotic arm when approaching the target from that direction, and therefore the higher the score.

[0059] It is calculated from the symbolic distance field. For a candidate proximity sector, several location points are sampled within that sector, and the distance at each sampled point is calculated. The minimum value among these is taken as the safety clearance for that sector. The larger the safety clearance, the less likely a collision is to occur in that direction, and therefore the higher the score.

[0060] This represents the angular deviation between the candidate approach direction and the direct approach direction of the target object, used to measure the consistency between the current angle of the grasping direction and the ideal straight-line approach direction. If the angular deviation is small, it means that the grasping action at this angle is close to the natural grasping direction and the execution difficulty is low; if the angular deviation is large, it means that the robotic arm needs to enter from a more off-center direction, the execution difficulty is higher, and therefore the score is lowered.

[0061] , and It is a normalized reference value. The maximum feasible sector width or the preset desired sector width can be selected; Take the reference value for the gripper safety clearance; Take the maximum permissible deflection angle. Using these reference values ​​can unify angles, distances, and deviations of different dimensions into a comparable range.

[0062] S32, Obstacle Removal Action Scoring For obstacle removal actions, this invention employs an obstacle removal utility function: in, This indicates the score for the obstacle removal action. This indicates the expected unlocking rewards for the obstacle removal candidate action; Indicates the execution of the robotic arm The relative level of free time; This represents the motion cost of performing a candidate action to remove an obstacle; This indicates that the penalty for recursively removing the penalty is being applied. Indicates the utilization weight of the idle robotic arm; This represents the weight of the motion cost.

[0063] Here This can be understood as either "obstacle removal action edges" or "obstacle removal candidate actions." In implementation, the edges in the action graph connect the executing robotic arm, the obstacle being removed, and the root target served by the removal action; therefore, each edge corresponds to a specific candidate action. To avoid confusion, we will refer to them uniformly as "obstacle removal candidate actions" from now on.

[0064] This indicates the benefit of removing an obstacle to subsequent target grasping. If an obstacle blocks a key approach direction to the target, or simultaneously affects the approach of multiple robotic arms to the target, then removing the obstacle yields a higher unlocking benefit.

[0065] Indicates robotic arm The relative idle level. If a robotic arm currently has no high-value grasping actions, or has been idle recently, its priority for performing obstacle removal actions can be increased, thereby improving the utilization rate of multiple robotic arms. This value can be calculated based on whether the robotic arm is idle, the number of currently available candidate actions, and whether any actions have been performed in the last few steps.

[0066] This indicates a penalty for recursive removal, used to prevent the system from getting stuck in a situation where it continuously removes obstacles but cannot reach the target. A larger penalty is applied if a removal action has a deep dependency hierarchy with the root target, or if the unlocking benefit of that action is weak. This can reduce unnecessary obstacle removal.

[0067] Expected unlocking rewards Defined as (part of the obstacle removal utility function): in, Indicates the target object; Indicates the obstacle that has been removed; Represents the target object The candidate grasping action set is generated jointly by the object graph and the action graph. Specifically, under the conditions of robot arm reachability, unobstructed approach and SDF safety clearance constraint, it is composed of direct grasping action edges and angle grasping action edges generated by robot arm-target object pair. Represents the target object The highest grabbing utility among current or potential candidate grabbing actions; Indicates the candidate grabbing action edge; Indicates obstacles The degree of occupancy of the target's feasible adjacent sector; Indicates an obstacle A collection of robotic arms that block approach directions; This indicates the total number of robotic arms.

[0068] Indicates obstacles The degree of occupancy of feasible approach directions to a target object is calculated based on the VFH direction discretization results. Specifically, the approach directions around the target object are divided into several angular sectors, and obstacles are statistically analyzed. The proportion occupied by these sectors is used as its value.

[0069] Indicates obstacles The proportion of the impact on the overall accessibility of multiple robotic arms. If an obstacle blocks multiple robotic arms simultaneously, this proportion is larger, indicating that the obstacle has a stronger cooperative removal value.

[0070] S4. Convert the scored executable candidate actions into candidate center structured observations, and combine them with the action feasibility mask input to the policy network to obtain action request vectors for multiple robotic arms; perform joint action allocation on the action request vectors to obtain the final joint action; Structured observations of candidate centers: After heuristic scoring of the candidate actions, the highest-scoring actions for each robotic arm are retained based on the scoring results. The system selects candidate actions and constructs structured observations based on these actions, which are then used as input to the policy network. Instead of directly inputting the complete object graph or raw geometry into the policy network, the system first retains the previous action for each robotic arm based on heuristic scoring. We then construct compact action-level observations from a pool of candidate actions. This reduces the search space of the policy network and improves training and decision-making efficiency.

[0071] Assume each robotic arm retains If there are 10 visible candidate actions, then the structured observation is defined as: , in, This represents the structured observations input to the policy network at the current moment, which serve as input for reinforcement learning. It consists of current global scene information, robotic arm state information, and candidate action features for each robotic arm. Represents global scene features; Indicates a joint context across robotic arms; Indicates the first Local motion availability characteristics of a robotic arm; Indicates the first The first robotic arm Each candidate action feature; Indicates the number of robotic arms; This indicates the number of candidate actions retained by each robotic arm.

[0072] This includes the number of current activity targets, the number of activity obstacles, the number of obstacles already removed, the global congestion level, the average goal reachability, and the maximum number of remaining decision steps. This feature helps the policy network understand the overall difficulty and progress of the current task.

[0073] This includes factors such as the overlap of workspaces between robotic arms, whether candidate actions point to the same target, whether multiple robotic arms have opportunities for parallel execution, and whether there are potential conflicts between robotic arms. This feature is used to express the competition and cooperation relationships between multiple robotic arms.

[0074] Including the This feature describes the current available actions of a single robotic arm, including whether it is currently idle, the number of candidate actions it can perform, the highest candidate action score, whether there are direct grasping actions, angular grasping actions, and obstacle removal actions.

[0075] Indicates the first The first robotic arm Each candidate action feature describes the semantic category, geometric feasibility, and heuristic scoring information of the action. This feature consists of three types of information: (1) Action semantic category features: including action type (direct grasp, angle grasp, obstacle removal) and operation object type, used to represent the task semantics of the candidate action; (2) Geometric and motion cost characteristics: including motion cost Accessibility information and approach direction angle deviation are used to describe the difficulty of performing the action; (3) Heuristic rating characteristics: including heuristic utility value The sorting number and the feasibility score obtained based on VFH and SDF are used to indicate the priority of the action in the candidate set.

[0076] Through the structured observations described above, the policy network does not need to directly process all combinations of objects, but instead makes collaborative decisions from a set of candidate actions that has undergone geometric filtering and heuristic compression. (Maskable-PPO) Joint action allocation: The policy network outputs action requests from multiple robotic arms, but these requests cannot be directly used as the final actions. This is because the actions requested independently by multiple robotic arms may be repetitive, conflicting, or lack sufficient collaborative value. For example, two robotic arms may simultaneously request to grasp the same target object, or the movement paths of two robotic arms may intersect and collide. Therefore, it is necessary to perform joint action allocation on the action requests output by the policy network.

[0077] Joint motion assignment first constructs a locally selectable motion set for each robotic arm, which includes motions requested by the policy network, several candidate motions with high heuristic scores, and empty motions. Then, it enumerates or searches multiple combinations of robotic arm motions and performs conflict filtering, duplicate filtering, root goal consistency checks, and collaborative benefit evaluation.

[0078] Conflict filtering eliminates combinations of actions that could lead to collisions or significant path overlap between robotic arms. Duplicate filtering eliminates combinations of multiple robotic arms performing repetitive actions on the same target or obstacle. Root target consistency checking ensures that obstacle removal actions are clearly related to the target object they serve, avoiding the removal of obstacles irrelevant to the current target exposure process. Collaborative benefit assessment encourages multiple robotic arms to perform complementary actions, such as one arm removing an obstacle while another grasps an already exposed target, or multiple robotic arms handling targets and obstacles in different areas.

[0079] The final joint action is determined in the following way: (Action Request Vector -> Joint Action Assignment -> Result) in, Indicates the final chosen joint action; This represents the set of feasible joint actions after conflict filtering and root target consistency checks. Indicates the first The single-arm movement of a robotic arm in a combined motion; This illustrates the heuristic effect of single-arm movements. Indicates the synergistic benefits of joint actions; This indicates a penalty for redundant actions.

[0080] The calculations are based on factors such as the number of non-empty actions in the action combination, whether the actions operate on different objects, whether they improve the accessibility of different targets, and whether they form a complementary "remove-grab" relationship. If multiple robotic arms perform non-conflicting and complementary actions, the collaborative benefits are higher.

[0081] This is used to penalize repetitive grasping and removal, multiple robotic arms competing for the same object, excessive meaningless empty movements, or low-return obstacle removal actions. This can reduce resource waste in multi-robotic arm systems.

[0082] S5. Verify the feasibility of the final combined action to obtain the final collision-free trajectory that each robotic arm can execute. For the high-level actions corresponding to each robotic arm in the joint motion, inverse kinematics solutions and target pose collision detection are performed to obtain the target joint configuration that satisfies the joint constraints. A linear interpolation trajectory in joint space is constructed between the current joint configuration and the target joint configuration, and local path collision detection is performed on each interpolation state. If the linear interpolation trajectory satisfies the collision-free constraint, it is used as a candidate execution trajectory. If the linear interpolation trajectory has a collision, the RRT-Connect sampling motion planning algorithm is called to generate an obstacle avoidance trajectory. Environmental collision detection and inter-robotic collision detection are performed on the trajectory of each robotic arm. The motion feasibility of the joint motion during the execution process is comprehensively judged to obtain the final collision-free trajectory that each robotic arm can execute. The high-level actions refer to task-level discrete operation instructions determined at the task decision layer based on the current scene state, the spatial relationship between the target object and obstacles, and the operability of each robotic arm. These instructions describe which robotic arm, which object, and what operation will be performed. The high-level actions include at least direct object grasping, oblique object grasping, and obstacle removal. The high-level actions only determine the target object and operation type of the robotic arm, without directly providing the specific motion trajectories of each joint. After determining the high-level actions, the target pose of the robotic arm's end effector is determined based on the corresponding target object and operation type. Then, through inverse kinematics solving, collision detection, and motion planning, the joint spatial motion trajectories for executing the high-level actions are generated.

[0083] Feasibility verification of the exercise: After obtaining the final joint motion through joint motion assignment, the system needs to convert the high-level motion into an executable trajectory for the robotic arm. Motion feasibility verification includes inverse kinematics (IK) solving, local path checking, and motion planning.

[0084] For two-dimensional approximations, collision detection can be performed using robotic arm links or end effector disks, line segment capsules, or bounding boxes, by calling the collision detection interface in the simulator or motion planning library. If a collision exists in the target pose or intermediate path, the action cannot be executed directly.

[0085] Linear interpolation trajectory optimization: For actions detected by inverse kinematics and target pose collision, examine the trajectory from the current joint configuration. Configuration to target joint Is a local path feasible? First, construct a linear interpolation trajectory in joint space, and then configure the current joint. Configuration with target joint Linear interpolation is performed between them to generate candidate paths: in, This represents the interpolation joint configuration on the current linear interpolation trajectory in joint space, i.e., determined by the current joint configuration. Configuration with target joint The intermediate state obtained by proportional interpolation in joint space. This indicates the current joint configuration of the robotic arm; Indicates the target joint configuration; Indicates the interpolation ratio. These represent the normalized interpolation coefficients for path parameterization. The system in... Multiple samples within the interval The value is used to perform collision detection for each interpolation joint. If there are no collisions among all interpolation points, a linear interpolation trajectory is generated.

[0086] If a collision occurs on the linear interpolation path, a sampling-based motion planning algorithm, such as RRT-Connect, is invoked to generate an obstacle avoidance trajectory. The trajectory generation process can be represented as follows: in, This represents the final generated trajectory; Represents the linear interpolation trajectory; This represents the trajectory generated by sampling motion planning.

[0087] After completing the inverse kinematics solution and target pose collision detection, to further improve the feasibility of trajectory planning, local path verification is performed on the robotic arm's motion process from the current joint configuration to the target joint configuration. This process, as part of the S5 motion feasibility verification, is used to enhance the stability and success rate of long path planning. Specifically, a linear interpolated trajectory is first constructed in joint space to initially verify whether there is a collision risk in the local path. When the linear interpolated path is not feasible, a staged trajectory is then used for compensation planning.

[0088] S6. The robotic arm performs a grasping action based on the generated trajectory. After the execution is completed, the execution result is determined and the scene status is updated. After execution, the system determines whether the action was successful based on the end effector status, changes in object position, and task success criteria.

[0089] For object grasping actions, if the robotic arm's end effector reaches the target grasping pose, the gripper successfully closes and establishes a stable grasping relationship with the target, and the target object is moved out of the congested area or the grasping success criteria are met, then the target grasp is considered successful. After successful target grasping, the target object is removed from the set of active target objects.

[0090] For obstacle removal, if the robotic arm successfully grasps the obstacle and moves it out of the current congested area, or moves it to a position that no longer obstructs the target's approach, the obstacle removal is considered successful. After successful obstacle removal, the obstacle is removed from the active obstacle set.

[0091] If an action fails, the corresponding target or obstacle remains in the activity set, and the robotic arm, object, action type, and reason for failure are recorded. This failure information can be used in the next round of action masking or scoring to avoid repeatedly selecting the same failed action within a short period.

[0092] The state update process can be written as: in, Indicates the state before execution; Indicates the final chosen joint action; This indicates the actual execution result, including successful target capture, successful obstacle removal, failed action, or partial success of the action; This indicates the state update rules for updating the target set, obstacle set, robotic arm state, and scene geometry based on the execution results.

[0093] After the update, the object graph and action graph are rebuilt, and candidate actions are regenerated. This is because the reachability, approach direction, and local safety clearance of the target object change after each obstacle is removed, so a closed-loop reconstruction is necessary instead of using the old graph structure.

[0094] Example 2 This embodiment provides a multi-robotic arm cooperative obstacle avoidance and rearrangement grasping system for congested scenarios, executing the multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios described in Embodiment 1, including: Scene perception module: used to acquire scene state information in the current workspace; the scene state information includes robotic arm state information, target object state information, obstacle state information, and geometric relationship information; Hierarchical Graph Construction Module: Used to construct a hierarchical graph structure based on the current scene state, including object graphs and action graphs; Heuristic scoring module: Used to perform heuristic scoring on executable candidate actions. Direct grabbing and angle grabbing actions use a unified grabbing utility function, while obstacle removal actions use a removal utility function to obtain heuristic score values ​​for each candidate action. Collaborative decision-making module: It is used to convert the scored executable candidate actions into candidate center structured observations, and combine them with action feasibility masks to input the policy network to obtain action request vectors of multiple robotic arms; it performs joint action allocation on the action request vectors to obtain the final joint action; Motion verification module: used to verify the motion feasibility of the final combined action and obtain the final collision-free trajectory that each robotic arm can execute; Execution control module: used to synchronously execute the motion trajectories of multiple robotic arms. The robotic arms perform grasping actions according to the generated trajectories. After execution, the module determines the execution result and updates the scene status.

[0095] Example 3 To verify the authenticity and beneficial effects of the method of this invention, a two-dimensional congestion grasping experimental scenario was constructed in the PyBullet simulation environment. The experimental scenario included multiple robotic arms, multiple target objects, and randomly distributed obstacles. The robotic arms needed to complete the target object grasping task through direct grasping, angled grasping, or obstacle removal.

[0096] In the experiments, the method of this invention was compared with three existing methods: GMAP, HSMP, and BES-RRT. GMAP is a greedy multi-manipulator planning method, HSMP is a heuristic search combined with motion planning, and BES-RRT is a bounded exhaustive scheduling combined with RRT-Connect motion verification. All methods were tested under the same simulation environment, the same scene generation rules, and the same motion planning backend to ensure fairness in the comparison. The paper also states that the compared methods share the same candidate action generation, motion feasibility verification, and execution backend; the main difference lies in the high-level action selection and collaborative strategy.

[0097] The experimental evaluation metrics include Task Completion Rate (TCR), Decision Steps (DSC), Number of Obstacles Removed (ROC), and Motion Cost (MCS). A higher TCR indicates a higher success rate in target acquisition; lower DSC, ROC, and MCS indicate higher decision-making efficiency, fewer obstacles removed, and lower motion execution cost, respectively.

[0098] Table 1 Comparison of Experimental Results As shown in Table 1, the method of this invention outperforms existing methods in four metrics: task completion rate, number of decision steps, number of obstacles removed, and motion cost. Compared to GMAP and HSMP, the method of this invention reduces unnecessary obstacle removal and high-level decision-making; compared to BES-RRT, the method of this invention significantly improves the task completion rate while maintaining a lower motion cost. These results demonstrate that the method of this invention can more stably complete the target grasping task in crowded scenarios, while reducing rearrangement and motion execution costs.

[0099] Figure 2 The results show a comparison of the task completion rates of the method of this invention and existing methods in a highly crowded scenario. This scenario contains 20 target objects and 50 obstacles, and is a complex scenario with a large number of obstacles and restricted approach directions for target objects.

[0100] Depend on Figure 2 The results show that the task completion rate of the method of this invention is 0.948, which is higher than that of the GMAP method (0.912), the HSMP method (0.906), and the BES-RRT method (0.584). This result indicates that the present invention uses object graphs and motion graphs to perform structured modeling of the relationship between the robotic arm, the target object, and obstacles, and combines heuristic scoring and motion feasibility masks to screen candidate actions. This enables the more stable selection of effective actions in highly crowded environments, thereby improving the success rate of target object grasping.

[0101] Figure 3 The results show a comparison of the average number of obstacles to be removed required to complete a task in a highly congested scenario between the method of this invention and existing methods. A lower average number of obstacles removed indicates that the system can more accurately identify critical obstacles and reduce the removal of irrelevant and redundant obstacles.

[0102] Depend on Figure 3 It can be seen that the average number of obstacles removed by the method of the present invention is 26.840, which is lower than that of the GMAP method (33.624), the HSMP method (29.157), and the BES-RRT method (28.613). This result shows that the present invention, through expansion interaction radius screening, VFH direction occupancy analysis, SDF safety gap calculation, and obstacle removal utility scoring, can prioritize the removal of key obstacles that have a significant impact on the approach direction of the target object, thereby reducing unnecessary rearrangement operations and improving rearrangement efficiency.

[0103] Figure 4 The results show a comparison of motion costs between the method of this invention and existing methods in highly congested scenarios. Motion cost is used to characterize the cumulative path cost during the robotic arm's execution of actions; the lower the motion cost, the shorter the robotic arm's execution path or the higher the overall motion efficiency.

[0104] Depend on Figure 4 It can be seen that the motion cost of the method of the present invention is 216.061, which is lower than that of the GMAP method (295.417), the HSMP method (263.572), and the BES-RRT method (249.166). This result shows that the present invention considers motion cost in the motion scoring stage and further performs joint motion allocation, motion feasibility verification, and trajectory synchronization execution after strategy output, which can reduce invalid motions and redundant motion paths, thereby reducing the overall motion cost in the multi-robotic arm execution process.

[0105] In conclusion, Figures 2 to 4 The beneficial effects of this invention were verified from three aspects: task success rate, rearrangement efficiency, and motion execution cost. Experimental results show that the method of this invention can not only improve the target grasping success rate in highly crowded scenarios, but also reduce the number of obstacles removed and the motion cost of the robotic arm, proving that this invention has better task completion ability, decision-making efficiency, and execution efficiency in multi-robotic arm collaborative obstacle avoidance, rearrangement, and grasping tasks.

[0106] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-robot arm cooperative obstacle avoidance rearrangement grabbing method for crowded scenes, characterized in that, Includes the following steps: S1. Obtain scene status information in the current workspace; The scene state information includes robotic arm state information, target object state information, obstacle state information, and geometric relationship information; S2. Construct a hierarchical graph structure based on the current scene state, including an object graph and an action graph; during the object graph construction process, perform step-by-step filtering by expanding the interaction radius, the VFH neighborhood influence radius, and the SDF safety gap; S3. Perform heuristic scoring on executable candidate actions. A unified grasping utility function is used for direct grasping actions and angle grasping actions, and a removal utility function is used for obstacle removal actions to obtain heuristic score values ​​for each candidate action. S4. Convert the scored executable candidate actions into candidate center structured observations, and combine them with the action feasibility mask input to the policy network to obtain action request vectors for multiple robotic arms; perform joint action allocation on the action request vectors to obtain the final joint action; S5. Verify the motion feasibility of the final combined action to obtain the final collision-free trajectory that each robotic arm can execute. S6. The robotic arm performs a grasping action based on the generated trajectory. After the execution is completed, the execution result is determined and the scene status is updated.

2. The crowded scene oriented multi-manipulator cooperative obstacle avoidance rearrangement and grasping method according to claim 1, characterized in that, Step S1 specifically includes: Assume there are a total of A robotic arm, a collection of robotic arms Represented as: ,in, Indicates the first The robotic arm; the first Status information of each robotic arm Represented as: ,in, , , , , , They represent the first The base position, current joint angle vector, end effector position, equivalent size of the end effector, reachable workspace, and current execution state of each robotic arm; Let the set of current target objects be: ,in, Represents the set of objects currently being targeted. Indicates the first The target object; the first State information of the target object Represented as: ,in, , , , They represent the first The center position, equivalent radius, priority, and activity status of each target object; Let the current set of obstacles be: ,in, Represents the current set of obstacles. Indicates the first The first obstacle; the second Status information of each obstacle Represented as: ,in, , , They represent the first The center position, equivalent radius, and state of each obstacle.

3. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 1, characterized in that, In step S2, construct the object graph: An object graph is constructed using a two-dimensional node-edge relationship graph method, where nodes represent scene entities and edges represent reachability, adjacency, occlusion, or interaction relationships between entities; the object graph is represented as follows. ,in, Represents a collection of nodes in an object graph. Represents the set of edges in the object graph; the nodes in the object graph include three types: robotic arm nodes, target object nodes, and obstacle nodes; Building an action graph: Action GIF Represented as: ,in, Represents a set of action graph nodes. Represents the set of edges in the action graph; each candidate action is uniformly represented as: , in, Indicates the first One candidate action; This refers to the robotic arm that performs the action; Indicates the object being operated on; Indicates the action type, including direct grab, angled grab, and obstacle removal; Indicates the prerequisite for the action to be performed; This indicates the state change after the action is successfully executed; Indicates the cost of performing the action; Indicates the expected duration of the action; generated candidate actions include grab action, angle grab action, and obstacle removal action.

4. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 3, characterized in that, A direct grasping action means that the robotic arm directly approaches the target object along the nominal approach direction and completes the grasping; for a robotic arm and target object For robotic arm-target object pairs that meet the conditions for generating direct grasping actions, corresponding direct grasping action edges are generated in the motion graph, forming a set of direct grasping action edges. The conditions for generating a direct grabbing action are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment The minimum distance; if the following condition is met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. This represents the minimum distance from the center of the obstacle to the line segment that approaches it. Indicates the radius of the obstacle; This represents the equivalent radius of action of the robotic arm's end effector or gripper on a two-dimensional plane. Indicates a safety margin; Condition 3: In the target local neighborhood, the signed distance field SDF safety margin is greater than a preset safety threshold .

5. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 3, characterized in that, An angle-grabbing action is generated when the direct approach to the target object along the nominal approach direction is blocked and there is at least one feasible approach sector; the final result is the set of angle-grabbing action edges; the conditions for generating the angle-grabbing action are as follows: Condition 1: Target object Located in the robotic arm Within the accessible workspace, meet ; Condition 2: There are no obstacles or collisions along the straight-line approach path from the current end-effector position of the robotic arm to the target object; specifically, the straight-line approach path is approximated as a line segment. For each obstacle Calculate the distance from its center to the line segment minimum distance If the following conditions are met for all obstacles: If the straight line is close to the unobstructed passage, then it is considered that the passage is unobstructed. Indicates the radius of the obstacle; Condition 3: In the local neighborhood of the target, the symbolic distance field SDF safety gap is greater than a preset safety threshold. ; Condition 4: There is a feasible access sector around the target that meets the safety clearance requirements.

6. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 3, characterized in that, For robotic arms Target object and obstacles An obstacle removal action will be generated if the following conditions are met simultaneously: Condition 1: Obstacles Located in the target object Within the local neighborhood, that is, the distance from the obstacle to the target object is less than or equal to the VFH neighborhood influence radius. Among them, the influence radius of the VFH neighborhood is: ,in, This represents the neighborhood influence radius of the vector field histogram. This represents the minimum angular resolution when discretizing the VFH direction. Indicates the radius of the expansion interaction; Condition 2: Obstacles In VFH direction analysis, it occupies one or more candidate approach directions of the target object; Condition 3: Obstacles Located in the robotic arm Within the accessible workspace.

7. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 1, characterized in that, Step S3 specifically includes: For both direct grabbing and angle-based grabbing actions, a unified grabbing utility function is used: , in, This indicates the scoring of the grabbing action. This represents a candidate grab action edge in the action graph; This represents the set of edges that are directly captured. This represents the set of edges representing the angle-grabbing action. Indicates the target to be captured Basic returns; Indicates the target to be captured The scarcity of opportunities to seize; This represents a fixed reward for a direct grabbing action; This indicates the angle quality item for the angle grabbing action; Indicates the cost of movement in an action; Indicates the target scarcity weight; Indicates the weight of the motion cost; For obstacle removal actions, the obstacle removal utility function is used: , in, This indicates the score for the obstacle removal action. This indicates the expected unlocking rewards for the obstacle removal candidate action; Indicates the execution of the robotic arm The relative level of free time; This represents the motion cost of performing a candidate action to remove an obstacle; This indicates that the penalty for recursively removing the penalty is being applied. Indicates the utilization weight of the idle robotic arm; This represents the weight of the motion cost.

8. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 1, characterized in that, Step S4 specifically includes: Assume each robotic arm retains If there are 10 visible candidate actions, then the structured observation is defined as: , in, This represents the structured observations input to the policy network at the current time. Represents global scene features; Indicates a joint context across robotic arms; Indicates the first Local motion availability characteristics of a robotic arm; Indicates the first The first robotic arm Each candidate action feature; Indicates the number of robotic arms; This indicates the number of candidate actions retained by each robotic arm; The final joint action was determined in the following way: , in, The joint action representing the final choice of the policy network output; Represents the set of feasible joint actions; Indicates the first The single-arm movement of a robotic arm in a combined motion; This illustrates the heuristic effect of single-arm movements. Indicates the synergistic benefits of joint actions; This indicates a penalty for redundant actions.

9. The multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios according to claim 1, characterized in that, Step S5 specifically includes: In the joint motion, the high-level motions corresponding to each robotic arm are solved by inverse kinematics and target pose collision detection to obtain the target joint configuration that satisfies the joint constraints. A linear interpolation trajectory in joint space is constructed between the current joint configuration and the target joint configuration, and local path collision detection is performed on each interpolation state. If the linear interpolation trajectory satisfies the collision-free constraint, it is used as a candidate execution trajectory. If the linear interpolation trajectory has a collision, the RRT-Connect sampling motion planning algorithm is called to generate an obstacle avoidance trajectory. Environmental collision detection and inter-robotic arm collision detection are performed on the trajectory of each robotic arm. The motion feasibility of the joint motion during the execution process is comprehensively judged to obtain the final collision-free trajectory that each robotic arm can execute.

10. A multi-robotic arm cooperative obstacle avoidance and rearrangement grasping system for congested scenarios, executing the multi-robotic arm cooperative obstacle avoidance and rearrangement grasping method for congested scenarios as described in any one of claims 1-9, characterized in that, include: Scene awareness module: used to acquire scene status information in the current workspace; The scene state information includes robotic arm state information, target object state information, obstacle state information, and geometric relationship information; Hierarchical Graph Construction Module: Used to construct a hierarchical graph structure based on the current scene state, including object graphs and action graphs; Heuristic scoring module: Used to perform heuristic scoring on executable candidate actions. Direct grabbing and angle grabbing actions use a unified grabbing utility function, while obstacle removal actions use a removal utility function to obtain heuristic score values ​​for each candidate action. Collaborative decision-making module: It is used to convert the scored executable candidate actions into candidate center structured observations, and combine them with action feasibility masks to input the policy network to obtain action request vectors of multiple robotic arms; it performs joint action allocation on the action request vectors to obtain the final joint action; Motion verification module: used to verify the motion feasibility of the final combined action and obtain the final collision-free trajectory that each robotic arm can execute; Execution control module: used to synchronously execute the motion trajectories of multiple robotic arms. The robotic arms perform grasping actions according to the generated trajectories. After execution, the module determines the execution result and updates the scene status.