Real-time collision avoidance and coordination control method for dual-arm robot and related equipment

Abstract

This invention discloses a real-time collision avoidance and coordinated control method and related equipment for a dual-arm robot, belonging to the field of robot control technology. The invention acquires joint state data of a first and second robotic arm, as well as sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm; calculates the shortest distance between the first and second robotic arms based on the joint state data, and determines collision avoidance bias data based on the shortest distance data and historical velocity data; determines deformed path point data based on the collision avoidance bias data and the sliding window path point data; and controls a servo driver to execute collision avoidance actions based on the deformed path point data, thereby improving the collision avoidance capabilities of the dual-arm robot in various scenarios.

Description

Technical Field

[0001] This invention relates to the field of robot control, and in particular to a real-time collision avoidance and coordinated control method and related equipment for a dual-arm robot. Background Technology

[0002] In the development of industrial robots, deep learning has gradually become the main method for controlling robots. However, due to its high computing power requirements, it is limited by hardware computing power and bus bandwidth in lightweight, interference-resistant scenarios, making it impossible for the system to run high-dimensional online optimization algorithms. Currently, in such scenarios, an instantaneous reflective obstacle avoidance mechanism is commonly used. This mechanism determines whether collision avoidance is necessary by repeatedly performing multiple steps of serial calculations at a hard real-time frequency of one thousand times per second. It then maps the virtual repulsive force in Cartesian space into joint collision avoidance torque using the Jacobian transpose matrix, thereby controlling the robotic arm to avoid collisions. This traditional reflective obstacle avoidance method encounters problems in some collision avoidance processes. For example, when the two arms move towards each other, the repulsive force and the main task thrust are unbalanced, leading to a standstill and making it unable to adapt to various collision avoidance scenarios.

[0003] The above content is only used to help understand the technical solution of the present invention and does not represent an admission that the above content is prior art. Summary of the Invention

[0004] The main objective of this invention is to provide a real-time collision avoidance and coordinated control method and related equipment for a dual-arm robot, aiming to improve the collision avoidance performance of the dual-arm robot in various scenarios. To achieve the above objective, this invention provides a real-time collision avoidance and coordinated control method for a dual-arm robot, which includes the following steps: Acquire the joint state data of the first robotic arm and the second robotic arm, as well as the sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm; The shortest distance between the first robotic arm and the second robotic arm is calculated based on the joint state data, and collision avoidance offset data is determined based on the shortest distance data and historical speed data. Determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; The servo driver is controlled to perform collision avoidance actions based on the deformed path point data.

[0005] Optionally, the step of calculating the shortest distance between the first robotic arm and the second robotic arm based on the joint state data includes: The axial segment data of each link of the first robotic arm and the axial segment data of each link of the second robotic arm are determined based on the joint state data. The axial segment data of each link of the first robotic arm and the axial segment data of each link of the second robotic arm are used to construct the capsule body enclosure box respectively; The shortest distance between the capsule enclosure of the first robotic arm and the capsule enclosure of the second robotic arm is calculated using analytical geometry and is used as the shortest distance data.

[0006] Optionally, the step of determining the collision avoidance bias data based on the shortest distance data and historical speed data includes: The normal repulsion data is determined based on the shortest distance vector in the shortest distance data, and the tangential sideslip direction data is determined based on the historical velocity data. The collision avoidance strength coefficient is determined based on the shortest distance data, and the normal weight coefficient and tangential weight coefficient are determined based on the shortest distance and the preset safety distance threshold. The collision avoidance bias data is determined based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weight coefficient, and the tangential weight coefficient.

[0007] Optionally, the step of determining the collision avoidance bias data based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weighting coefficient, and the tangential weighting coefficient includes: The weighted normal component is calculated based on the normal repulsion force data and the collision avoidance strength coefficient, and the normal weight of the weighted normal component is adjusted according to the normal weight coefficient. Calculate the weighted tangential components based on the tangential sideslip direction data and the tangential weighting coefficients; The collision avoidance bias data is determined by vector superposition of the weighted normal component and the weighted tangential component.

[0008] Optionally, the step of determining the deformed path point data based on the collision avoidance offset data and the sliding window path point data includes: Based on the sliding window index value and total length of each path point in the sliding window path point data, the deformation weight value corresponding to each path point is determined by the parabolic weight allocation function. Based on the deformation weight value and collision avoidance offset data corresponding to each path point, the position offset of each sliding window path point is calculated to determine the deformed path point position corresponding to each path point. The deformed path point data is constructed based on the position of each deformed path point.

[0009] Optionally, before the step of determining the deformation weight value corresponding to each path point using a parabolic weight allocation function based on the sliding window index value and the total length of the sliding window in the sliding window path point data, the method further includes: Calculate the relative approach speed data between the first robotic arm and the second robotic arm based on the historical speed data; Determine whether the relative proximity speed data is greater than a preset speed threshold; When the relative proximity speed data is greater than the preset speed threshold, the first length is taken as the total length of the sliding window; When the relative proximity speed data is less than or equal to the preset speed threshold, the second length is used as the total length of the sliding window, wherein the first length is greater than the second length.

[0010] Optionally, before the step of determining the deformed path point data based on the collision avoidance offset data and the sliding window path point data, the method further includes: Determine whether the shortest distance value in the shortest distance data is less than a preset complex obstacle threshold; When the shortest distance value is less than the preset complex obstacle threshold, the position of the temporary virtual guide point is determined based on the normal repulsion force data and the tangential sideslip direction data. Based on the location of the temporary virtual guide point and the sliding window path point data, cubic spline interpolation is performed to determine the detour path point data; Replace the sliding window path point data with the detour path point data.

[0011] Furthermore, to achieve the above objectives, the present invention also provides a real-time collision avoidance and coordination control system for a dual-arm robot, the real-time collision avoidance and coordination control system for the dual-arm robot comprising: The acquisition module is used to acquire joint state data of the first robotic arm and the second robotic arm, as well as sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points in the current sliding window extracted from the global path of the first robotic arm. The analysis module is used to calculate the shortest distance data between the first robotic arm and the second robotic arm based on the joint state data, and to determine the collision avoidance offset data based on the shortest distance data and historical speed data. The adjustment module is used to determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; The control module is used to control the servo driver to perform collision avoidance actions based on the deformed path point data.

[0012] Furthermore, to achieve the above objectives, the present invention also provides a dual-arm robot, the dual-arm robot comprising: a memory, a processor, and a real-time collision avoidance and coordination control program for the dual-arm robot stored in the memory and executable on the processor, the real-time collision avoidance and coordination control program for the dual-arm robot being configured to implement the steps of the real-time collision avoidance and coordination control method for the dual-arm robot described in any of the above claims.

[0013] In addition, to achieve the above objectives, the present invention also provides a storage medium storing a real-time collision avoidance and coordination control program for a dual-arm robot, wherein when the real-time collision avoidance and coordination control program for the dual-arm robot is executed by a processor, the program implements the steps of the real-time collision avoidance and coordination control method for the dual-arm robot described in any of the above claims.

[0014] This invention proposes a real-time collision avoidance and coordinated control method for a dual-arm robot. This method acquires joint state data of the first and second robotic arms, as well as sliding window path point data of the first robotic arm. The sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm. Based on the joint state data, the shortest distance between the first and second robotic arms is calculated. Collision avoidance offset data is determined based on the shortest distance data and historical speed data. Deformed path point data is determined based on the collision avoidance offset data and the sliding window path point data. Finally, a servo driver is controlled to execute collision avoidance actions based on the deformed path point data, thereby adapting to various collision avoidance scenarios. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the electrical hardware of a dual-arm robot in the hardware operating environment of an embodiment of the present invention. Figure 2 This is a flowchart illustrating the first embodiment of the real-time collision avoidance and coordinated control method for a dual-arm robot according to the present invention. Figure 3 This is a flowchart illustrating the second embodiment of the real-time collision avoidance and coordinated control method for the dual-arm robot of the present invention. Figure 4 This is a flowchart illustrating the third embodiment of the real-time collision avoidance and coordinated control method for the dual-arm robot of the present invention.

[0016] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0017] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0018] Reference Figure 1 , Figure 1 This is a schematic diagram of the electrical hardware of a dual-arm robot in the hardware operating environment of an embodiment of the present invention.

[0019] like Figure 1 As shown, the electrical hardware schematic may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, an interactive device 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to enable communication between these components. The interactive device 1003 may include a display screen or an input unit such as a keyboard. Optionally, the interactive device 1003 may also be connected to the communication bus via standard wired or wireless interfaces. The network interface 1004 may optionally include standard wired or wireless interfaces (such as a Wi-Fi interface). The memory 1005 may be a high-speed random access memory (RAM) or a stable non-volatile memory (NVM), such as a disk drive. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0020] Those skilled in the art will understand that Figure 1 The structures shown do not constitute a limitation on the electrical hardware diagram and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0021] like Figure 1 As shown, the memory 1005, which serves as a storage medium, may include an operating system, a data storage module, a network communication module, a user interface module, and a real-time collision avoidance and coordination control program for the dual-arm robot.

[0022] exist Figure 1 In the electrical hardware schematic diagram shown, the network interface 1004 is mainly used for data communication with other devices; the interactive device 1003 is mainly used for data interaction with the user; the processor 1001 and memory 1005 in the electrical hardware schematic diagram of the present invention can be set in the electrical hardware schematic diagram. The electrical hardware schematic diagram calls the real-time collision avoidance and coordination control program of the dual-arm robot stored in the memory 1005 through the processor 1001, and executes the real-time collision avoidance and coordination control method of the dual-arm robot provided in the embodiment of the present invention.

[0023] This invention provides a real-time collision avoidance and coordinated control method for a dual-arm robot, referring to... Figure 2 , Figure 2This is a flowchart illustrating a first embodiment of a real-time collision avoidance and coordinated control method for a dual-arm robot according to the present invention. In this embodiment, the real-time collision avoidance and coordinated control method for the dual-arm robot includes: Step S1: Obtain the joint state data of the first robotic arm and the second robotic arm, as well as the sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm. In this embodiment, it is applied to a lightweight anti-interference scenario equipped with an industrial control computer and collaborative dual arms. The industrial control computer can also be a low-power industrial PC, an embedded Linux controller, or a regular PLC, etc.

[0024] Specifically, the joint state data includes the encoder positions of each joint in both arms, which can be read via a real-time bus at 1-millisecond intervals. The sliding window pathpoint data here is a sequence of pathpoints within the current sliding window, extracted from the global cooperative trajectory generated at a lower frequency by the top-level path planner. Preferably, this lower frequency refers to 50Hz or lower; however, there is no actual limitation on the generation frequency of the sliding window pathpoint data. Furthermore, the window of this sliding window pathpoint data typically contains 5 to 10 future pathpoints.

[0025] Step S2: Calculate the shortest distance data between the first robotic arm and the second robotic arm based on the joint state data, and determine the collision avoidance offset data based on the shortest distance data and historical speed data; In this embodiment, the calculation process for the shortest distance data is as follows: First, forward kinematics is run based on the joint state data to determine the axis segment data of each link of the two arms in the base coordinate system; then, the axis segments of each link are constructed as capsule-shaped bounding boxes, and the shortest distance and shortest distance vector between the two capsules are calculated using analytical geometry. Historical velocity data comes from a small buffer set in hardware memory, which records the coordinate sequence of the nearest points of the two arms over a past period. The historical velocity is determined based on the coordinate sequence of the nearest points of the two arms. Optionally, the past period can be 20 milliseconds before the current moment. The relative approach velocity between the interference points of the two arms is calculated using these historical coordinate differences, and then the tangential sideslip direction is derived. The collision avoidance bias data is a vector in Cartesian space, composed of a weighted superposition of the normal repulsive force component and the tangential sideslip component: the normal component is along the direction of the shortest distance vector and is used to push the two arms away from each other; the tangential component is perpendicular to the shortest distance vector and combined with the historical motion trend, and is used to guide the two arms to sideslip around the other. This bias design combining normal and tangential components directly addresses the shortcomings of existing technologies where pure normal repulsion leads to deadlock and regression: when the two arms move head-on, the tangential component provides a lateral sliding direction option, breaking the balance deadlock between normal repulsion and main task thrust, and transforming the head-on collision that would have inevitably resulted in deadlock into a reasonable path of lateral brushing.

[0026] Step S3: Determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; In this embodiment, the path deformation uses a parabolic weight allocation function. Each path point within the sliding window is assigned a deformation weight value, determined by the parabolic weight allocation function: the weight value is zero at the beginning of the sliding window (i.e., zero at the current position), reaches its maximum value at the midpoint of the sliding window, and returns to zero at the end of the sliding window (i.e., zero at the farthest future point). The effects of this parabolic distribution are as follows: the current position does not experience jump shifts, avoiding joint jitter caused by sudden path changes; path points near the collision point receive the maximum offset, ensuring the path is at its maximum deviation at obstacles; and the end of the sliding window strongly connects back to the original global trajectory, ensuring the robotic arm can return to the global cooperative trajectory after bypassing obstacles, thus preventing serpentine twisting or continuous deviation. Furthermore, since the deformed position of each path point is equal to its original position plus the product of its corresponding weight value and the collision avoidance bias vector, the entire calculation involves only floating-point additions and multiplications over the length of the sliding window, resulting in a linear computational complexity. Therefore, this method has low computational cost and high real-time performance.

[0027] Step S4: Control the servo driver to perform collision avoidance actions based on the deformed path point data.

[0028] In this embodiment, inverse kinematic mapping or Jacobi pseudo-inverse calculation is performed on the next target point immediately adjacent to the current moment in the deformed path point sequence to obtain the desired angle or velocity command in joint space. It should be noted that the Jacobi matrix establishes a linear mapping relationship between joint space and Cartesian space, determining the contribution of each joint's angular velocity to the linear and angular velocities of the end effector. The Jacobi pseudo-inverse is the generalized inverse of this mapping. When the end effector needs to track the Cartesian space motion determined by the deformed path points, the Jacobi pseudo-inverse calculation can uniquely map the Cartesian space velocity command to the angular velocity or torque command that each joint motor should output, thereby driving the joints to move in coordination so that the end effector reaches the corresponding required pose. This is then sent to the servo driver via torque superposition or direct position tracking. Because the deformed path has incorporated the global path intent (i.e., the end of the sliding window is fixed to the original trajectory) and local collision avoidance requirements (i.e., midpoint arch offset), the servo driver can generate smooth, safe obstacle avoidance motion during execution.

[0029] In this embodiment, by acquiring joint state data of the first and second robotic arms, and sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm, the shortest distance data between the first and second robotic arms is calculated based on the joint state data, and collision avoidance offset data is determined based on the shortest distance data and historical speed data, and deformed path point data is determined based on the collision avoidance offset data and the sliding window path point data, and the servo driver is controlled to perform collision avoidance actions based on the deformed path point data, thereby adapting to various collision avoidance scenarios.

[0030] Furthermore, based on the first embodiment, a second embodiment of the real-time collision avoidance and coordinated control method for the dual-arm robot of the present invention is proposed. In this embodiment, referring to... Figure 3 The step of calculating the shortest distance between the first robotic arm and the second robotic arm based on the joint state data includes: Step S21: Determine the axis segment data of each link of the first robotic arm and the axis segment data of each link of the second robotic arm based on the joint state data. In this embodiment, the system performs forward kinematics based on the position of the joint encoder, calculates the three-dimensional coordinates of the joints at both ends of each link in the base coordinate system, and thus determines the axial segment of each link, that is, the spatial line segment defined by the starting coordinate and the ending coordinate.

[0031] Step S22: Construct capsule enclosures using the axial segment data of each link of the first robotic arm and the axial segment data of each link of the second robotic arm. Specifically, the capsule enclosure is defined by an axial segment and a preset radius parameter. The preset radius parameter is determined by taking the maximum contour radius of the corresponding link entity in the direction perpendicular to the axial segment, plus a preset safety margin. The link entity includes the link body, the joint motor housing, and the end effector. The maximum contour radius is obtained by scanning a section perpendicular to the axial segment of the 3D model of the link entity and taking the maximum value. The preset safety margin is used to cover the robot's repeatability error and motion vibration offset, and is preferably between 5 mm and 10 mm. The lower the robot's repeatability accuracy, the larger the preset safety margin needs to be. Here, the capsule is a combination of a cylinder and two hemispheres. As a geometrically simplified model of the robotic arm link, the capsule can significantly reduce computational complexity while maintaining collision detection accuracy.

[0032] Step S23: Calculate the shortest distance between the capsule enclosure of the first robotic arm and the capsule enclosure of the second robotic arm using analytical geometry, and use this distance as the shortest distance data.

[0033] In this embodiment, the analytical geometry method directly calls the closed algebraic formula for the shortest distance between spatial line segments, without iterative optimization. Since each arm often has more than one link, the inter-arm interference calculation involves forty-nine pairs of link combinations, but each pair of calculations only takes a few microseconds, and the total time consumption is completely acceptable on a low-end industrial control computer. The calculation results include the shortest distance value and the shortest distance vector, which is the direction vector from the first robotic arm to the second robotic arm. The two together constitute the shortest distance data. The shortest distance vector is the line vector connecting the nearest points on the two capsule enclosures, with the direction from the first robotic arm to the second robotic arm; the shortest distance value is the magnitude of the shortest distance vector. The two have different functions in subsequent calculations: the shortest distance value is a scalar, used to compare and judge the values ​​with the preset safety distance threshold and the preset complex obstacle threshold, and is used to determine the collision avoidance strength coefficient, normal weight coefficient, and tangential weight coefficient; the shortest distance vector is a vector, which is normalized and used as normal repulsion data, and participates in the double cross product calculation of the tangential sideslip direction data. Furthermore, it needs to be explained that within each control cycle, the shortest distance is calculated for all combinations of each link of the first robotic arm and each link of the second robotic arm, resulting in multiple sets of candidate shortest distance values ​​and candidate shortest distance vectors. The candidate shortest distance value and candidate shortest distance vector of the group containing the minimum value among all sets of candidate shortest distance values ​​are used as the shortest distance data for this control cycle. Subsequent calculations of collision avoidance offset data and deformed path point data are all performed based on this set of shortest distance data. For example, taking a seven-axis dual-arm system as an example, there are forty-nine pairs of combinations. Since the calculation for each pair is a closed-form algebraic operation that takes several microseconds, iterating through each pair is feasible in real time on a low-end industrial control computer. Therefore, the overall collision risk of the dual arms is dominated by the closest pair of links, and the minimum value group is taken as the input.

[0034] It needs to be further explained that, for each group of links, the closed algebraic formula, while solving for the shortest distance, outputs two points located on the two axis segments that minimize the distance between the two axis segments. These two points are the two nearest points of this group of links, and the shortest distance vector is the line vector connecting these two nearest points.

[0035] The two nearest points corresponding to the group with the smallest candidate shortest distance value in this control cycle are taken as the current nearest points of the two arms, and the shortest distance value of this group is compared with the preset safe distance threshold.

[0036] When the shortest distance value is less than the preset safe distance threshold, it is determined that there is interference between the two arms. The two nearest points are determined as the mutual interference points between the first robotic arm and the second robotic arm. The subsequent calculation of normal repulsion data, tangential side-slip direction data and collision avoidance offset data are all performed based on the mutual interference points. When the shortest distance value is greater than or equal to the preset safe distance threshold, it is determined that the two arms do not interfere and collision avoidance calculation is not triggered. The buffer continuously records the three-dimensional coordinates of the current nearest point of the two arms in each control cycle. When the nearest point is determined to be an interference point, the recorded historical coordinate sequence is the historical coordinate sequence of that interference point, which is used to determine the historical velocity data.

[0037] Furthermore, based on the first or second embodiment, a third embodiment of the real-time collision avoidance and coordinated control method for the dual-arm robot of the present invention is proposed, referring to... Figure 4 In this embodiment, the step of determining the collision avoidance bias data based on the shortest distance data and historical speed data includes: Step S24: Determine the normal repulsion data based on the shortest distance vector in the shortest distance data, and determine the tangential sideslip direction data based on the historical velocity data; In this embodiment, the normal repulsion data is obtained by normalizing the shortest distance vector, denoted as . Its direction is along the line connecting the nearest points of the two capsules, from the first robotic arm to the second robotic arm, used to push the two arms apart along the shortest path. The tangential lateral sliding direction data is denoted as... The determination process is as follows: extract the relative velocity vectors of the interference points of the two arms from historical velocity data. Specifically, a buffer is set in the industrial control computer's memory. This buffer records the three-dimensional coordinate sequence of the nearest points of the two arms in each control cycle within a preset duration prior to the current moment. The preset duration is preferably 20 milliseconds. The nearest points are the two endpoints of the current shortest distance vector on the enclosing box of the two arm capsules. A first-order difference operation is performed on the three-dimensional coordinate sequence over time, that is, the coordinate difference between two adjacent control cycles is divided by the control cycle duration to obtain the instantaneous relative velocity vector corresponding to each control cycle. Then, a moving average is performed on each instantaneous relative velocity vector within the preset duration to obtain the relative velocity vector. The relative velocity vector The projection of the shortest distance vector direction is the relative speed at which the two arms approach each other.

[0038] Using the double cross product method Projected onto the repulsive force perpendicular to the normal direction On the normal plane. The specific calculation formula is as follows:

[0039] In the formula The data in the tangential sideslip direction can be referred to as the tangential sideslip direction vector. This is normal repulsion data, which can be referred to as normal repulsion vector. Let be the relative velocity vectors at the points of interference between the two arms, and `normalize` denotes the normalization operation. The significance of this formula is that it first uses... Cross product Get a value that is simultaneously perpendicular to and The transition vector, and then the transition vector is combined with The cross product always results in a product perpendicular to the x-axis. On the plane, while preserving to the greatest extent possible It contains information about historical motion trends. Through this double cross product operation, it is possible to find a tangential obstacle avoidance path that best follows the original motion trend under the current velocity and normal conditions.

[0040] Step S25: Determine the collision avoidance intensity coefficient based on the shortest distance data, and determine the normal weight coefficient and tangential weight coefficient based on the shortest distance and the preset safety distance threshold. In this embodiment, the collision avoidance strength coefficient is denoted as K_avoid, which varies non-linearly and exponentially with the shortest distance value d. The specific calculation formula is as follows:

[0041] In the formula, K_avoid is the collision avoidance strength coefficient, η is the preset adjustment gain, e is the natural constant, γ is the preset attenuation coefficient, d is the shortest distance value, and d_safe is the preset safe distance threshold. When d is greater than or equal to... When this happens, the system automatically sets K_avoid to zero, resulting in a zero vector for the final calculated collision avoidance bias data. At this point, the system naturally reverts to normal trajectory tracking in the data stream without triggering collision avoidance.

[0042] Preferably, the value of the preset adjustment gain η is based on the maximum collision avoidance bias amplitude allowed by the servo driver: when the shortest distance value approaches zero, the amplitude of the collision avoidance bias data does not exceed a preset safety limit, which is determined according to the maximum allowable output torque of the servo driver and the Jacobian mapping relationship; the value of the preset attenuation coefficient γ is based on the expected establishment rate of the collision avoidance response: the minimum braking distance required to brake the maximum allowable relative approach speed to zero is determined according to the maximum braking deceleration of the robotic arm, and the collision avoidance strength coefficient reaches the strength level required to decelerate both arms when the shortest distance value is equal to this minimum braking distance, thus obtaining γ; as γ is larger, the collision avoidance strength increases faster as the distance decreases, and the collision avoidance intervention is stronger.

[0043] When d is less than At this point, K_avoid increases sharply with increasing distance. The determination of the normal weight coefficient α and the tangential weight coefficient β follows the distance-adaptive principle: at medium distances, for example, 0.5 times... Less than d Less than At this time, the tangential component becomes dominant, preferentially guiding the robotic arm to side-slide and detour in advance to prevent it from freezing; at close range, such as when d is less than or equal to 0.5 times... At this time, the normal component becomes dominant, prioritizing the collision safety force for emergency push-out.

[0044] Step S26: Determine the collision avoidance bias data based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weight coefficient, and the tangential weight coefficient.

[0045] Furthermore, the step of determining the collision avoidance bias data based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weighting coefficient, and the tangential weighting coefficient includes: The weighted normal component is calculated based on the normal repulsion force data and the collision avoidance strength coefficient, and the normal weight of the weighted normal component is adjusted according to the normal weight coefficient. Calculate the weighted tangential components based on the tangential sideslip direction data and the tangential weighting coefficients; The collision avoidance bias data is determined by vector superposition of the weighted normal component and the weighted tangential component.

[0046] In this embodiment, the determination process consists of three steps: First, the normal repulsion force data is... Multiplying the collision avoidance strength coefficient K_avoid yields the weighted normal component, which is then multiplied by the normal weight coefficient α for normal weight adjustment; secondly, the tangential sideslip direction data... Multiplying by the collision avoidance strength coefficient K_avoid and the tangential weight coefficient β yields the weighted tangential component; finally, the weighted normal component and the weighted tangential component are vector-superimposed to obtain the final collision avoidance bias data B_vec. The specific calculation formula is as follows:

[0047] In the formula For collision avoidance bias data, K_avoid is the collision avoidance strength coefficient, and α is the normal weight coefficient. This represents the normal repulsion force data, where β is the tangential weighting coefficient. This is the tangential sideslip direction data. This bias data is a complete Cartesian space vector that includes both the pushing normal force and the sideslip tangential force, and its direction is a comprehensive indication of the optimal obstacle avoidance direction.

[0048] As an alternative implementation method, to address the issues of enormous kinetic energy and high-frequency edge oscillations in high-speed collision scenarios, the collision avoidance bias data can also be directly calculated using an analytical formula that incorporates a relative velocity damping term. The specific calculation formula is as follows:

[0049] In the formula The data represents the collision avoidance bias when using a dynamic damping field mechanism, where η is the preset adjustment gain and d is the shortest distance value. To preset a safe distance threshold, The data represents the normal repulsive force, where λ is the preset dynamic damping coefficient. The relative velocity vector is the point of mutual interference between the two arms. The value of the preset adjustment gain η is determined based on the same criteria as in the third embodiment, according to the maximum allowable collision avoidance bias amplitude of the servo driver; the value of the preset dynamic damping coefficient λ is determined based on the system's desired damping braking intensity: when the amplitude of the relative velocity vector reaches the preset maximum allowable relative velocity, the damping term... The generated bias amplitude is on the same order of magnitude as the bias amplitude generated by the distance term and does not exceed the preset safety limit. The larger λ is, the stronger the damping braking effect.

[0050] By directly introducing a relative velocity compensation term, negative λ is multiplied by... This mechanism can spontaneously generate a strong reverse damping braking effect when the two arms approach each other at high speed, preemptively consuming the kinetic energy and preventing physical interference. When the two arms are close but past trends indicate that they are moving away from each other (i.e., the projection of the relative velocity vector on the shortest distance vector direction is negative and the two arms are moving away from each other), the virtual repulsive force will be dynamically suppressed or even reduced to zero, and the two arms will not blindly avoid obstacles, thus maintaining the main mission trajectory. When past trends indicate that they are colliding at extremely high speeds, the repulsive force will be instantly amplified, forcing the robotic arms to decelerate and slide to the side in advance. This dynamic damping field mechanism and the aforementioned Fatange weighted superposition mechanism can be used independently or configured simultaneously in the system, with the controller automatically switching or superimposing them according to the scenario.

[0051] Furthermore, based on any of the above embodiments, a fourth embodiment of the real-time collision avoidance and coordinated control method for a dual-arm robot of the present invention is proposed. In this embodiment, the step of determining the deformed path point data based on the collision avoidance bias data and the sliding window path point data includes: Step S31: Based on the sliding window index value and total length of the sliding window for each path point in the sliding window path point data, determine the deformation weight value corresponding to each path point through the parabolic weight allocation function; In this embodiment, within the current 1-millisecond control cycle, the path point sequence within the current sliding window includes the current path point and m future path points, where m is the total length of the sliding window. Let the sliding window index value of the current path point be i, where the sliding window index value i takes values ​​of 0, 1, 2, ..., m. The specific mathematical expression of the parabolic weight allocation function is:

[0052] w in the formula i Let be the deformation weight value corresponding to the i-th path point, where i is the sliding window index value of the current path point, and m is the total length of the sliding window. This function forms a discrete parabola that opens downwards and is symmetrical about the point where the sliding window index value i equals half the total length m of the sliding window. When the sliding window index value i equals 0, it corresponds to the currently executing control point. An index of 0 ensures that collision avoidance deformation will not interfere with the current servo closed-loop starting point, preventing sudden jitter; when the sliding window index value i equals half of the total sliding window length m, it corresponds to the middle section of the sliding window. Reaching the maximum value of 1.0 allows the collision hazard core area to achieve the maximum obstacle avoidance arching deformation; when the sliding window index value i equals the total length m of the sliding window, that is, at the end of the sliding window, If the value is 0, the deformed local path will be seamlessly connected back to the original global trajectory planned by the top layer.

[0053] Step S32: Based on the deformation weight value and collision avoidance offset data corresponding to each path point, perform position offset calculation on each sliding window path point to determine the deformed path point position corresponding to each path point; In this embodiment, the position offset is calculated using direct algebraic addition in Cartesian space, and the formula for instantaneous reconstruction of the local path is:

[0054] In the formula Let i be the three-dimensional Cartesian coordinates of the i-th path point after deformation. Let i be the original 3D Cartesian coordinates of the i-th path point within the sliding window. Let be the deformation weight value corresponding to the i-th path point. To avoid collisions, biased data is used. Since the weight values ​​are zero at the start and end points of the sliding window, the current position and the farthest future point do not shift. Only the path points in the middle of the sliding window experience an arched shift, forming a smooth, hump-shaped bypass path. This reconstruction process completely avoids high-dimensional matrix inversion and iterative optimization, and its time complexity is strictly limited to first-order linear O(m). On low-power industrial PCs or conventional PLCs, full-window point updates can be completed instantaneously within nanoseconds to microseconds, thus meeting the window constraints of real-time control buses.

[0055] Step S33: Construct the deformed path point data based on the position of each deformed path point.

[0056] Furthermore, before the step of determining the deformation weight value corresponding to each path point using a parabolic weight allocation function based on the sliding window index value and total length of each path point in the sliding window path point data, the method further includes: Calculate the relative approach speed data between the first robotic arm and the second robotic arm based on the historical speed data; Determine whether the relative proximity speed data is greater than a preset speed threshold; When the relative proximity speed data is greater than the preset speed threshold, the first length is taken as the total length of the sliding window; When the relative proximity speed data is less than or equal to the preset speed threshold, the second length is used as the total length of the sliding window, wherein the first length is greater than the second length.

[0057] In this embodiment, both the first length and the second length are calculated based on the number of future path points contained within the sliding window. The first length is preferably 10 path points, and the second length is preferably 5 path points. Their values ​​are determined by the following: the sliding window time span equals the number of path points divided by the path planning frequency. For example, with a path planning frequency of 50 Hz, the second length corresponds to a 100-millisecond look-ahead time, and the first length corresponds to a 200-millisecond look-ahead time. The sliding window time span should not be less than the braking time required to bring the current relative approach speed to zero using the maximum braking deceleration of the robotic arm. The greater the relative approach speed, the longer the braking time required. Therefore, a larger first length is used in high-speed conditions, and a smaller second length is used in low-speed conditions. The relative approach speed data is calculated by the difference between the coordinate sequences of the nearest points of the two arms within the historical window, i.e., the rate of change of distance over time. A positive speed value indicates that the two arms are approaching each other, and a negative value indicates that they are moving away from each other. This judgment constitutes the core of the zoom sliding window mechanism. When the two arms collide at high speed, a longer sliding window length (the first length) is used to create a larger and gentler arc in the path, preventing physical interference due to inertia preventing braking. When the arms slowly approach each other, a shorter sliding window length (the second length) is used, allowing for minor adjustments to the path and minimizing interference with the main trajectory. This mechanism of dynamically adjusting the sliding window length based on relative speed effectively solves the problems of overreaction or interception failure caused by using fixed parameters for different speed scenarios in existing technologies.

[0058] Furthermore, based on any of the above embodiments, a fifth embodiment of the real-time collision avoidance and coordinated control method for a dual-arm robot of the present invention is proposed. In this embodiment, before the step of determining the deformed path point data based on the collision avoidance bias data and the sliding window path point data, the method further includes: Determine whether the shortest distance value in the shortest distance data is less than a preset complex obstacle threshold; When the shortest distance value is less than the preset complex obstacle threshold, the position of the temporary virtual guide point is determined based on the normal repulsion force data and the tangential sideslip direction data. Based on the location of the temporary virtual guide point and the sliding window path point data, cubic spline interpolation is performed to determine the detour path point data; Replace the sliding window path point data with the detour path point data.

[0059] In this embodiment, the preset complex obstacle threshold is less than the preset safe distance threshold in step S25, used to identify emergency collision scenarios of extreme proximity. When the shortest distance value is greater than or equal to this threshold, it indicates that the collision risk is controllable, and the normal process is executed directly; when the shortest distance value is less than this threshold, it indicates that the two arms have entered a complex obstacle area at extremely close range, and a more refined detour strategy needs to be initiated. The temporary virtual guide point is located at the outer extension position after the normal repulsion direction and the tangential lateral sliding direction are combined, that is, on the normal plane of the line connecting the shortest distances of the two arms, a spatial point generated by offsetting a preset offset distance along the lateral sliding direction; the value of the preset offset distance is based on: not less than the sum of the preset radius parameters of the capsule enclosure boxes corresponding to the two connecting rods currently interfering with each other of the first and second robotic arms, plus a preset safety margin, and preferably 0.5 to 1 times the preset safe distance threshold. This guide point serves as an intermediate transit point of the local detour path, providing the robotic arm with a clear geometric detour landmark. Cubic spline interpolation uses the current end effector position of the robotic arm, the temporary virtual guide point position, and the farthest point in the sliding window path point sequence as control points to generate a smooth curve in joint space or Cartesian space that starts from the current position, passes through the guide point, and then returns to the original trajectory. Cubic spline interpolation ensures the continuity of the path in terms of position and velocity, avoiding joint jitter caused by sudden torque changes.

[0060] Furthermore, this invention also proposes a real-time collision avoidance and coordination control system for a dual-arm robot, the real-time collision avoidance and coordination control system for the dual-arm robot comprising: The acquisition module is used to acquire joint state data of the first robotic arm and the second robotic arm, as well as sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points in the current sliding window extracted from the global path of the first robotic arm. The analysis module is used to calculate the shortest distance data between the first robotic arm and the second robotic arm based on the joint state data, and to determine the collision avoidance offset data based on the shortest distance data and historical speed data. The adjustment module is used to determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; The control module is used to control the servo driver to perform collision avoidance actions based on the deformed path point data.

[0061] Furthermore, this invention also proposes a dual-arm robot, which includes: a memory, a processor, and a real-time collision avoidance and coordination control program for the dual-arm robot stored in the memory and executable on the processor. The real-time collision avoidance and coordination control program for the dual-arm robot is configured to implement the steps of the real-time collision avoidance and coordination control method for the dual-arm robot described in any of the above embodiments.

[0062] Furthermore, this embodiment of the invention also proposes a storage medium storing a real-time collision avoidance and coordination control program for a dual-arm robot. When the real-time collision avoidance and coordination control program for the dual-arm robot is executed by a processor, it implements the steps of the real-time collision avoidance and coordination control method for the dual-arm robot described in any of the above claims.

[0063] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0064] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0065] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0066] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A real-time collision avoidance and coordinated control method for a dual-arm robot, characterized in that, The real-time collision avoidance and coordinated control method for the dual-arm robot includes the following steps: Acquire the joint state data of the first robotic arm and the second robotic arm, as well as the sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points within the current sliding window extracted from the global path of the first robotic arm; The shortest distance between the first robotic arm and the second robotic arm is calculated based on the joint state data, and collision avoidance offset data is determined based on the shortest distance data and historical speed data. Determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; The servo driver is controlled to perform collision avoidance actions based on the deformed path point data.

2. The control method as described in claim 1, characterized in that, The step of calculating the shortest distance between the first robotic arm and the second robotic arm based on the joint state data includes: The axial segment data of each link of the first robotic arm and the axial segment data of each link of the second robotic arm are determined based on the joint state data. The axial segment data of each link of the first robotic arm and the axial segment data of each link of the second robotic arm are used to construct the capsule body enclosure box respectively; The shortest distance between the capsule enclosure of the first robotic arm and the capsule enclosure of the second robotic arm is calculated using analytical geometry and is used as the shortest distance data.

3. The control method as described in claim 1, characterized in that, The step of determining the collision avoidance bias data based on the shortest distance data and historical speed data includes: The normal repulsion data is determined based on the shortest distance vector in the shortest distance data, and the tangential sideslip direction data is determined based on the historical velocity data. The collision avoidance strength coefficient is determined based on the shortest distance data, and the normal weight coefficient and tangential weight coefficient are determined based on the shortest distance and the preset safety distance threshold. The collision avoidance bias data is determined based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weight coefficient, and the tangential weight coefficient.

4. The control method as described in claim 3, characterized in that, The step of determining the collision avoidance bias data based on the normal repulsion force data, the tangential sideslip direction data, the collision avoidance strength coefficient, the normal weighting coefficient, and the tangential weighting coefficient includes: The weighted normal component is calculated based on the normal repulsion force data and the collision avoidance strength coefficient, and the normal weight of the weighted normal component is adjusted according to the normal weight coefficient. Calculate the weighted tangential components based on the tangential sideslip direction data and the tangential weighting coefficients; The collision avoidance bias data is determined by vector superposition of the weighted normal component and the weighted tangential component.

5. The control method as described in claim 1, characterized in that, The step of determining the deformed path point data based on the collision avoidance offset data and the sliding window path point data includes: Based on the sliding window index value and total length of each path point in the sliding window path point data, the deformation weight value corresponding to each path point is determined by the parabolic weight allocation function. Based on the deformation weight value and collision avoidance offset data corresponding to each path point, the position offset of each sliding window path point is calculated to determine the deformed path point position corresponding to each path point. The deformed path point data is constructed based on the position of each deformed path point.

6. The control method as described in claim 5, characterized in that, Before the step of determining the deformation weight value corresponding to each path point using a parabolic weight allocation function based on the sliding window index value and total length of each path point in the sliding window path point data, the method further includes: Calculate the relative approach speed data between the first robotic arm and the second robotic arm based on the historical speed data; Determine whether the relative proximity speed data is greater than a preset speed threshold; When the relative proximity speed data is greater than the preset speed threshold, the first length is taken as the total length of the sliding window; When the relative proximity speed data is less than or equal to the preset speed threshold, the second length is used as the total length of the sliding window, wherein the first length is greater than the second length.

7. The control method according to any one of claims 1 to 6, characterized in that, Before the step of determining the deformed path point data based on the collision avoidance offset data and the sliding window path point data, the method further includes: Determine whether the shortest distance value in the shortest distance data is less than a preset complex obstacle threshold; When the shortest distance value is less than the preset complex obstacle threshold, the position of the temporary virtual guide point is determined based on the normal repulsion force data and the tangential sideslip direction data. Based on the location of the temporary virtual guide point and the sliding window path point data, cubic spline interpolation is performed to determine the detour path point data; Replace the sliding window path point data with the detour path point data.

8. A real-time collision avoidance and coordination control system for a dual-arm robot, characterized in that, The real-time collision avoidance and coordinated control system of the dual-arm robot includes: The acquisition module is used to acquire joint state data of the first robotic arm and the second robotic arm, as well as sliding window path point data of the first robotic arm, wherein the sliding window path point data is a sequence of path points in the current sliding window extracted from the global path of the first robotic arm. The analysis module is used to calculate the shortest distance data between the first robotic arm and the second robotic arm based on the joint state data, and to determine the collision avoidance offset data based on the shortest distance data and historical speed data. The adjustment module is used to determine the deformed path point data based on the collision avoidance offset data and the sliding window path point data; The control module is used to control the servo driver to perform collision avoidance actions based on the deformed path point data.

9. A dual-arm robot, characterized in that, The dual-arm robot includes: a memory, a processor, and a real-time collision avoidance and coordination control program for the dual-arm robot stored in the memory and executable on the processor, wherein the real-time collision avoidance and coordination control program for the dual-arm robot is configured to implement the steps of the real-time collision avoidance and coordination control method for the dual-arm robot as described in any one of claims 1 to 7.

10. A storage medium, characterized in that, The storage medium stores a real-time collision avoidance and coordination control program for a dual-arm robot. When the processor executes the real-time collision avoidance and coordination control program for the dual-arm robot, it implements the steps of the real-time collision avoidance and coordination control method for a dual-arm robot as described in any one of claims 1 to 7.

Images