A collision prediction and motion admission control method and system for dual-arm robots

Abstract

The application provides a collision prediction and motion access control method and system of a dual-arm robot, applied to the technical field of dual-arm robots, and overcomes the problem that the path planning of a mechanical arm and the motion constraint of other components of a system are independent of each other in the prior art by establishing a unified collision model containing a chassis, a lifting device, dual arms and a load. By adopting multi-level collision detection (key points, interpolation points and swept regions), blind spots in the trajectory execution process are effectively investigated, and the probability of collision is significantly reduced. In combination with the motion access mechanism of the chassis obstacle avoidance and the lifting safety limiting state, it is ensured that the dual-arm robot only executes actions in the safe state of the whole machine. In addition, the introduction of the rolling rechecking mechanism in the execution process can respond to environmental changes and execution deviations in real time, and has the beneficial effects of improving the safety and stability of the mobile dual-arm operating dual-arm robot in the tasks of grabbing, carrying and moving.

Description

Technical Field

[0001] This application relates to the field of dual-arm robot technology, and in particular to a collision prediction and motion access control method and system for a dual-arm robot. Background Technology

[0002] Mobile dual-arm robots typically consist of a mobile chassis, a lifting mechanism, two robotic arms, an end effector, vision sensors, and a control system. Compared to fixed robotic arms, mobile dual-arm robots exhibit more complex motion patterns. Their chassis can move, the lifting mechanism can change the overall height of the two arms, the left and right robotic arms can perform multi-degree-of-freedom collaborative operations, and the end effector can hold boxes, containers, workpieces, or other loads.

[0003] In actual operation, dual-arm robots may encounter various collision risks. For example, the left and right robotic arms may collide with each other; the robotic arms may collide with the chassis, lifting platform, camera bracket, or the dual-arm robot body; the robotic arms or end-effectors may collide with environmental obstacles; after the dual-arm robot grasps an object, the grasped object itself may also collide with the robot body, robotic arms, or the external environment.

[0004] In existing technologies, robotic arm path planning typically focuses primarily on the reachability of the robotic arm itself and local collision detection. Chassis obstacle avoidance, lifting and limiting, and robotic arm motion planning are often handled by different modules, lacking a unified motion access judgment for the entire machine. Even if the robotic arm generates a seemingly feasible path in the simulation environment, collisions may still occur during actual execution if only discrete target points are detected without detecting the interpolated trajectories or motion sweep areas between adjacent points.

[0005] Furthermore, in dual-arm robot handling tasks, the feasibility of a single robotic arm's path does not guarantee the feasibility of simultaneous execution by both arms. When the left and right robotic arms simultaneously approach, grip, move, or place objects, complex spatial coupling relationships exist between the arm links, end effectors, and the gripped load. Without collision detection during synchronized dual-arm motion, interference or collisions can easily occur during task execution.

[0006] Therefore, there is a need for a method that can integrate the chassis, lifting mechanism, dual robotic arms, end effector, dual-arm robot body, environmental obstacles, and end load into the safety assessment, and perform multi-level collision prediction and motion access assessment of the target motion before the dual-arm robot sends out its actions, thereby improving the safety and reliability of the operation of the mobile dual-arm robot.

[0007] Therefore, existing technologies urgently need to be improved to address the aforementioned problems. Summary of the Invention

[0008] In view of the shortcomings of the prior art, this application provides a collision prediction and motion access control method and system for a dual-arm robot, which aims to solve the problem that the various functional modules of a mobile dual-arm robot, such as chassis obstacle avoidance, lifting limit, and robotic arm collision detection, are independent of each other and lack a unified motion access judgment mechanism, which makes collisions very easy to occur during execution.

[0009] In a first aspect, a collision prediction and motion access control method for a dual-arm robot is provided, the method comprising the following steps: S1: Establish a unified collision model; S2: Obtain the current operating status of the dual-arm robot; S3: Receive the target motion command and generate a candidate motion trajectory in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. S4: Based on the unified collision model, perform multi-level collision detection on the candidate motion trajectory to obtain collision detection results; the multi-level collision detection includes at least collision detection of the trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of the swept region formed by the motion between adjacent trajectory key points. S5: Obtain the obstacle avoidance status of the mobile chassis of the dual-arm robot, the safety limit status of the lifting mechanism, and the synchronous movement status of the two arms; S6: Generate motion access results based on the collision detection results, the obstacle avoidance state, the safety limit state, and the synchronous movement state of the two arms; S7: When the motion admission result is allowed to execute, a motion command is issued to the corresponding execution mechanism. During the execution process, the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory is continuously compared. When the deviation exceeds a preset threshold, the execution is paused and the reconstruction of the unified collision model and the rolling re-verification of the motion admission are performed again.

[0010] Furthermore, in step S1, the unified collision model includes at least a mobile chassis, a lifting mechanism, a left robotic arm, a right robotic arm, an end effector, a dual-arm robot body mounting structure, environmental obstacles, and a spatial envelope representation of the clamped load.

[0011] Furthermore, the current operating state includes at least the mobile chassis state of the dual-arm robot, the height of the lifting mechanism, the joint state of the left robotic arm, the joint state of the right robotic arm, the end effector state, and the safety limit state of the lifting mechanism.

[0012] Furthermore, step S3 includes: S31: Receive the target motion command issued by the upper-level scheduling system or operator, and parse the target motion command into at least one of the following target pose or position information of the dual-arm robot: left robotic arm target pose, right robotic arm target pose, dual-arm synchronous target pose, lifting mechanism target height, or mobile chassis target position. S32: Using the current running state as the initial state and the target pose or position information as the target constraint, call the path planning algorithm in the simulation environment to perform motion planning and generate candidate motion trajectories from the current running state to the target constraint. Each of the key points in the candidate motion trajectory includes at least the target pose or position information of the corresponding component.

[0013] Furthermore, in step S32, the path planning algorithm is at least one of a random sampling algorithm, a tree search algorithm, an interpolation planning algorithm, or an inverse kinematics solution algorithm.

[0014] Furthermore, step S4 includes: S41: Update the position and orientation of the spatial envelope of each component in the unified collision model at each of the trajectory key points, and calculate the first distance between the spatial envelopes of each component. S42: When the first distance is less than the preset safe distance threshold, it is determined that there is a collision risk at the key point of the trajectory; S43: Interpolate between two adjacent trajectory key points to generate an interpolated trajectory point sequence, update the position and orientation of the spatial envelope of each component in the unified collision model at each interpolated trajectory point, and calculate the second distance between the spatial envelopes of each component. S44: When the second distance is less than the safe distance threshold, it is determined that the interpolated trajectory point has a collision risk; S45: Based on the motion between two adjacent trajectory key points, construct the sweeping region spatial envelope formed when each component moves between adjacent trajectory key points, and calculate the third distance between the sweeping region spatial envelope and the spatial envelope of other components in the unified collision model. S46: When the third distance is less than the safe distance threshold, it is determined that there is a collision risk in the trajectory segment; S47: Generate the collision detection result by combining the detection results of each level.

[0015] Furthermore, in step S5, the obstacle avoidance status of the mobile chassis includes at least the real-time distance between the mobile chassis and the obstacle, whether the mobile chassis is performing an obstacle avoidance action, and whether the mobile chassis is in a stopped state. The safety limit status of the lifting mechanism includes at least the current height position of the lifting mechanism and whether the lifting mechanism is in the upper limit position, lower limit position, or near the limit region; the near limit region means that the distance between the current height position of the lifting mechanism and the upper limit position or lower limit position is less than the near threshold. The synchronous motion state of the two arms includes at least whether the left and right robotic arms are in motion at the same time, and the joint positions, link positions, and end positions of the left and right robotic arms at the same time step.

[0016] Furthermore, step S6 includes: S61: When the candidate motion trajectory has a collision risk, or the mobile chassis is in the process of performing obstacle avoidance or in a stopped state, or the lifting mechanism is in the upper limit position, lower limit position or close to the limit position area, or the left robotic arm and the right robotic arm are in motion at the same time, the motion access restriction is triggered. S62: When the condition for triggering motion access restriction is that the candidate motion trajectory has a collision risk, replanning is performed. If a collision risk still exists after replanning, a motion access result that prohibits execution is generated. S63: When the condition for triggering motion access restriction is that the mobile chassis is in the process of performing obstacle avoidance or in a stopped state, generate motion access result for restricted execution; S64: When the condition for triggering motion access restriction is that the lifting mechanism is in the upper limit position, lower limit position or close to the limit position area, generate a motion access result that prohibits execution; S65: When the condition for triggering motion access restriction is that the left robotic arm and the right robotic arm are both in motion, calculate the minimum distance between the joint positions, link positions and end positions of the left robotic arm and the right robotic arm at the same time step. If any of the minimum distances is less than the preset double-arm safe distance threshold, then replanning is performed. If any of the minimum distances after replanning is still less than the preset double-arm safe distance threshold, then a motion access result with restricted execution is generated.

[0017] Furthermore, step S7 includes: S71: During the execution of the candidate motion trajectory, the actual operating state of the dual-arm robot is continuously acquired; S72: Compare the actual running state with the target pose or position information at the corresponding time point in the candidate motion trajectory, and calculate the deviation between the actual running state and the target pose or position information; S73: When the deviation exceeds a preset threshold, pause the execution of the current motion command; S74: Reacquire the current operating state of the dual-arm robot and reconstruct the unified collision model; S75: Based on the reconstructed unified collision model, perform multi-level collision detection again on the remaining trajectory parts of the candidate motion trajectory that have not been executed, and re-acquire the obstacle avoidance status of the mobile chassis, the safety limit status of the lifting mechanism, and the synchronous motion status of the two arms to generate new motion admission results. S76: Based on the new motion admission result, perform the corresponding processing operation, which includes at least continuing execution, replanning, speed-limited execution, reverting to a safe posture, and stopping execution and issuing an alarm.

[0018] Secondly, a collision prediction and motion access control system for a dual-arm robot, used to implement the steps of any of the methods described above, the system comprising: Modeling module: Establish a unified collision model; First acquisition module: Acquires the current operating status of the dual-arm robot; Simulation operation module: Receives target motion command and generates candidate motion trajectory in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. Collision detection module: Based on the unified collision model, multi-level collision detection is performed on the candidate motion trajectory to obtain collision detection results; the multi-level collision detection includes at least collision detection of the trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of the swept area formed by the motion between adjacent trajectory key points. The second acquisition module acquires the obstacle avoidance status of the mobile chassis of the dual-arm robot, the safety limit status of the lifting mechanism, and the synchronous movement status of the two arms. Generation module: Generates motion access results based on the collision detection results, the obstacle avoidance state, the safety limit state, and the synchronous movement state of the two arms; Execution module: When the motion admission result is allowed to execute, it issues motion commands to the corresponding execution mechanism. During the execution process, it continuously compares the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory. When the deviation exceeds a preset threshold, it pauses execution and reconstructs the unified collision model and performs a rolling re-verification of the motion admission.

[0019] Beneficial Effects: This application proposes a collision prediction and motion access control method and system for a dual-arm robot. By establishing a unified collision model including the chassis, lifting mechanism, dual arms, and load, it overcomes the problem in existing technologies where the path planning of the robotic arm and the motion constraints of other system components are independent. Through multi-level collision detection (key points, interpolation points, and swept areas), blind spots in the trajectory execution process are effectively identified, significantly reducing the probability of collisions. Combined with the chassis obstacle avoidance and lifting safety limit motion access mechanism, it ensures that the dual-arm robot performs actions only when the entire machine is in a safe state. Furthermore, the introduction of a rolling re-verification mechanism during execution enables real-time responses to environmental changes and execution deviations, effectively improving the safety and stability of the mobile dual-arm robot in grasping, handling, and movement tasks. Attached Figure Description

[0020] Figure 1 This is a flowchart of a collision prediction and motion access control method for a dual-arm robot proposed in this application.

[0021] Figure 2 This is a structural diagram of a collision prediction and motion access control system for a dual-arm robot proposed in this application.

[0022] Figure 3 This is a perspective view of the dual-arm robot proposed in this application.

[0023] Figure 4 This is a trajectory diagram for the multi-level collision detection proposed in this application.

[0024] Figure 5 This is a schematic diagram of the collision detection between the left and right robotic arms proposed in this application.

[0025] Figure 6 This is a schematic diagram of collision detection between the end effector and the clamped load proposed in this application.

[0026] Labeling Explanation: 1. Dual-arm robot; 2. Left robotic arm; 3. Right robotic arm; 4. Lifting mechanism; 5. Mobile chassis; 6. End effector; 7. Dual-arm robot body mounting structure; 8. Environmental obstacles; 9. Gripped load; 201. Modeling module; 202. First acquisition module; 203. Simulation operation module; 204. Collision detection module; 205. Second acquisition module; 206. Generation module; 207. Execution module. Detailed Implementation

[0027] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and marked in the accompanying drawings can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0028] Mobile dual-arm robots typically consist of a mobile chassis 5, a lifting mechanism 4, two robotic arms, and an end effector 6. They are widely used in warehousing and logistics, industrial manufacturing, and other fields for grasping, handling, placing, and moving tasks. Compared to fixed robotic arms, mobile dual-arm robots have more complex motion patterns. Their chassis can move, the lifting mechanism 4 can change the overall height of the two arms, and the left and right robotic arms can perform multi-degree-of-freedom collaborative operations. In actual operation, dual-arm robots face various collision risks. For example, collisions may occur between the left robotic arm 2 and the right robotic arm 3, or the robotic arms may collide with the chassis, the lifting platform, or the robot body itself, or even with environmental obstacles 8. In existing technologies, robotic arm path planning usually focuses mainly on the mobility and local collision detection of the robotic arm body, while chassis obstacle avoidance, lifting limit, and robotic arm motion planning are often handled separately by different modules, lacking a unified motion access judgment for the entire machine. Even if a seemingly feasible path is generated in the simulation environment, collisions may still occur during actual execution if only discrete target points are detected, ignoring the interpolated trajectories or motion sweep areas between adjacent points. Furthermore, when both arms perform tasks synchronously, there are complex spatial coupling relationships between the arm links and the end effector 6. Without comprehensive safety assessment at the system level, interference or collisions during task execution are highly likely. Therefore, a technical solution is urgently needed that can uniformly incorporate all components into safety assessments, performing multi-level collision prediction and motion access assessments of the target motion before the action is issued.

[0029] Please refer to Figure 1 A collision prediction and motion access control method for a dual-arm robot, the method comprising the following steps: S1: Establish a unified collision model; S2: Obtain the current operating status of the dual-arm robot 1; S3: Receive the target motion command and generate candidate motion trajectories in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. S4: Based on the unified collision model, perform multi-level collision detection on the candidate motion trajectory to obtain collision detection results; multi-level collision detection includes at least collision detection of trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of the swept region formed by the motion between adjacent trajectory key points. S5: Obtain the obstacle avoidance status of the mobile chassis 5 of the dual-arm robot 1, the safety limit status of the lifting mechanism 4, and the synchronous movement status of the two arms. S6: Generate motion access results based on collision detection results, obstacle avoidance status, safety limit status, and synchronized movement status of both arms; S7: When the motion admission result is allowed to execute, a motion command is issued to the corresponding actuator. During the execution process, the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory is continuously compared. When the deviation exceeds the preset threshold, the execution is paused and the unified collision model is reconstructed and the motion admission is re-verified.

[0030] The unified collision model refers to a set of virtual models constructed in a simulation environment to characterize the spatial geometric occupancy of each physical component of a dual-arm robot. It is used to determine, at the computational level, whether spatial interference occurs between different components. This unified collision model is not merely a single geometric object, but includes multiple interconnected component models and their pose update relationships. For example, when the mobile chassis 5 moves in a plane, its model as a whole undergoes translation and rotation; when the lifting mechanism 4 operates, the dual robotic arm models mounted on it are updated synchronously with changes in height; when the end effector 6 clamps the housing, the housing's spatial envelope also participates in subsequent detection as part of the unified collision model. Therefore, the unified collision model reflects the complete spatial occupancy state of the entire robot at a given moment, rather than the static shape of a single local component.

[0031] Multi-level collision detection refers to a mechanism that progressively checks for interference from multiple dimensions, including discrete points, continuous interpolation points, and the spatial volume formed during the motion process. Its core principle is to avoid situations where the starting point and ending point are safe, but the intermediate process is unsafe. For example, when a robotic arm link swings between two key points, although neither key point itself is in contact with an obstacle, the area swung by the link in the middle may have crossed the edge of an adjacent robotic arm or shelf. Therefore, further checks of the interpolation trajectory points and swept areas are necessary.

[0032] Motion admission result refers to the final decision made by the system after comprehensively evaluating various safety constraints regarding whether to allow the dual-arm robot physical entity to execute the current target motion command. This result can be permission to execute, denial of execution, or restriction of execution.

[0033] The term "restricted execution" means that the target action is not completely unexecutable, but only a portion of it is allowed to be executed in the current state. For example, only the chassis is allowed to adjust its position first, only the lifting mechanism 4 is allowed to return to a safe height first, or only one arm is allowed to exit the interference zone before continuing the subsequent actions.

[0034] The swept area refers to the entire continuous spatial volume swept by the geometry of a component of a dual-arm robot as it moves from one spatial pose to the next. This concept can be understood as the union of all regions swept through space when the component moves continuously from its starting position to its ending position. For example, when a gripper holds a long, rectangular load and turns it from left to front, what truly needs to be checked is not only the space occupied by the load in the initial and ending poses, but also the fan-shaped spatial region swept by the load during rotation. Detecting this region allows for the earlier identification of potential collision risks during rotation.

[0035] In step S1 above, establishing a unified collision model is the foundation for subsequent spatial interference judgment. In a probabilistic example, the geometric bounding box models of each link of the robotic arm body can be pre-constructed using 3D modeling software, and these bounding box models can be assembled according to the kinematic topology of the robotic arm to form a basic model for collision detection.

[0036] Specifically, in step S1, the unified collision model includes at least the spatial envelope representation of the mobile chassis 5, the lifting mechanism 4, the left robotic arm 2, the right robotic arm 3, the end effector 6, the dual-arm robot body mounting structure 7, the environmental obstacles 8, and the clamped load 9.

[0037] In practical applications, in order to balance computational efficiency and detection accuracy, capsule bodies or simplified convex hulls are usually used to approximate the complex appearance of robotic arm links.

[0038] Furthermore, in order to enable the unified collision model to be directly used for subsequent dynamic detection, the establishment process may include the following: First, based on the structural design data, assembly dimension data, or physical measurement data of the dual-arm robot, corresponding geometric shape models are established for the mobile chassis 5, lifting mechanism 4, left robotic arm 2, right robotic arm 3, end effector 6, and dual-arm robot body mounting structure 7; for regular shaped parts, basic geometric shapes such as cuboids, cylinders, and capsules can be combined to represent them; for parts with irregular shapes but clear contour boundaries, simplified convex hulls can be used for envelope representation.

[0039] Secondly, establish installation and motion relationships consistent with the actual mechanism for each model component. For example, the chassis model serves as the root node of the whole machine, the lifting mechanism 4 model changes vertically relative to the chassis model, the dual robotic arm model is installed on the lifting mechanism model and rises and falls with it as a whole, and the end effector 6 model is installed at the end of its respective robotic arm and changes synchronously with the end flange position.

[0040] Next, establish static or dynamic spatial models for environmental obstacles 8. Static obstacles can be derived from a pre-built work area map, such as shelves, fences, columns, workbenches, etc.; dynamic obstacles can be derived from real-time environmental perception results, such as temporarily placed pallets, turnover boxes, or other equipment entering the work area.

[0041] For the clamped load 9, after the clamping action is confirmed to be successful, a corresponding spatial envelope model can be generated based on the load shape information provided by the task system, the visual recognition results, or the size information manually entered, and a rigid following relationship can be established between the model and the end effector 6 that clamps the load.

[0042] After this processing, the unified collision model can not only represent the dual-arm robot itself, but also the overall space occupancy state of the dual-arm robot combined with external working objects.

[0043] Furthermore, before the model is put into use, a model verification process can be performed, in which the dual-arm robot is placed in several typical poses, and the relative positions of the parts in the simulation model are compared with the relative positions of the actual objects. If a significant offset is found in a certain part model, its mounting reference or envelope dimensions are corrected. The unified collision model established in the above way not only preserves the spatial correlation at the whole machine level, but also takes into account the computational efficiency required for subsequent real-time collision detection, thus providing a reliable foundation for multi-level collision detection in step S4.

[0044] In step S2, obtaining the current operating state aims to provide accurate initial conditions for the simulation environment. Specifically, as shown in the following example, by reading the absolute encoder data configured at each joint of the robotic arm, the current joint angle information of the robotic arm is obtained, thereby determining the spatial orientation of the robotic arm at the current moment.

[0045] Furthermore, in step S2, the acquisition of the current operating state is not limited to the joint angle of the robotic arm. The current operating state includes at least the mobile chassis state of the dual-arm robot 1, the height of the lifting mechanism, the joint state of the left robotic arm, the joint state of the right robotic arm, the state of the end effector 6, and the safety limit state of the lifting mechanism 4.

[0046] In practical implementation, the dual-arm robot controller can periodically collect and summarize the status information of multiple subsystems: for the mobile chassis 5, the position and orientation information of the chassis in the current map coordinate system can be obtained through wheel speed encoders, inertial measurement units, and navigation and positioning modules; for the lifting mechanism 4, the current height position can be obtained through linear displacement sensors, servo drive feedback values, or lifting shaft encoders; for the left robotic arm 2 and the right robotic arm 3, the joint position, joint speed, and drive enable status fed back by each joint driver can be read respectively; for the end effector 6, the gripper opening and closing status, adsorption status, gripping confirmation signal, or tool switching status can be read; for safety-related statuses, emergency stop status, servo alarm status, limit trigger status, and communication connection status can also be acquired synchronously.

[0047] To ensure consistency between the initial state in the simulation environment and the physical entity state, preferably, the time alignment of the aforementioned states can be completed within the same sampling period. This involves organizing state data from different subsystems according to a unified timestamp before writing them into the simulation environment. If a state data point is missing, times out, or is clearly abnormal in the current period—for example, if a joint feedback value does not update for an extended period—the system can mark the state as invalid and directly provide a motion admission result of rejection or restriction in subsequent steps, rather than continuing planning with an incomplete state.

[0048] By using this whole-machine-level, time-consistent state acquisition method, it is possible to ensure that the starting point of the candidate motion trajectory is consistent with the actual state of the dual-arm robot, thus avoiding distortion of subsequent collision detection results due to initial state deviation.

[0049] In step S3, after receiving the target motion command, the system does not directly drive the physical dual-arm robot to move. Instead, it first performs a pre-simulation in a virtual environment. The system uses the current operating state as the starting point and the target pose parsed from the target motion command as the ending point, and calls a path planning algorithm to generate a candidate motion trajectory. This candidate trajectory consists of a series of trajectory key points arranged in chronological order, with each key point recording the desired pose data of the dual-arm robot at a specific moment.

[0050] Furthermore, step S3 includes: S31: Receive the target motion command issued by the upper-level scheduling system or operator, and parse the target motion command into at least one of the following target pose or position information of the left robotic arm target pose, right robotic arm target pose, dual-arm synchronous target pose, lifting mechanism target height or mobile chassis target position of the dual-arm robot 1. S32: Using the current running state as the initial state and the target pose or position information as the target constraint, the path planning algorithm is called in the simulation environment to perform motion planning and generate candidate motion trajectories from the current running state to the point where the target constraint is met. Each key point in the candidate motion trajectory includes at least the target pose or position information of the corresponding component.

[0051] This specific implementation plan details the process for generating candidate motion trajectories. First, the target motion command received by the system may only contain the target object's identifier or rough location. The parsing module needs to combine the environmental map and the kinematic model of the dual-arm robot to transform this into specific numerical constraints. For example, the target pose of the left robotic arm can be represented as the six-degree-of-freedom pose matrix of the end flange in the base coordinate system; the synchronous target pose of both arms includes not only the individual poses of the two arms but also the relative positional constraints between them; the target height of the lifting mechanism and the target position of the moving chassis correspond to one-dimensional and two-dimensional coordinate values, respectively.

[0052] To more clearly and completely describe this analysis process, the environmental map can be obtained in advance through LiDAR mapping, 3D camera scanning modeling, or manual calibration. In an implementable example, during the initial deployment in the workshop, a mobile chassis 5 carrying a LiDAR first cruises at low speed along the work area to generate a basic map containing shelves, workbenches, aisle boundaries, and restricted areas. Subsequently, manual annotation of semantic points such as the gripping position of shelf A, the docking position in front of the workbench, and the dual-arm collaborative pick-and-place area is performed in the software interface, and these semantic points are mapped to map coordinates to form a directly queryable task location library. The kinematic model of the dual-arm robot can be established using the equipment's factory parameters, joint zero-position calibration results, link dimension parameters, and tool coordinate calibration results, and stored in the controller configuration file.

[0053] Thus, when the target motion command includes grabbing a box on shelf A, the parsing module can first read the coordinates of the storage location corresponding to shelf A from the task location library, and then combine the placement direction of the box in the storage compartment, the installation posture of the gripping fixture, the gripping clearance, and the requirements for the advance and retreat path to further generate the approach posture, gripping posture, and exit posture of the left robotic arm 2 or the right robotic arm 3. When the command includes synchronous handling by both arms, the parsing module needs to read the length of the object being handled, the position of the gripping point, and the allowable posture range, and then generate the gripping posture of the ends of the two arms relative to the two ends of the object, as well as the relative distance and relative orientation constraints that the two arms need to maintain.

[0054] Secondly, using the current operating state as the initial state ensures that the starting point of the planning is completely consistent with the actual state of the physical entity, avoiding ineffective planning that is detached from reality. The current operating state can be obtained by: reading the output of the positioning module of the mobile chassis 5 to obtain the chassis's current position and orientation in the map coordinate system; reading the encoder or displacement sensor data of the lifting mechanism to obtain the current height position; reading the joint angles, angular velocities, and enable states fed back by the joint actuators of the left robotic arm 2 and the right robotic arm 3; and, if necessary, reading the opening / closing state, clamping state, and current tool number of the end effector 6. To ensure state reliability, the system can perform a time alignment and freshness check before entering step S32. That is, only when the state data of the chassis, lifting mechanism 4, and both arms are all collected within the allowed time delay will they be written into the simulation environment as a unified current operating state.

[0055] Finally, the generated candidate motion trajectories are discretized into trajectory key points arranged in chronological order. This data structure facilitates subsequent point-to-point collision detection and also makes it easier to send the data to the servo driver for execution. In addition to containing the target pose or position information of the corresponding component, each trajectory key point can further record the action stage identifier, expected arrival time, corresponding execution component identifier, and necessary velocity level label, thereby facilitating subsequent segmented detection, segmented execution, and anomaly backtracking.

[0056] In practice, assume that the upper-level scheduling system issues a target motion command to "coordinate the movement of long strip materials to the designated workbench with two arms". The system first parses the command into: the target position of the moving chassis (the safe stopping point in front of the workbench), the target height of the lifting mechanism (the height level with the workbench table), and the target pose of the two arms (the specific six-degree-of-freedom pose of the left and right arms holding the two ends of the material respectively, and the distance between the ends of the two arms remains constant).

[0057] Furthermore, the system can first query the length range, cross-sectional shape, allowable clamping area, and approximate center of gravity of the elongated material from the manufacturing execution system or material database; then, based on the calibration position of the workbench on the environmental map, determine the safe stopping point reserved for the mobile chassis 5 in front of the workbench; then, combining the relative height relationship between the lifting mechanism 4 and the workbench surface, obtain the target height of the lifting mechanism; finally, based on the size of the end grippers of the dual arms, the clamping opening range, and the safe clamping distance at both ends of the material, generate the approach pose, clamping pose, and collaborative transport pose of the left and right arms respectively. If the system supports vision assistance, it can also acquire images of the actual placement of the material before execution using an industrial camera installed on the dual-arm robot body or workstation, and determine the precise orientation and end position of the material at the current moment through image recognition, correcting the previously parsed dual-arm synchronous target pose to reduce gripping failures caused by material placement deviations.

[0058] Subsequently, in the simulation environment, the system uses the current actual position, height, and joint angles of the dual-arm robot as the initial state, and the previously parsed target pose as the target constraint, and calls the path planning algorithm. This simulation environment is not an abstract blank space, but a digital planning space that loads the current unified collision model, environment map, task area constraints, and motion boundaries of each execution component in real time. Before planning, the system checks whether the target pose falls within the reachable range of each component, for example, whether the ends of the dual arms can reach both ends of the material at the current chassis position and lifting height; if not, it prioritizes adjusting the target position of the moving chassis or the target height of the lifting mechanism before replanning. After planning, a candidate motion trajectory containing 100 trajectory key points is output. Key points 1 to 30 mainly describe the chassis movement process, key points 31 to 50 describe the lifting process of the lifting mechanism 4, and key points 51 to 100 describe the action sequence of the dual arms extending towards the material and completing the gripping action. Each key point accurately records the desired state of the chassis, lifting mechanism 4, and dual arms at the corresponding moment.

[0059] To facilitate execution, these key points can be grouped and stored according to the "chassis adjustment stage, height adjustment stage, dual-arm coordinated approach stage, and dual-arm clamping stage." The chassis key points mainly record the planar position and orientation, the lifting key points mainly record the target height, and the dual-arm key points mainly record the pose of the left and right robotic arm joints or end effectors and the relative constraint markers between the two arms. If a local constraint conflict is found during the planning process of a certain stage, such as a conflict between the dual-arm approach path and the edge space of the worktable, the system can replan only that stage without overturning the previously feasible chassis and lifting plans.

[0060] In the process of generating candidate motion trajectories, the dual-arm robot 1 has extremely high degrees of freedom (e.g., the two arms plus the lifting mechanism 4 and the chassis may exceed 15 degrees of freedom), resulting in an extremely large configuration space filled with complex obstacle constraints. Using a single or simple planning algorithm often makes it difficult to find a smooth path that avoids obstacles while satisfying the collaborative constraints of the two arms within a finite time, easily leading to local optima or planning timeouts. To solve this path search problem in a high-dimensional space, it is necessary to introduce a diverse and efficient path planning algorithm library to adapt to different types of motion requirements.

[0061] Furthermore, in step S32, the path planning algorithm is at least one of the following: random sampling algorithm, tree search algorithm, interpolation planning algorithm, or inverse kinematics solution algorithm.

[0062] This specific implementation plan clarifies the types of path planning algorithms that can be invoked in the simulation environment. Random sampling algorithms (such as RRT, RRT*, etc.) are particularly suitable for solving complex obstacle avoidance problems in high-dimensional spaces by randomly scattering points in the configuration space and constructing a search tree; tree search algorithms (such as A, Dijkstra, etc.) are typically used for global optimal path search in low-dimensional spaces (such as two-dimensional planar movement of a chassis); interpolation planning algorithms (such as cubic spline interpolation, fifth-order polynomial interpolation, etc.) are used to generate smooth, continuous trajectories between known key points, ensuring the continuity of joint velocities and accelerations; and inverse kinematics algorithms are used to infer the angles of each joint from the desired end-effector pose, which is the foundation for achieving precise grasping and placement.

[0063] The system can flexibly select a single algorithm or a combination of multiple algorithms for planning based on the parsed target constraint type. To more clearly and completely describe how these algorithms are implemented, the system can pre-establish a mapping table between task types and planning algorithms, and use it as the scheduling rule for the planning algorithm library. For example: when the input task only involves moving the mobile chassis 5 from the starting point A to the ending point B, and the environmental map is known and the passage boundaries are clear, the task classifier marks the task as a two-dimensional global movement and prioritizes the tree search algorithm from the planning algorithm library; when the input task involves single or double arms performing obstacle avoidance approach inside the shelf, around tooling fixtures, or at equipment openings, the task classifier marks it as a high-dimensional obstacle avoidance approach and prioritizes the random sampling algorithm; when the input task emphasizes continuous translation of the end effector along a given trajectory, maintaining stable attitude, or smooth speed changes, the task classifier marks it as a fine continuous operation and prioritizes the combination of inverse kinematics solving algorithm and interpolation planning algorithm.

[0064] This correspondence table can be formed through experience accumulated during equipment debugging, or it can be manually set by engineers in the configuration interface according to process requirements. To avoid the use of arbitrary selection rules, the system can also record the success rate, average planning time, and trajectory smoothness of each type of task based on historical planning results during actual operation, and maintain the correspondence table in a versioned manner. For example, if a certain type of narrow space grasping task consistently yields good results using a random sampling algorithm, it can be set as the default algorithm; if it is found that some fine assembly tasks have local velocity abrupt changes in the joint trajectories obtained by using only inverse kinematics, an interpolation planning algorithm can be forcibly added in subsequent configurations for smoothing.

[0065] In practice, when the target action is to move the chassis from one end of the warehouse to the other, the system prioritizes using a tree search algorithm to plan a globally optimal, collision-free path on a two-dimensional grid map. During this process, the two-dimensional grid map is discretized from the environment map: passable areas, obstacle areas, no-stopping areas, and narrow passage areas are marked as different types of grid cells. Then, based on the chassis dimensions and safe outward expansion distance, the grid cells near obstacle boundaries are expanded to ensure that the searched path still has sufficient clearance during actual execution.

[0066] After obtaining the path using the tree search algorithm, the system can further convert the polyline path into a smooth steering path suitable for chassis execution, and discretize it into a continuous path point sequence according to the chassis control cycle.

[0067] When the target action is for the left robotic arm 2 to bypass the shelf pillars to grab items inside, due to obstacle avoidance in high-dimensional joint space, the system calls a random sampling algorithm to quickly search for a joint motion path that avoids the pillars. At this time, the system can first load the spatial envelopes corresponding to the shelf pillars, the shelf frame, adjacent stored objects, and each link of the robotic arm into the simulation environment. Then, taking the current joint state of the left robotic arm 2 as the starting point and the feasible inverse kinematics solution set corresponding to the grabbing pose as the target region, the search tree is gradually expanded in the joint space through the random sampling algorithm.

[0068] Once the search tree expands to the target area, an initial feasible path is obtained. This path usually has many turns or local redundant actions. Therefore, the system can further prune and smooth the path, removing unnecessary intermediate detours, so that the left robotic arm 2 can complete the approach action as simply as possible while ensuring obstacle avoidance.

[0069] When the target action is for the two arms to work together to move the material along a straight line into the test equipment, the system first calls the inverse kinematics algorithm to calculate the joint angles of each discrete point on the translation trajectory, and then calls the interpolation planning algorithm to smooth these joint angles to ensure that the two arms move smoothly and without sudden changes in speed during the translation process.

[0070] More specifically, the system first generates an end-point reference trajectory based on the test equipment's inlet position, material posture requirements, and the relationship between the gripping points of the two arms. Then, it selects several discrete points on this reference trajectory and calculates the joint targets for the left and right arms at each point. If some points have no solution or the joints are close to their limits, the system can reduce the single displacement step size, adjust the auxiliary position of the chassis or lifting mechanism, or reselect the gripping posture of the two arms before performing inverse kinematics calculation. After obtaining the joint targets for all discrete points, the interpolation programming algorithm connects these discrete points into a continuous and smooth collaborative motion trajectory of the two arms.

[0071] In step S4, multi-level collision detection is the core step in ensuring trajectory safety. First, for each discrete trajectory key point in the candidate motion trajectory, the system updates the spatial positions of each component in the unified collision model and checks for any interference.

[0072] Secondly, due to the time intervals between key points, the system performs mathematical interpolation between adjacent trajectory key points to generate denser interpolated trajectory points, and performs the same collision detection on these interpolated trajectory points. Finally, based on the kinematic interpolation between adjacent trajectory key points, the system calculates the sweep region formed by the component's movement in space and detects whether this sweep region intersects with other models.

[0073] This multi-level detection, from point to line and from line to volume, can thoroughly eliminate potential spatial interference risks.

[0074] To further clarify the specific implementation of this step, it can be understood as three progressive levels. The first level is collision detection at key points of the trajectory. The system updates the poses of the moving chassis 5, lifting mechanism 4, left and right robotic arms, end effector 6, and clamped load 9 in a unified collision model point by point according to the time sequence of each key point in the candidate motion trajectory. At each key point, the system checks the following types of collision relationships: collision between left robotic arm 2 and right robotic arm 3, collision between robotic arm and chassis or lifting mechanism 4, collision between end effector 6 and dual-arm robot body mounting structure 7, collision between dual-arm robot and environmental obstacles 8, and collision between clamped load 9 and environmental obstacles 8. If spatial overlap, penetration, or insufficient safety clearance is found at any key point, the key point can be directly recorded as a risk point.

[0075] For details regarding the collision between the left robotic arm 2 and the right robotic arm 3, please refer to the attached document. Figure 5 This diagram exemplifies the spatial relationship between the left robotic arm 2 and the right robotic arm 3 as they move along their planned trajectories during synchronized dual-arm movement. The system generates the left and right arm trajectories in a simulation environment and simultaneously compares and calculates the joint positions, link positions, and end effector positions of the left and right robotic arms 2 and 3 at the same time step. When the trajectories of the two arms are too close in space—that is, when the minimum distance between them is less than a preset safe distance threshold for both arms—the system determines that there is a risk of collision between the two arms.

[0076] The second layer is collision detection for interpolated trajectory points. Since there may be significant attitude changes between adjacent key points, checking only the key points themselves might miss collisions in the intermediate process. Therefore, the system needs to supplement adjacent key points with interpolated trajectory points. The interpolation method can be determined according to the type of actuator: for chassis pose changes, intermediate poses can be supplemented along the planned path in chronological order; for lifting mechanism 4, intermediate height states can be supplemented between the current height and the target height; for robotic arm joint movements, intermediate joint states can be supplemented between adjacent joint states. After generating interpolated trajectory points, the system repeats the same collision detection process as for key points for each interpolated point. The purpose of this approach is to transform the originally sparse trajectory checking into a more continuous process checking.

[0077] like Figure 4 As shown, this application employs a progressive detection strategy, from discrete points to continuous trajectories, to perform layer-by-layer safety checks on candidate motion trajectories. The figure exemplifies a sampling path (represented by a broken line) from start point A to end point B, where P1, P2, and P3 are sampling points generated by path planning (i.e., trajectory key points). The system first performs collision detection on each of these sampling points to check for collision risks. However, detecting only the sampling points themselves is insufficient; the interpolated trajectories (represented by smooth curves) between adjacent sampling points may cross areas containing obstacles. Therefore, the system further performs collision detection on the interpolated trajectories between adjacent sampling points to identify whether the interpolated trajectories intersect with environmental obstacles 8 or other components. Based on this, the system also constructs a trajectory sweep area formed by the movement between adjacent trajectory points and performs collision detection on this area to cover the entire spatial volume swept by the robotic arm or load during continuous movement. Through this multi-level detection method from sampling points to interpolated trajectories to the sweep area (represented by dashed lines for collision detection), blind spots in collision detection during trajectory execution are effectively eliminated.

[0078] The third layer is collision detection in the swept area. Even if the interpolation points are relatively dense, there may still be cases where some high-speed rotating parts sweep across the edges of narrow obstacles between adjacent samples. Therefore, it is necessary to further examine the swept area formed by the parts during continuous movement.

[0079] For details regarding collision detection of the clamped load 9, please refer to the appendix. Figure 6This diagram exemplifies how, when the end effector 6 of a dual-arm robot moves while gripping a box (i.e., the gripped load 9), the system incorporates the spatial envelope of the gripped box as an independent collider into a unified collision model for detection. As the gripped box moves along the direction of motion, the system not only detects whether the robotic arm collides with environmental obstacles 8 or the structure of the dual-arm robot itself, but also includes the envelope of the gripped box, which moves along with the end effector 6, in the collision detection range to detect whether it will interfere with obstacles or the structure of the dual-arm robot during movement. By incorporating the spatial envelope of the gripped load 9 into the unified collision model and continuously updating its position and orientation during movement, collisions between the load itself and the surrounding environment can be effectively avoided, further improving the safety of the handling task.

[0080] In specific implementation, step S4 further includes: S41: Update the position and orientation of the spatial envelope of each component in the unified collision model at each trajectory key point, and calculate the first distance between the spatial envelopes of each component. S42: When the first distance is less than the preset safe distance threshold, it is determined that there is a collision risk at the key point of the trajectory; S43: Interpolate between two adjacent trajectory key points to generate an interpolated trajectory point sequence. Update the position and orientation of the spatial envelope of each component in the unified collision model at each interpolated trajectory point, and calculate the second distance between the spatial envelopes of each component. S44: When the second distance is less than the safe distance threshold, it is determined that the interpolated trajectory point has a collision risk; S45: Based on the motion between two adjacent trajectory key points, construct the sweep region spatial envelope formed when each component moves between adjacent trajectory key points, and calculate the third distance between the sweep region spatial envelope and the spatial envelope of other components in the unified collision model. S46: When the third distance is less than the safe distance threshold, it is determined that there is a collision risk in this segment of the trajectory; S47: Generate collision detection results by combining the detection results from each level.

[0081] This specific implementation plan details the quantitative calculation process for multi-level collision detection. The first distance, second distance, and third distance correspond to the minimum spatial distances at the three levels: trajectory key points, interpolated trajectory points, and swept regions, respectively. By calculating the shortest Euclidean distance between the spatial envelopes of components (such as capsules or convex hulls), the system can determine not only whether physical contact has occurred (distance less than or equal to zero) but also whether a dangerous approach zone has been entered (distance greater than zero but less than the safe distance threshold).

[0082] The safe distance threshold can be set according to the control precision, running speed and environmental uncertainties of the dual-arm robot. For example, a larger threshold can be set when moving at high speed and a smaller threshold can be set when operating at low speed.

[0083] In a feasible example, the preset safety distance threshold can be configured through a tiered calibration method during the equipment commissioning phase. This can be divided into three steps: First, for typical action scenarios such as chassis movement, lifting mechanism 4 movement, single-arm extension, and dual-arm coordination, the stopping response distance, control following error, and end effector jitter range of the dual-arm robot are collected under low-speed, medium-speed, and high-speed operating conditions. Second, based on the maximum deviation of different components at different speed levels, corresponding safety margins are reserved, forming a parameter table of "component type—speed level—safety distance threshold." Third, in the actual deployment site, considering the rigidity of obstacles, the narrowness of the working space, and the dynamics of the environment, this parameter table is revised and then fixed into the system configuration file. For example, a more conservative safety distance threshold can be used when near areas with personnel activity; a smaller safety distance threshold that still meets the control error coverage requirements can be used when performing low-speed precision insertion inside enclosed tooling.

[0084] Therefore, the preset safe distance threshold can be configured executablely based on the equipment's responsiveness and the characteristics of the operating environment. Regarding information acquisition methods, the current position and attitude information of each component in the unified collision model can be obtained through multiple channels: the position of the mobile chassis can come from the positioning module, LiDAR matching results, or visual positioning results; the height of the lifting mechanism can come from the encoder or linear displacement sensor; the state of each joint of the dual arms can come from joint servo feedback; the positions of static obstacles such as shelves and workbenches in the environment can come from mapping results and manual calibration data; and the real-time positions of temporary cartons, pallets, or surrounding equipment can be obtained through secondary detection by visual sensors or LiDAR. The system uniformly writes this information into the simulation environment and the unified collision model, ensuring that each distance calculation corresponds to a verifiable real or near-real-time state.

[0085] In a specific embodiment, assume that the dual-arm robot is performing the action of extending the right robotic arm 3 into the shelf. In step S41, the system extracts the 10th key point in the trajectory, updates the envelope pose of each link of the right arm, and calculates that the first distance between the envelope of the right arm end and the envelope of the shelf beam is 50mm.

[0086] Since the set safety distance threshold is 30 mm, the system determines that the key point is safe. Then, proceeding to step S43, the system generates 5 interpolated trajectory points between the 10th and 11th key points. When updating the 3rd interpolated trajectory point, the calculated second distance between the right arm elbow envelope and the shelf upright is 25 mm, less than the 30 mm threshold, so the system immediately determines that this interpolated trajectory point has a collision risk. Even if all interpolated points are safe, the system will proceed to step S45 to construct the continuous sweeping area envelope formed when the right arm moves from the 10th to the 11th key point. Calculations show that the third distance between this sweeping area envelope and the envelope of the temporarily placed cardboard box next to it is only 10 mm, also less than the threshold, thus determining that this segment of the trajectory has a collision risk.

[0087] Finally, in step S47, the system summarizes all the above calculation results and outputs collision detection results containing the specific risk location and risk type. For ease of implementation, the processing method for each level is further explained below.

[0088] For step S41, the system can read the chassis attitude, lifting height, and left and right robotic arm joint status point by point according to the trajectory key points, and update the position and orientation of each spatial envelope through positive kinematic relationship and geometric transformation rules, and then perform distance calculation on all envelope pairs that need to be checked.

[0089] Among them, the envelope pairs that need to be checked can be predefined by the configuration table. For example, key objects such as robotic arm to shelf, robotic arm to workbench, between two arms, robotic arm to chassis, and lifting mechanism to four pairs of peripheral equipment should be checked to reduce meaningless calculations.

[0090] For step S43, the number of interpolation trajectory points generated can be adaptively set according to the displacement change, attitude change, or action risk level between adjacent key points: if the change between two key points is small, the number of interpolation points can be small; if the change is large or the segment is located in a narrow area, the number of interpolation points can be increased.

[0091] In a feasible example, the system automatically increases the interpolation density when entering the shelf interior, approaching the test equipment opening, and engaging in cross-arm coordination to cover intermediate poses that might be missed by key points. For step S45, the spatial envelope of the sweep area can be formed by connecting and expanding the spatial envelopes of corresponding components at adjacent key points, or by sampling multiple intermediate poses during continuous movement and taking their combined outer envelope area. This sweep area is particularly suitable for detecting risky situations where "the starting point is safe, the ending point is safe, but the intermediate swing amplitude is too large."

[0092] Finally, in step S47, the system can organize the risk results into a structured record, including at least: the trajectory segment number to which the risk belongs, the risk level, the component that caused the risk, the associated obstacle, the minimum distance value, the index of the corresponding key point or interpolation point, and the suggested handling method. The suggested handling method may include replanning, re-inspecting after reducing speed, or retrying after adjusting the parking position, so that the collision detection results can be directly used by the subsequent control module.

[0093] In the aforementioned application scenarios, although geometric interference detection based on a unified collision model can identify most spatial risks, the actual operation of the physical dual-arm robot is still strictly constrained by the underlying hardware status and control logic. For example, the chassis may be performing an emergency obstacle avoidance maneuver, the lifting mechanism 4 may have reached its physical limit switch, or the two arms may have experienced timing errors during collaborative tasks. If motion commands are blindly issued based solely on geometric detection results, it can easily lead to hardware damage, servo alarms, or collaborative task failures. To address this issue, further, in step S5, the obstacle avoidance status of the mobile chassis 5 includes at least the real-time distance between the mobile chassis 5 and the obstacle, whether the mobile chassis 5 is performing an obstacle avoidance maneuver, and whether the mobile chassis 5 is in a stopped state. The safety limit status of the lifting mechanism 4 includes at least the current height position of the lifting mechanism 4 and whether the lifting mechanism 4 is in the upper limit position, lower limit position, or near the limit area; the near limit area means that the distance between the current height position of the lifting mechanism 4 and the upper limit position or lower limit position is less than the near threshold. The synchronous motion state of the two arms includes at least whether the left robotic arm 2 and the right robotic arm 3 are in motion at the same time, as well as the joint position, link position and end position of the left robotic arm 2 and the right robotic arm 3 at the same time step.

[0094] For the mobile chassis 5, in addition to focusing on its real-time distance from obstacles, it is more important to obtain its underlying control intent (such as whether it is autonomously avoiding obstacles or has stopped in an emergency), which determines whether the chassis can respond to new upper-level commands.

[0095] For the lifting mechanism 4, an approach limit zone is introduced, which provides an early warning when there is still a safe distance (approaching the threshold) from the limit switch, in order to prevent mechanical impact caused by inertia.

[0096] For synchronized movement of both arms, it is not only necessary to determine whether the two arms move simultaneously in time, but also to accurately align the joints, links and end-effectors at the same time step in space. This is the basic data for assessing whether internal interference or coordination breakdown occurs between the two arms.

[0097] Specifically, the obstacle avoidance status of the mobile chassis 5 can be obtained jointly by the chassis controller, the lidar processing node, and the safety control unit: among which, the lidar or depth camera is used to sense the distance of obstacles in front, the chassis controller outputs whether the obstacle avoidance action is currently being executed, and the safety control unit outputs whether the deceleration, stop, or recovery conditions are triggered.

[0098] The safety limit status of the lifting mechanism 4 can be obtained through the encoder, limit switch, and driver status word. The encoder provides the current height position; the limit switch provides hard trigger signals for the upper and lower limits; and the driver status word reflects whether the mechanism has entered a prohibited movement state due to overload, following error, or limit protection.

[0099] The synchronized motion state of the two arms can be obtained through the dual-arm collaborative controller, feedback from each joint actuator, and unified time synchronization module: the unified time synchronization module ensures that the state samples of the left and right robotic arms 3 come from the same time step, the joint actuator feedback provides the position of each joint, the collaborative controller calculates the link position and end position based on the forward kinematics, and checks whether the current movement stage of the two arms is consistent.

[0100] The preset proximity threshold can be obtained during the equipment installation and commissioning phase through limit protection testing of the lifting mechanism 4. In one feasible example, the lifting mechanism 4 is first driven at low speed to approach the upper and lower limits. The actual buffer distance required from issuing a stop command to the mechanism coming to a complete stop is recorded at different speed levels. Then, combined with the elastic deformation of the mechanical structure and the control margin, the proximity threshold is determined and written into the parameter table of the lifting mechanism 4. In this way, when the current height position enters the proximity threshold range, the system can take a limiting action in advance, even if the physical limit switch has not yet been triggered.

[0101] In practical implementation, assume that the dual-arm robot is performing a complex assembly task. The system obtains information through the chassis's LiDAR and the underlying controller: the real-time distance between the mobile chassis 5 and the person in front is 1.2m, and the chassis's underlying deceleration logic has been triggered, indicating that it is in the process of obstacle avoidance.

[0102] Meanwhile, by reading the encoder data of the lifting mechanism 4, it was found that the current height position is 1.95m, while the physical upper limit is 2.0m and the set approach threshold is 0.1m. Therefore, it was determined that the lifting mechanism 4 is in the approach limit area.

[0103] Furthermore, the system obtains the synchronous motion status of the two arms through a dual-arm collaborative controller: both the left robotic arm 2 and the right robotic arm 3 are in motion, and at the current time step, the end-effector coordinates of the left arm are specific spatial coordinates, while the end-effector coordinates of the right arm are another specific spatial coordinate. The link position data of both arms are also synchronously transmitted back. The system summarizes these low-level state data to provide a basis for subsequent comprehensive access judgment.

[0104] To enable direct implementation of this process, the data aggregation method can be further explained. The system can establish a status aggregation module to periodically poll or subscribe to data messages from each subsystem controller: chassis-side messages should include at least the current position, current speed level, distance to the nearest obstacle, obstacle avoidance status flag, and stop status flag; lifting mechanism 4 messages should include at least the current height position, upper safety limit status, lower safety limit status, and proximity to the limit area status; dual-arm-side messages should include at least the current motion status of the left and right robotic arms, the positions of each joint, the end effector position, and the synchronization timestamp. Upon receiving these messages, the status aggregation module first checks whether the timestamps are consistent or fall within the allowed synchronization window; if the time steps on the left and right sides of the dual arms are inconsistent, the abnormal synchronization motion status of the dual arms is directly written into the overall status result.

[0105] Subsequently, the system can make a preliminary judgment according to the predefined access rules. For example, as long as the mobile chassis 5 is in the process of obstacle avoidance, it is prohibited to issue a new chassis movement segment; as long as the lifting mechanism 4 is in the upper limit position, lower limit position or close to the limit position area and the new command still requires to continue to move in that direction, it is prohibited to execute the lifting command; as long as the action stage indicators of the two arms are inconsistent at the same time step, or one side is in motion while the other side is stationary, it is prohibited to execute the synchronous segment action of the two arms.

[0106] Through this convergence and prediction method, hardware constraints beyond geometric detection can be explicitly transformed into executable admission logic.

[0107] Furthermore, step S6 includes: S61: When there is a collision risk in the candidate motion trajectory, or the mobile chassis 5 is in the process of performing obstacle avoidance or in a stopped state, or the lifting mechanism 4 is in the upper limit position, lower limit position or close to the limit position area, or the left robotic arm 2 and the right robotic arm 3 are in motion at the same time, the motion access restriction is triggered. S62: When the condition for triggering motion access restrictions is that there is a collision risk in the candidate motion trajectory, replanning is performed. If there is still a collision risk after replanning, a motion access result that prohibits execution is generated. S63: When the condition for triggering motion access restriction is that the mobile chassis 5 is in the process of performing obstacle avoidance or in a stopped state, generate motion access result for restricted execution; S64: When the condition for triggering motion access restriction is that the lifting mechanism 4 is in the upper limit position, lower limit position, or close to the upper limit position or the limit area, a motion access result that prohibits execution is generated; S65: When the condition for triggering motion access restriction is that the left robotic arm 2 and the right robotic arm 3 are in motion at the same time, calculate the minimum distance between the joint positions, link positions and end positions of the left robotic arm 2 and the right robotic arm 3 at the same time step. If any minimum distance is less than the preset safe distance threshold between the two arms, then replanning is performed. If any minimum distance after replanning is still less than the preset safe distance threshold between the two arms, then the motion access result of restricted execution is generated.

[0108] In step S61, the system first summarizes all potential risk sources. If any one of the following is met—geometric collision risk, chassis obstacle avoidance interference, lifting limit triggering, or dual-arm synchronous interference—the motion access restriction is triggered. Subsequently, the system enters a differentiated processing branch based on the specific reason for triggering the restriction.

[0109] In step S62, regarding the risk of geometric collision, the system does not immediately give up. Instead, it calls the path planning algorithm in the simulation environment to attempt replanning (e.g., adjusting the robotic arm's orbital posture or changing the chassis's docking position). Only when multiple replanning attempts fail to find a collision-free path will a result prohibiting execution be generated. If the candidate trajectory no longer poses a collision risk after replanning, the system marks the restriction as lifted and continues to evaluate the remaining admission conditions. Only after all restrictions are lifted will a motion admission result allowing execution be generated.

[0110] In step S63, when the chassis cannot move due to the underlying obstacle avoidance logic, the system generates a restricted execution result. Restricted execution here means that the system can strip the chassis movement portion of the instruction, allowing the robotic arm to perform partial operations only at the current safe position, or instruct the chassis to wait in place for the obstacle to be removed before continuing execution, thus avoiding a complete interruption of the task.

[0111] In step S64, in the case where the lifting mechanism 4 touches the physical boundary, since continued movement is very likely to cause damage to the mechanical structure, the system takes the strictest protection measures, directly generates a result prohibiting execution, and can simultaneously report the limit abnormality to the upper-level scheduling system.

[0112] In step S65, for dual-arm collaborative operation, the system further quantifies the interference risk between the two arms. By calculating the minimum distance between key parts (joints, links, and ends) of the two arms at the same time step and comparing it with a preset dual-arm safe distance threshold, the system can accurately determine whether the two arms are too close. If the distance is insufficient, the system will first attempt to replan to increase the distance between the two arms; if the replanning fails, it will generate a result that restricts execution, such as downgrading the original synchronous action of the two arms to a time-separated action where the left arm moves first and the right arm moves later, trading space for safety. If the minimum distance between the joint positions, link positions, and end positions of the left and right robotic arms at the same time step is greater than or equal to the preset dual-arm safe distance threshold, the restriction caused by this factor will be lifted.

[0113] If all conditions that could trigger restrictions have been removed, the system should naturally generate the final result that allows motion access to be performed.

[0114] Through the above technical solutions, the system can intelligently choose to replan, restrict local actions, or completely prohibit execution for different types of risks. Under the premise of ensuring the absolute safety of the whole machine, it can save tasks that may fail due to local constraint conflicts to the greatest extent, and significantly improve the task execution continuity and overall operation efficiency of the dual-arm robot 1 in complex and dynamic working environments.

[0115] After generating the permissible motion access results and issuing commands, the physical dual-arm robot may be affected by mechanical errors, control lag, ground slippage, or sudden changes in the external environment (such as a person suddenly entering) during actual execution, causing the actual running trajectory to deviate from the planned trajectory in the simulation environment. To address this dynamic execution deviation, this application further proposes step S7, which includes: S71: During the execution of the candidate motion trajectory, continuously acquire the actual operating status of the dual-arm robot; S72: Compare the actual running state with the target pose or position information at the corresponding time point in the candidate motion trajectory, and calculate the deviation between the actual running state and the target pose or position information; S73: When the deviation exceeds the preset threshold, pause the execution of the current motion command; S74: Reacquire the current operating state of the dual-arm robot and reconstruct the unified collision model; S75: Based on the reconstructed unified collision model, multi-level collision detection is performed again on the remaining trajectory parts that were not executed in the candidate motion trajectory, and the obstacle avoidance status of the mobile chassis 5, the safety limit status of the lifting mechanism 4, and the synchronous motion status of the two arms are re-acquired to generate new motion access results. S76: Based on the new motion admission result, perform the corresponding processing operation, which includes at least continuing execution, replanning, speed-limited execution, returning to a safe posture, and stopping execution and alarming.

[0116] This specific implementation plan details the rolling re-verification mechanism during execution. In steps S71 and S72, the system establishes a high-frequency real-time monitoring closed loop. The system periodically reads the chassis odometer, lifting encoder, and robotic arm servo feedback to obtain the actual operating status and calculates the difference between this status and the expected status corresponding to the current timestamp in the candidate motion trajectory. For example, it calculates the Euclidean distance deviation between the actual chassis coordinates and the planned coordinates, or it calculates the difference between the actual joint angle of the robotic arm and the planned angle.

[0117] In step S73, once any deviation of the actuator is found to exceed a preset threshold (this threshold can be dynamically set according to the control accuracy of the actuator and the confinement of the current environment), the system immediately issues a pause command to the underlying controller, freezing all motion axes. After the pause, the system proceeds to steps S74 and S75, starting from the latest actual operating state, to update the poses of each component in the unified collision model, and to perform multi-level collision detection and underlying state verification again on the remaining unexecuted portion of the original trajectory. This process is equivalent to performing a snapshot-style safety review midway through execution.

[0118] In step S76, the system determines the subsequent action based on the review results: if the deviation is only caused by control lag and the remaining trajectory is still safe, it chooses to continue execution or execute at a limited speed to reduce the subsequent following error; if the deviation causes the remaining trajectory to have a collision risk, it chooses to replan and generate a new continuation trajectory; if the environment deteriorates and replanning is not possible, it chooses to retreat to a safe posture to allow the dual-arm robot to withdraw from the danger zone, or directly stop execution and alarm to hand over manual intervention.

[0119] Combining the technical challenges of the aforementioned application scenarios with the overall operational logic of the technical solution, the collision prediction and motion access control method of this dual-arm robot 1 achieves significant technical results. Firstly, by generating candidate motion trajectories in a simulation environment and performing multi-level collision detection, it effectively compensates for the safety hazards caused by traditional methods that only detect discrete target points while ignoring intermediate motion processes, ensuring absolute safety of the trajectory in continuous space. More specifically, trajectory key point detection addresses the safety of the target posture, interpolated trajectory point detection addresses the safety of the transition process between key points, and swept region detection addresses whether the spatial volume formed by continuous motion intrudes into other objects. These three aspects are progressively enhanced, thus covering the complete risk chain from static posture to dynamic processes.

[0120] Secondly, by deeply integrating the chassis obstacle avoidance status, lifting safety limit status, and dual-arm synchronous movement status with the geometric collision detection results, the isolated effect of independent control of each subsystem is broken, achieving unified action access judgment for the entire machine. This is because geometric collision detection only indicates whether spatial interference is possible, while chassis obstacle avoidance, lifting limit, and dual-arm synchronous status further indicate whether the control conditions for execution are met. When combined, the system's output access result is no longer limited to local mechanisms but can truly reflect whether the entire machine is suitable to execute the target action at the current moment.

[0121] Finally, a continuous deviation comparison and rolling re-verification mechanism is introduced during the execution process, which enables the dual-arm robot to dynamically adapt to mechanical errors or sudden environmental changes during the execution process, avoiding physical collisions caused by blindly executing the predetermined trajectory, and greatly improving the safety and reliability of the mobile dual-arm operation dual-arm robot in complex working environments.

[0122] It can be seen that the system performs unified modeling and multi-level prediction before the action is issued, and continuously performs state back-reading and deviation verification during the action execution, which enables the solution to have both pre-execution prevention and in-execution correction capabilities. Therefore, compared with solutions that rely solely on single planning or single detection, it can more stably adapt to dynamic and complex scenarios.

[0123] Please refer to Figure 2 A collision prediction and motion access control system for a dual-arm robot, used to implement the steps of any of the above methods, the system comprising: Modeling Module 201: Establish a unified collision model; First acquisition module 202: Acquires the current operating status of the dual-arm robot 1; Simulation Module 203: Receives the target motion command and generates candidate motion trajectories in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. Collision detection module 204: Based on a unified collision model, performs multi-level collision detection on candidate motion trajectories to obtain collision detection results; multi-level collision detection includes at least collision detection of trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of swept regions formed by the motion between adjacent trajectory key points. The second acquisition module 205: acquires the obstacle avoidance status of the mobile chassis 5 of the dual-arm robot 1, the safety limit status of the lifting mechanism 4, and the synchronous movement status of the two arms. Generation module 206: Generates motion access results based on collision detection results, obstacle avoidance status, safety limit status, and synchronized movement status of both arms; Execution module 207: When the motion admission result is allowed to execute, it issues motion commands to the corresponding execution mechanism. During the execution process, it continuously compares the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory. When the deviation exceeds the preset threshold, it pauses the execution and reconstructs the unified collision model and performs a rolling re-verification of the motion admission.

[0124] Through the above technical solution, the system, through modular design, decouples the complex collision prediction and motion access control logic into multiple functionally independent execution units. The modeling module 201 and the first acquisition module 202 provide the system with a precise digital twin foundation; the simulation operation module 203 and the collision detection module 204 constitute the core prediction and evaluation engine, ensuring absolute safety at the spatial geometry level; the second acquisition module 205 and the generation module 206 deeply integrate the underlying hardware constraints and geometric safety, outputting access decisions that conform to physical reality; finally, the execution module 207 completes the instruction issuance and dynamic correction.

[0125] This highly integrated system architecture not only ensures the efficiency and consistency of data flow between each step, but also significantly improves the maintainability and scalability of the system, enabling the control system to be easily deployed in various mobile dual-arm operation dual-arm robot platforms, providing them with comprehensive safety operation guarantees.

[0126] The above description is merely an embodiment of this application and is not intended to limit the scope of protection of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A collision prediction and motion access control method for a dual-arm robot, characterized in that, The method includes the following steps: S1: Establish a unified collision model; S2: Obtain the current operating status of the dual-arm robot; S3: Receive the target motion command and generate a candidate motion trajectory in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. S4: Based on the unified collision model, perform multi-level collision detection on the candidate motion trajectory to obtain collision detection results; the multi-level collision detection includes at least collision detection of the trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of the swept region formed by the motion between adjacent trajectory key points. S5: Obtain the obstacle avoidance status of the mobile chassis of the dual-arm robot, the safety limit status of the lifting mechanism, and the synchronous movement status of the two arms; S6: Generate motion access results based on the collision detection results, the obstacle avoidance state, the safety limit state, and the synchronous movement state of the two arms; S7: When the motion admission result is allowed to execute, a motion command is issued to the corresponding execution mechanism. During the execution process, the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory is continuously compared. When the deviation exceeds a preset threshold, the execution is paused and the reconstruction of the unified collision model and the rolling re-verification of the motion admission are performed again.

2. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, In step S1, the unified collision model includes at least a mobile chassis, a lifting mechanism, a left robotic arm, a right robotic arm, an end effector, a dual-arm robot body mounting structure, environmental obstacles, and a spatial envelope representation of the clamped load.

3. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, The current operating state includes at least the mobile chassis state of the dual-arm robot, the height of the lifting mechanism, the joint state of the left robotic arm, the joint state of the right robotic arm, the end effector state, and the safety limit state of the lifting mechanism.

4. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, Step S3 includes: S31: Receive the target motion command issued by the upper-level scheduling system or operator, and parse the target motion command into at least one of the following target pose or position information of the dual-arm robot: left robotic arm target pose, right robotic arm target pose, dual-arm synchronous target pose, lifting mechanism target height, or mobile chassis target position. S32: Using the current running state as the initial state and the target pose or position information as the target constraint, call the path planning algorithm in the simulation environment to perform motion planning and generate candidate motion trajectories from the current running state to the target constraint. Each of the key points in the candidate motion trajectory includes at least the target pose or position information of the corresponding component.

5. The collision prediction and motion access control method for a dual-arm robot according to claim 4, characterized in that, In step S32, the path planning algorithm is at least one of the following: random sampling algorithm, tree search algorithm, interpolation planning algorithm, or inverse kinematics solution algorithm.

6. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, Step S4 includes: S41: Update the position and orientation of the spatial envelope of each component in the unified collision model at each of the trajectory key points, and calculate the first distance between the spatial envelopes of each component. S42: When the first distance is less than the preset safe distance threshold, it is determined that there is a collision risk at the key point of the trajectory; S43: Interpolate between two adjacent trajectory key points to generate an interpolated trajectory point sequence, update the position and orientation of the spatial envelope of each component in the unified collision model at each interpolated trajectory point, and calculate the second distance between the spatial envelopes of each component. S44: When the second distance is less than the safe distance threshold, it is determined that the interpolated trajectory point has a collision risk; S45: Based on the motion between two adjacent trajectory key points, construct the sweeping region spatial envelope formed when each component moves between adjacent trajectory key points, and calculate the third distance between the sweeping region spatial envelope and the spatial envelope of other components in the unified collision model. S46: When the third distance is less than the safe distance threshold, it is determined that there is a collision risk in the trajectory segment; S47: Generate the collision detection result by combining the detection results of each level.

7. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, In step S5, the obstacle avoidance status of the mobile chassis includes at least the real-time distance between the mobile chassis and the obstacle, whether the mobile chassis is performing an obstacle avoidance action, and whether the mobile chassis is in a stopped state. The safety limit status of the lifting mechanism includes at least the current height position of the lifting mechanism and whether the lifting mechanism is in the upper limit position, lower limit position, or near the limit region; the near limit region means that the distance between the current height position of the lifting mechanism and the upper limit position or lower limit position is less than the near threshold. The synchronous motion state of the two arms includes at least whether the left and right robotic arms are in motion at the same time, and the joint positions, link positions, and end positions of the left and right robotic arms at the same time step.

8. The collision prediction and motion access control method for a dual-arm robot according to claim 7, characterized in that, Step S6 includes: S61: When the candidate motion trajectory has a collision risk, or the mobile chassis is in the process of performing obstacle avoidance or in a stopped state, or the lifting mechanism is in the upper limit position, lower limit position or close to the limit position area, or the left robotic arm and the right robotic arm are in motion at the same time, the motion access restriction is triggered. S62: When the condition for triggering motion access restriction is that the candidate motion trajectory has a collision risk, replanning is performed. If a collision risk still exists after replanning, a motion access result that prohibits execution is generated. S63: When the condition for triggering motion access restriction is that the mobile chassis is in the process of performing obstacle avoidance or in a stopped state, generate motion access result for restricted execution; S64: When the condition for triggering motion access restriction is that the lifting mechanism is in the upper limit position, lower limit position or close to the limit position area, generate a motion access result that prohibits execution; S65: When the condition for triggering motion access restriction is that the left robotic arm and the right robotic arm are both in motion, calculate the minimum distance between the joint positions, link positions and end positions of the left robotic arm and the right robotic arm at the same time step. If any of the minimum distances is less than the preset double-arm safe distance threshold, then replanning is performed. If any of the minimum distances after replanning is still less than the preset double-arm safe distance threshold, then a motion access result with restricted execution is generated.

9. The collision prediction and motion access control method for a dual-arm robot according to claim 1, characterized in that, Step S7 includes: S71: During the execution of the candidate motion trajectory, the actual operating state of the dual-arm robot is continuously acquired; S72: Compare the actual running state with the target pose or position information at the corresponding time point in the candidate motion trajectory, and calculate the deviation between the actual running state and the target pose or position information; S73: When the deviation exceeds a preset threshold, pause the execution of the current motion command; S74: Reacquire the current operating state of the dual-arm robot and reconstruct the unified collision model; S75: Based on the reconstructed unified collision model, perform multi-level collision detection again on the remaining trajectory parts of the candidate motion trajectory that have not been executed, and re-acquire the obstacle avoidance status of the mobile chassis, the safety limit status of the lifting mechanism, and the synchronous motion status of the two arms to generate new motion admission results. S76: Based on the new motion admission result, perform the corresponding processing operation, which includes at least continuing execution, replanning, speed-limited execution, reverting to a safe posture, and stopping execution and issuing an alarm.

10. A collision prediction and motion access control system for a dual-arm robot, characterized in that, The system, used to implement the method according to any one of claims 1-9, comprises: Modeling module: Establish a unified collision model; First acquisition module: Acquires the current operating status of the dual-arm robot; Simulation operation module: Receives target motion command and generates candidate motion trajectory in the simulation environment based on the current running state and the target motion command. The candidate motion trajectory contains several trajectory key points arranged in chronological order. Collision detection module: Based on the unified collision model, multi-level collision detection is performed on the candidate motion trajectory to obtain collision detection results; the multi-level collision detection includes at least collision detection of the trajectory key points, collision detection of interpolated trajectory points between adjacent trajectory key points, and collision detection of the swept area formed by the motion between adjacent trajectory key points. The second acquisition module acquires the obstacle avoidance status of the mobile chassis of the dual-arm robot, the safety limit status of the lifting mechanism, and the synchronous movement status of the two arms. Generation module: Generates motion access results based on the collision detection results, the obstacle avoidance state, the safety limit state, and the synchronous movement state of the two arms; Execution module: When the motion admission result is allowed to execute, it issues motion commands to the corresponding execution mechanism. During the execution process, it continuously compares the deviation between the actual running state of the dual-arm robot and the candidate motion trajectory. When the deviation exceeds a preset threshold, it pauses execution and reconstructs the unified collision model and performs a rolling re-verification of the motion admission.

Images