A robot multi-arm cooperative anti-collision control method and system

Abstract

The application relates to the technical field of robot control, and discloses a robot multi-mechanical arm cooperative anti-collision control method and system, which comprises the following steps: a three-dimensional global reference coordinate system with a kinematic pivot point as the origin is established, an inside and outside work area is divided, and an outside virtual node is defined; a virtual safety monitoring section is constructed in the outside driving area, a dynamic repulsion spot is generated, collision early warning is generated through position and trend compound judgment; the virtual repulsion force is mapped into equivalent joint damping torque, and is superimposed into a servo driver current control loop; the application realizes mathematical reconstruction of the outside non-perception blind area state without an external sensor through kinematic reverse deduction and virtual force field construction, and effectively prevents implicit physical collision of external connecting rods caused by the 'cohesion and external dispersion' motion coupling of the multi-mechanical arms through force feedback of the current control loop level.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a method and system for collaborative collision avoidance control of multiple robotic arms in a robot. Background Technology

[0002] In high-precision fields such as minimally invasive surgical robots and the internal maintenance of complex industrial equipment, multi-robotic arm collaborative operation systems are widely used because they can perform delicate operations through tiny incisions or openings. These systems typically consist of an operating console, a control system, and a driven trolley containing multiple robotic arms. High-precision kinematic algorithms ensure the safe operation of each robotic arm within confined spaces. To prevent interference between robotic arms during collaborative movements, establishing an effective collision avoidance control mechanism is a core technical requirement for ensuring both operational safety and efficiency.

[0003] Among existing technical solutions, Chinese Patent No. CN119575817B discloses a safe cooperative control method for a multi-robot system. This method achieves formation cooperation and safe operation of multiple robotic arms in a specific task space by constructing a distributed fuzzy observer error and a Lyapunov obstacle function. Furthermore, Chinese Patent No. CN120816459B discloses a control system and method for a multi-arm mobile robot. It calculates correction quantities using a joint space point trajectory constraint model and superimposes them onto the original control quantities. The aim is to achieve cooperative obstacle avoidance between multiple arms through the spatial coordinates output by a hard-analytical unit, thereby improving the robot's dynamic response speed and agility.

[0004] However, while existing technologies have achieved some success in obstacle avoidance in general task space or joint space, certain blind spots and hidden dangers still exist when facing special operational scenarios with "single-hole constraints" or "telecentric center of motion (RCM)" constraints. In these scenarios, the robotic arm is strictly constrained to move around a fixed physical entry point, which acts as the fulcrum of a "spatial lever." When the operator focuses on the internal working domain under the endoscopic view and controls the end effectors of multiple robotic arms to converge towards the central lesion or working point to perform coordinated fine operations, driven by the lever effect, the drive linkage mechanism located outside the entry point will inevitably produce divergent or scissor-like motions that are amplified in the opposite direction. Since existing algorithms mostly focus on collision avoidance based on known internal coordinates or end effectors, and the outer drive domain is usually a "non-perceptual blind spot" lacking visual monitoring and external sensor coverage, the control system has difficulty detecting the actual spatial occupancy of the outer linkages in real time. This lack of monitoring dimension leads to a highly hidden risk: that is, while the internal end effectors appear to be safely converging, the dense drive linkages on the outside may already be physically interfering within the blind spot. If the system fails to recognize this implicit collision caused by kinematic coupling and continues to output driving torque, the servo motor will be subjected to extremely high mechanical stall torque in an instant, which will trigger overload protection shutdown, robotic arm lock-up, or even directly cause irreversible physical deformation and damage to the precision guide channel device at the single-point entrance, resulting in equipment damage. Summary of the Invention

[0005] This invention is primarily applicable to multi-arm single-port laparoscopic surgery, such as the da Vinci single-port surgical system, as well as industrial applications such as aircraft engine cavity exploration and chemical storage tank cleaning. To overcome the aforementioned deficiencies of existing technologies, this invention provides a collaborative collision avoidance control method and system for multiple robotic arms. By utilizing rigid body geometric constraints, the known state of the inner working domain is extrapolated to the outer blind zone, constructing a virtual safety monitoring section and a dynamic repulsion spot. When a collision tendency is detected in the outer link, a flexible damping torque is generated through a force feedback mechanism at the current control loop level. This allows the operator to intuitively perceive and actively avoid the hidden collision risk on the outer side without shifting their gaze, solving the "cohesion and dispersion" interference problem in multi-arm collaborative operations within confined spaces.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A method for cooperative collision avoidance control of multiple robotic arms in a robot includes:

[0008] Establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin and divide the inner working domain and the outer driving domain. Define the outer virtual node in the three-dimensional Cartesian global reference coordinate system.

[0009] A virtual safety monitoring section is constructed within the outer drive domain. The outer virtual nodes are connected to the kinematic pivot points to form the physical axis of the outer drive link. The intersection of the physical axis of the outer drive link and the virtual safety monitoring section is calculated as the dynamic projection center. A dynamic repulsion spot is generated with the dynamic projection center as the center. The collision trend composite judgment combining position judgment and trend judgment is performed by traversing the dynamic repulsion spots of all robotic arms and a collision warning flag is generated.

[0010] In response to the collision warning flag, a virtual repulsion force vector is calculated, which is then mapped to an equivalent joint damping torque. This equivalent joint damping torque is then converted into a current command compensation value and superimposed onto the current control loop of the servo driver for each joint of the robotic arm.

[0011] The kinematic pivot point refers to the position coordinates of the multi-manipulator system passing through the single-point constraint entrance;

[0012] The method for dividing the inner working domain and the outer driving domain is as follows: the positive direction of the vertical axis of the three-dimensional Cartesian global reference coordinate system is defined as the insertion direction, the spatial region located downstream of the kinematic pivot point along the insertion direction is defined as the inner working domain, and the spatial region located upstream of the kinematic pivot point along the insertion direction is defined as the outer driving domain.

[0013] The method for defining outer virtual nodes in a three-dimensional Cartesian global reference coordinate system includes:

[0014] Obtain the end-effector coordinates of the robotic arm, construct a unit vector along the axis direction based on the end-effector coordinates and the kinematic pivot point, and extend the unit vector along the axis direction to obtain the outer virtual node.

[0015] The method for obtaining the end position coordinates includes: real-time acquisition of absolute position encoder values ​​of each robotic arm joint module, and calculation of the spatial coordinates of the robotic arm end in the inner working domain using the absolute position encoder values ​​as the end position coordinates;

[0016] The method for constructing the axial direction unit vector is as follows: construct a direction vector from the end position coordinates to the kinematic pivot point based on the end position coordinates and the kinematic pivot point, and then normalize the direction vector to obtain the axial direction unit vector.

[0017] The method for obtaining the outer virtual node by extending a unit vector along the axial direction includes:

[0018] The length of the rigid link in the outer drive domain is obtained as the length of the outer drive link. The distance of the outer drive link length is extended from the kinematic pivot point to the outer drive domain direction by a unit vector along the axis. The position point corresponding to the end point of the extension in the three-dimensional Cartesian global reference coordinate system is defined as the outer virtual node.

[0019] The radius of the dynamic repulsion spot is dynamically adjusted according to the moving speed of the dynamic projection center on the virtual security monitoring section.

[0020] The method for determining the composite collision trend includes:

[0021] Within the same control cycle, the dynamic repulsion light spots corresponding to all robotic arms in the multi-robotic arm system are traversed. The position and trend of any two dynamic repulsion light spots are determined. When the position determination result and the trend determination result simultaneously meet the collision condition, a collision warning flag is generated and the collision warning flag is set to an effective state.

[0022] The method for determining the location is as follows:

[0023] Calculate the Euclidean distance between the centers of any two dynamic repulsion spots and compare it with the sum of the radii of the two dynamic repulsion spots. When the Euclidean distance is less than or equal to the sum of the radii, it is determined that there is geometric interference between the two dynamic repulsion spots.

[0024] The method for determining the trend is as follows: calculate the time derivative of the Euclidean distance between the centers of any two dynamic repulsion spots to obtain the approach rate. When the approach rate is negative, it is determined that the two dynamic repulsion spots are in a state of continuous approach.

[0025] The simultaneous fulfillment of the collision condition by the position determination result and the trend determination result means that both geometric interference and the continuous approach state are simultaneously established.

[0026] The method for calculating the virtual repulsive force vector includes:

[0027] When the collision warning flag is active, the intrusion depth of the two dynamic repulsion spots with geometric interference is calculated, and the virtual repulsion force vector is calculated based on the intrusion depth.

[0028] A robot multi-arm cooperative collision avoidance control system is provided to implement the aforementioned robot multi-arm cooperative collision avoidance control method. The system includes:

[0029] The outer state reconstruction module is used to establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin and to divide the inner working domain and the outer driving domain. It also defines the outer virtual node in the three-dimensional Cartesian global reference coordinate system.

[0030] Collision Trend Determination Module: This module is used to construct a virtual safety monitoring section within the outer drive domain, connect the outer virtual nodes with the kinematic pivot points to form the physical axis of the outer drive link, calculate the intersection of the physical axis of the outer drive link and the virtual safety monitoring section as the dynamic projection center, generate a dynamic repulsion spot with the dynamic projection center as the center, traverse the dynamic repulsion spots of all robotic arms to perform a collision trend composite determination that combines position determination and trend determination, and generate a collision warning flag.

[0031] Force feedback control module: Calculates virtual repulsion force vector in response to collision warning flag, maps virtual repulsion force vector to equivalent joint damping torque, and converts equivalent joint damping torque into current command compensation value and superimposes it into the current control loop of the servo driver of each joint of the robotic arm.

[0032] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0033] This invention establishes a spatial mapping mechanism centered on kinematic pivot points, using rigid body motion geometric constraints to accurately back-calculate the known state of the inner working domain to the outer driving domain. This achieves mathematical reconstruction of the spatial attitude of the driving links in the non-perceptible blind zone without the need for additional external sensors. By constructing a virtual safety monitoring section and dynamic repulsion spot, the "cohesion and divergence" three-dimensional interference risk caused by the movement of multiple robotic arms around a fixed point is reduced to a two-dimensional plane spot composite judgment, accurately capturing the critical collision trend before physical contact occurs. Furthermore, through a force feedback mechanism at the current loop level, the virtual collision risk on the outside is converted into a flexible damping torque that directly acts on the servo joint in real time. This allows the operator to intuitively perceive the interference situation in the outer blind zone through tactile sense without shifting visual attention, achieving active suppression and flexible protection against the hidden collision risk of the external links while maintaining the continuity and smoothness of the operation. Attached Figure Description

[0034] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0035] Figure 1 A flowchart of a robot multi-arm cooperative collision avoidance control method provided in an embodiment of the present invention;

[0036] Figure 2 This is a schematic diagram of the spatial region division of the robotic arm system provided in an embodiment of the present invention;

[0037] Figure 3A flowchart for determining the composite collision trend provided in an embodiment of the present invention;

[0038] Figure 4 A flowchart illustrating force feedback and safety protection provided in an embodiment of the present invention;

[0039] Figure 5 A schematic diagram of a virtual repulsive force vector provided in an embodiment of the present invention;

[0040] Figure 6 This is a functional block diagram of a robot multi-arm collaborative collision avoidance control system provided in an embodiment of the present invention. Detailed Implementation

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

[0042] Example 1:

[0043] Please see Figure 1 As shown, this embodiment provides a robot multi-arm cooperative collision avoidance control method, including:

[0044] Step S10: Establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin and divide the inner working domain and the outer driving domain. Obtain the end position coordinates of the robotic arm. Construct a unit vector along the axis direction based on the end position coordinates and the kinematic pivot point. Extend the unit vector along the axis direction to obtain the outer virtual node.

[0045] Further, step S10 includes:

[0046] Step S11: Obtain the position coordinates of the multi-robotic arm system passing through the single-point constraint entrance and define the position coordinates as the kinematic pivot point. Establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin. Define the positive direction of the vertical axis of the three-dimensional Cartesian global reference coordinate system as the insertion direction. Define the spatial region downstream of the kinematic pivot point along the insertion direction as the inner working domain and the spatial region upstream of the kinematic pivot point along the insertion direction as the outer driving domain.

[0047] A multi-arm robotic system refers to a robotic system comprising two or more robotic arms working collaboratively. These arms are kinematically coupled and need to perform cooperative tasks within the same workspace. For example, in industrial applications, a multi-arm robotic system could be represented as a nuclear power plant reactor vessel internal maintenance robot system. This system includes two or three maintenance robotic arms, each carrying welding tools, a vision probe, and a gripper, which enter the sealed reactor vessel through process inspection ports on the equipment casing to perform collaborative maintenance work. In minimally invasive interventional procedures, a multi-arm robotic system could be represented as the da Vinci surgical robot system. This system includes four robotic arms, each carrying an endoscope, electrocautery forceps, scissors, and grasping forceps, which enter the closed abdominal or pelvic cavity through puncture holes on the patient's skin to perform collaborative and delicate operations. A single-point constraint entrance refers to the spatial constraint channel through which all robotic arms in a multi-arm system converge, forming a physical constraint point. See also... Figure 2 This is a schematic diagram of the spatial region division of the multi-robotic arm system provided in an embodiment of this application. For example... Figure 2 As shown, the main axes of multiple robotic arms spatially intersect at the kinematic pivot point at a single-point constraint entrance, forming a lever-like physical constraint structure. For example, in industrial scenarios, a single-point constraint entrance can be represented as a narrow passageway for an aircraft engine blade inspection robot to enter the engine cavity, or as a manhole at the top of a chemical storage tank for an internal cleaning robot to enter the tank's internal cavity. In minimally invasive interventional procedures, a single-point constraint entrance can be represented as a single-hole puncture channel established through the umbilicus, typically with a diameter between three and four centimeters. Multiple robotic arms enter the patient's abdominal or pelvic cavity through the same puncture port via a kit-type guide channel device.

[0048] The position of the kinematic pivot point is obtained through the structural calibration data of the multi-arm system. After the hardware assembly of the multi-arm system is completed, and the physical connection between the guide channel device and the robot base is relatively fixed, an external, independent precision measuring device, such as a laser tracker or optical positioning system, is used as a calibration tool to measure the geometric center of the guide channel device to obtain its precise three-dimensional coordinates in space. This coordinate data is then written into and permanently stored in the robot's own controller parameter configuration library. In subsequent actual operation phases, the robot system does not need to perform measurements again, but directly calls this preset coordinate value from internal storage, defines it as the kinematic pivot point to establish a coordinate system, and performs collision avoidance control calculations. The kinematic pivot point, serving as the geometric fulcrum for the robotic arm's motion, restricts the translational degree of freedom of the robotic arm at that point, allowing only pitch rotation, yaw rotation, and feed motion along the robotic arm's axis. This constraint characteristic stems from the physical limiting effect of the guide channel device on the robotic arm, causing the robotic arm's kinematic model to exhibit a telecentric motion center characteristic. The kinematic pivot point is the telecentric point defined in the telecentric motion center mechanism, and the two are completely equivalent in physical meaning and geometric position. When establishing a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin, the orthogonal three axes of the coordinate system are determined according to the structural characteristics of the system. For example, in minimally invasive interventional procedures, the direction perpendicular to the surface of the work object and pointing towards the internal work space can be set as the positive direction of the vertical axis of the coordinate system, the direction parallel to the surface of the work object and pointing to the right side of the operator can be set as the positive direction of the horizontal axis of the coordinate system, and the direction perpendicular to the plane containing the vertical and horizontal axes and conforming to the right-hand rule can be set as the positive direction of the vertical axis of the coordinate system. In this coordinate system, the positive direction of the vertical axis (i.e., the direction pointing to the inner working space) is defined as the insertion direction, and the negative direction of the vertical axis is defined as the direction pointing to the outer driving domain. Combined with... Figure 2 As shown, when dividing the coordinate system space into regions based on the insertion direction of the robotic arm, the insertion direction is defined as the direction from the robotic arm base to the end effector. Figure 2 The area downstream of the kinematic pivot point, indicated by a downward-pointing arrow, is the workspace where the end effector actually performs fine operations. This area is defined as... Figure 2 The inner working region marked in the middle; the spatial region upstream of the kinematic pivot point along the insertion direction is the space where the drive link and transmission mechanism are located, and this region is defined as Figure 2 The outer driving domain is marked in the middle. Figure 2 This intuitively demonstrates that this division method uses the kinematic pivot point as the fulcrum of a "spatial lever," so that any angular displacement occurring in the inner working domain can produce a corresponding angular displacement in the outer driving domain with the opposite direction but the same angular value. This provides a physical modeling benchmark for the subsequent reverse deduction of the outer link state based on rigid body geometric constraints.

[0049] Step S12: Real-time acquisition of absolute position encoder values ​​of each robotic arm joint module; calculation of the spatial coordinates of the robotic arm end in the inner working domain using the absolute position encoder values ​​as end position coordinates; construction of a direction vector from the end position coordinates to the kinematic pivot point based on the end position coordinates and the kinematic pivot point; and normalization of the direction vector to obtain the axial direction unit vector.

[0050] A joint module refers to a drive and measurement unit integrated at each joint of a robotic arm. Each joint module includes a servo motor, a reduction mechanism, an absolute position encoder, and a torque sensor. The absolute position encoder outputs the absolute angle value of the joint rotation, allowing direct reading of the current joint's angle position without requiring zeroing calibration after system power-on. When acquiring the absolute position encoder values ​​of each robotic arm joint module in real time, the system polls each joint module at a fixed control cycle via industrial Ethernet or fieldbus. The control cycle is typically set to the millisecond level to meet real-time control requirements. The acquired joint angle values ​​undergo digital low-pass filtering to eliminate high-frequency noise interference in the encoder signal. The filtered angle values ​​serve as input data for subsequent kinematic calculations. Based on the filtered joint angle values, the forward kinematics calculation method is used to calculate the spatial coordinates of the end effector in the inner working domain. The forward kinematics calculation establishes homogeneous transformation relationships between links based on the De Navet-Hartenberg parameter table. The transformation matrices of each link from the base to the end effector are multiplied sequentially, and the translation component in the product matrix is ​​extracted as the spatial coordinate value of the end effector relative to the origin of the coordinate system. This coordinate value is the end effector position coordinate. After determining the end effector position coordinate and the kinematic pivot point, a direction vector is constructed from the end effector position coordinate to the kinematic pivot point. The three components of this direction vector are equal to the difference between the kinematic pivot point's axis coordinate value and the corresponding axis coordinate value of the end effector position coordinate. When normalizing this direction vector, the magnitude of the direction vector is calculated, and each component of the direction vector is divided by the magnitude to obtain a unit vector of the axial direction with a magnitude of one. The unit vector of the axial direction represents the spatial orientation of the robot arm's main axis from the inner working domain to the kinematic pivot point and continuing to extend into the outer driving domain.

[0051] Step S13: Obtain the length of the rigid link in the outer drive domain as the length of the outer drive link. Extend the length of the outer drive link from the kinematic pivot point to the outer drive domain direction by a unit vector along the axis direction. Define the position point of the extension endpoint in the three-dimensional Cartesian global reference coordinate system as the outer virtual node.

[0052] The length of the outer drive link is obtained by calling the pre-stored robot arm structural parameter library in the system. This library stores the length values ​​of the outer drive links from the elbow joint or base drive unit to the kinematic pivot point. These length values ​​are determined through precise measurement and entered into the library during robot arm assembly. In the multi-robot single-hole constraint system applicable to this invention, the robot arm's configuration in the outer drive domain is a rigid straight rod. That is, the link from the kinematic pivot point to the farthest drive joint in the outer drive domain is a single rigid straight rod, and the length of the outer drive link is the geometric length of this rigid straight rod. This rigid straight rod configuration is determined by the physical structure of the single-point constraint entrance: the portion of the robot arm passing through the guide channel device must maintain a straight line shape to meet the spatial constraints of the channel. Therefore, the links in the outer drive domain naturally exhibit a single rigid straight rod configuration, and there is no situation where the axes of multiple links in the outer drive domain are not collinear. For other robotic arm configurations with multiple link segments in the outer drive domain, where the axes of these segments may not be completely collinear, the length of the outer drive link can be defined as the equivalent length from the kinematic pivot point along the main drive axis of the robotic arm to the farthest joint in the outer drive domain. This equivalent length is calculated by multiplying the geometric length of each link segment by the cosine of the angle between the link segment's axial direction and the main drive axis direction, and then summing these products sequentially. The sum is the equivalent length. Based on the continuity principle of rigid body kinematics, the main drive axis of the robotic arm can be considered a rigid straight line passing through the kinematic pivot point. In three-dimensional space, the end-effector coordinates and the kinematic pivot point uniquely determine the direction of this straight line. Therefore, once the end-effector coordinates in the inner work domain are determined, the trajectory of this straight line in the outer drive domain is uniquely locked. When a unit vector along the axial direction extends from the kinematic pivot point towards the outer drive domain, since the direction of the unit vector along the axial direction is from the end position coordinates to the kinematic pivot point, continuing to extend along the same direction from the kinematic pivot point naturally extends beyond the kinematic pivot point and enters the outer drive domain. The distance of extension is equal to the length of the outer drive link. The spatial coordinates of the extension endpoint are calculated using the following relationship: the coordinate components of each axis of the outer virtual node are equal to the coordinate values ​​of the corresponding axes of the kinematic pivot point plus the product of the length of the outer drive link and the corresponding axis components of the unit vector along the axial direction. The position point corresponding to this extension endpoint in the three-dimensional Cartesian global reference coordinate system is defined as the outer virtual node, which represents the spatial position of the end of the outer drive link at the current moment.

[0053] The kinematic pivot point established in step S10 serves as the origin of the coordinate system and is used throughout the calculation processes of steps S20 and S30. This unified coordinate reference ensures that data from each stage—from state reconstruction to collision determination to force feedback—is processed within the same spatial reference frame, avoiding calculation errors introduced by coordinate system transformation. The division of the inner working domain and the outer driving domain in step S11 provides the physical basis for the torque mapping in step S30. Since the outer driving domain and the inner working domain form a rigid lever relationship through the kinematic pivot point, step S30 can map the virtual repulsive force acting on the outer driving domain into a damping torque perceptible to the operator in the inner working domain using the Jacobian transpose matrix. If step S11 does not establish a clear definition of both working domains, the force space mapping in step S30 will lack a geometric correspondence, and the virtual repulsive force cannot be correctly converted into a joint damping torque. Step S10 transforms the implicit state of the outer drive domain into a computable mathematical model through kinematic inverse deduction. This achieves state reconstruction of the non-perceptual blind zone without the need for additional external physical sensors. Utilizing the geometric constraints of rigid body kinematics, this method infers the spatial position of the outer link from joint encoder data obtainable from the inner working domain and known mechanical structural parameters. Compared to traditional solutions relying on external cameras or distance sensors, this reduces system hardware costs and integration complexity, while avoiding the potential impact of external sensors being obstructed, contaminated, or subjected to electromagnetic interference. The design using the kinematic pivot point as the origin of the coordinate system naturally couples the coordinate system with the physical constraints of the multi-arm system. The spatial division between the inner working domain and the outer drive domain, bounded by the origin, directly corresponds to the kinematic coupling relationship on both sides of the lever fulcrum. This design allows for direct collision detection and torque mapping calculations in subsequent steps without additional coordinate transformations, reducing the computational load on the control algorithm.

[0054] Step S20: Construct a virtual safety monitoring section in the outer drive domain, connect the outer virtual nodes with the kinematic pivot points to form the physical axis of the outer drive link, calculate the intersection of the physical axis of the outer drive link and the virtual safety monitoring section as the dynamic projection center, generate a dynamic repulsion spot with the dynamic projection center as the center, traverse all the dynamic repulsion spots of the robotic arm to perform a collision trend composite judgment combining position judgment and trend judgment and generate a collision warning flag.

[0055] Further, step S20 includes:

[0056] Step S21: Select a plane parallel to the mounting plane of the robotic arm base within the outer drive domain as the virtual safety monitoring section, construct a line segment connecting the kinematic pivot point and the outer virtual node as the physical axis of the outer drive link, and calculate the geometric intersection of the physical axis of the outer drive link and the virtual safety monitoring section as the dynamic projection center.

[0057] The virtual safety monitoring section is a mathematically constructed plane within the outer drive domain. This plane is not a physical entity, but rather a geometric reference surface used to project the complex attitude changes of the outer drive links in three-dimensional space onto a two-dimensional plane for analysis. A plane parallel to the mounting plane of the robotic arm base is selected as the virtual safety monitoring section. This design is based on the structural layout characteristics of multi-robotic arm systems: the bases of multiple robotic arms are typically mounted on the same or approximately the same plane. The outer drive links of each robotic arm extend from the base towards the kinematic pivot point, forming an approximately conical spatial distribution within the outer drive domain. When the end effectors of the inner working domain converge towards the central region, the outer drive links inevitably diverge outwards into the surrounding space. This divergent motion is most pronounced on the section parallel to the mounting plane of the base. Therefore, selecting this parallel plane can capture the spatial region where the risk of interference from the outer links is most concentrated. The axial distance between the virtual safety monitoring section and the kinematic pivot point is defined as the monitoring section distance, which needs to be configured according to the length of the outer drive links and the layout of the robotic arm base. The principle for determining the monitoring tangential distance is as follows: based on the motion range of each robotic arm's outer drive link, the tangential distance is selected in the middle region of each link's motion envelope to maximize the capture of the link's lateral displacement. Specifically, the monitoring tangential distance can be determined through the following relationship: Monitoring tangential distance = Length of outer drive link × Coefficient k, where the recommended value range for coefficient k is 0.4 ≤ k ≤ 0.85. A larger k value means the tangential distance is closer to the far end of the outer drive link, resulting in higher sensitivity for detecting lateral displacement at the link's end; a smaller k value means the tangential distance is closer to the kinematic pivot point, resulting in smaller lateral displacement of the link at that point, but a larger response distance reserved for collision avoidance. The specific selection of the k value should comprehensively consider the characteristics of the link's motion envelope and the system's response requirements, achieving a balance between collision detection sensitivity and response margin. For example, when the length of the outer drive link is 30 cm and the bases of each robotic arm are circumferentially distributed, k is taken to be approximately 0.67 to 0.83, and the monitoring section distance can be set to 20 cm to 25 cm. This places the virtual safety monitoring section in the middle section of the outer drive link, which can fully reflect the lateral offset of the link and provide sufficient response distance for subsequent collision avoidance. The monitoring section distance is a system configuration parameter. After the multi-robotic arm system completes hardware assembly, it is set through the parameter configuration interface and stored in the controller parameter configuration library. During the actual control operation phase, the system directly calls this preset value from the parameter configuration library to construct the planar equation of the virtual safety monitoring section.

[0058] The construction of the physical axis of the outer drive link depends on the coordinates of the kinematic pivot point and the coordinates of the outer virtual node. The kinematic pivot point, as the geometric fulcrum when the robot arm passes through the single-point constraint entrance, has its coordinates determined and fixed in the system parameter library in step S11; the coordinates of the outer virtual node, as the spatial position of the end of the outer drive link, have been derived from the rigid body geometric constraints in step S13. Connecting the two coordinate points in three-dimensional Cartesian space forms a line segment, which is the physical axis of the outer drive link. Its geometric significance is that the line segment coincides with the actual mechanical axis of the outer drive link in space, the direction of the line segment represents the spatial attitude of the link at the current moment, and the length of the line segment is equal to the length of the outer drive link. By establishing the physical axis of the outer drive link, the system abstracts the physical entity of the outer drive link into a directed line segment with a clear start and end point, providing a mathematical basis for the subsequent calculation of the geometric intersection of this line segment and the virtual safety monitoring section. The calculation of the dynamic projection center adopts the standard method of finding the intersection of a line and a plane in analytical geometry. The physical axis of the outer drive link can be expressed by a parametric equation. Taking the kinematic pivot point as the origin and the unit vector of the axis direction as the direction, the coordinates of any point on the line are equal to the coordinates of the kinematic pivot point plus the product of the parameter factor and the unit vector of the axis direction. The virtual safety monitoring section can be expressed by a plane equation. Since this section is parallel to the mounting plane of the robotic arm base, its normal vector direction is consistent with the longitudinal axis direction of the system. The constant term in the plane equation is determined by the axial distance between the section and the kinematic pivot point. Specifically, in a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin, assuming the longitudinal axis is the Z-axis, the plane equation of the virtual safety monitoring section can be expressed as Z = -L. monitor L monitor To monitor the sectional distance, the plane is located at the distance offset from the kinematic pivot point along the negative longitudinal direction, perpendicular to the longitudinal axis. Substituting the parameterized equation of the physical axis of the outer drive link into the plane equation of the virtual safety monitoring section, the value of the parameter factor is solved. Substituting this parameter factor value back into the linear parameterized equation yields the coordinates of the geometric intersection point between the physical axis of the outer drive link and the virtual safety monitoring section. This intersection point is the dynamic projection center. The dynamic projection center is called "dynamic" because its coordinates are continuously updated in real-time with the changes in the robot arm's posture: when the operator controls the end effector located in the inner working domain to change position, the end effector's position coordinates change, leading to a change in the unit vector of the axis direction. The coordinates of the outer virtual node are updated accordingly, and the spatial position of the physical axis of the outer drive link changes accordingly, ultimately causing a displacement of the intersection point coordinates between this axis and the virtual safety monitoring section. The displacement trajectory of the dynamic projection center directly reflects the motion trajectory of the outer drive link in the spatial layer where the virtual safety monitoring section is located, providing coordinate basis for subsequent generation of dynamic repulsion spots and collision determination.

[0059] Step S22: Obtain the physical cross-sectional radius of the outer drive link of the robotic arm. Using the dynamic projection center as the center and the physical cross-sectional radius as the base radius, generate a circular two-dimensional virtual area on the virtual safety monitoring surface as a dynamic repulsion spot. Calculate the moving speed of the dynamic projection center on the virtual safety monitoring surface in real time and dynamically adjust the radius of the dynamic repulsion spot according to the moving speed.

[0060] The physical cross-sectional radius of the outer drive link of the robotic arm refers to the geometric radius of the outer drive link on its cross-section perpendicular to its axis. This value is determined by the mechanical structural dimensions of the link and is precisely measured during robotic arm assembly and pre-stored in the system's robotic arm structural parameter library. The outer drive link typically adopts a circular or near-circular cross-section design to facilitate compact arrangement and flexible rotation of multiple links at a single-point constraint entry point. For links with non-circular cross-sections, the physical cross-sectional radius is taken as the radius of its largest circumcircle to ensure that collision detection covers all outer contour areas of the link's cross-section. The physical cross-sectional radius is obtained by calling the system's structural parameter library. During actual control operation, the system does not need to perform real-time measurements but directly reads the preset parameter values ​​as the basis for subsequent dynamic repulsion spot radius calculations.

[0061] The dynamic repulsion spot refers to a circular two-dimensional virtual region generated on the virtual safety monitoring surface with the dynamic projection center as its center. This region is not a physical entity, but rather a virtual safety boundary established by the control system in a two-dimensional plane, used to characterize the area occupied by the outer drive link in the spatial layer of the virtual safety monitoring surface. The design of the dynamic repulsion spot as circular is based on the following: the cross-section of the outer drive link is circular or approximately circular, and the circular spot can accurately enclose the outer boundary of the cross-section; the circle has rotational symmetry in the two-dimensional plane, so no matter how the link rotates around its axis, the circular spot can always completely cover any possible posture of the cross-section, eliminating the need to adjust the spot shape according to the link's rotation angle, thus simplifying the geometric calculations for collision detection. The dynamic repulsion spot, centered on the dynamic projection center, ensures that its position strictly corresponds to the projection position of the outer drive link on the virtual safety monitoring surface. When the dynamic projection center moves due to changes in the robotic arm's posture, the dynamic repulsion spot moves accordingly, achieving real-time tracking of the space occupied by the outer drive link.

[0062] The radius of the dynamic repulsion spot is not a fixed value, but a time-varying variable composed of three superimposed components. The dynamic repulsion spot radius equals the sum of the physical cross-sectional radius, the basic safety margin, and the velocity expansion term. The physical cross-sectional radius, as the basic component, characterizes the actual physical size of the outer drive link in the spatial layer of the virtual safety monitoring section, ensuring that the dynamic repulsion spot at least covers the actual cross-sectional area of ​​the link. The basic safety margin, as a static compensation component, is used to compensate for positional deviations caused by static uncertainties such as machining tolerances, assembly clearances, encoder measurement errors, and kinematic model simplification errors. The basic safety margin is determined by summing the maximum possible deviation values ​​of the above static error sources, synthesizing the comprehensive static error using the root mean square method, and then multiplying it by the safety margin coefficient to obtain the basic safety margin value. The sum of the physical cross-sectional radius and the basic safety margin is defined as the static radius of the dynamic repulsion spot. For example, when the physical cross-sectional radius is 1.5 cm, the comprehensive static error is 0.3 cm, and the safety margin coefficient is 1.2, the basic safety margin is 0.36 cm, and the static radius of the dynamic repulsion spot is 1.86 cm. The velocity expansion term, as a dynamic compensation component, is proportional to the moving speed of the dynamic projection center on the virtual safety monitoring cross-section. When the moving speed of the dynamic projection center increases, the velocity expansion term linearly increases the radius of the dynamic repulsion spot according to the preset velocity expansion ratio coefficient.

[0063] The calculation process for the moving speed of the dynamic projection center on the virtual safety monitoring plane includes: recording the coordinate values ​​of the dynamic projection center within consecutive control cycles; subtracting the coordinate values ​​from those of two adjacent control cycles to obtain the displacement vector; and then dividing the magnitude of the displacement vector by the control cycle duration to obtain the instantaneous moving speed of the dynamic projection center at the current moment. The direction of the moving speed is determined by the direction of the displacement vector, and the magnitude of the moving speed is determined by the ratio of the magnitude of the displacement vector to the control cycle duration. A speed expansion term is calculated based on the magnitude of the moving speed: the speed expansion term equals the product of the moving speed and the speed expansion proportionality coefficient. The speed expansion proportionality coefficient is determined as follows: based on the response delay time of the servo drive system, the inertial time constant of the mechanical transmission link, and the operation cycle of the control algorithm, the braking distance required for the robotic arm to come to a complete stop from issuing a braking command at the highest expected moving speed is calculated. Dividing the braking distance by the highest expected moving speed yields the baseline value of the speed expansion proportionality coefficient.

[0064] The principle behind introducing a velocity expansion term to dynamically adjust the radius of the dynamic repulsion spot is as follows: high-speed moving objects require longer braking distances and more reaction time to stop. If collision judgment is based solely on the static distance at the current moment, when the system detects that two spots are about to contact, due to mechanical inertia and servo response delay, the outer drive linkage cannot stop immediately and may continue to move in the collision direction during braking, resulting in actual contact. By making the radius of the dynamic repulsion spot expand linearly with increasing speed, the system automatically expands the virtual safety boundary of each robotic arm during high-speed movement, allowing collision warnings to be triggered at a greater distance, reserving sufficient response time and braking space for subsequent force intervention and braking operations. The velocity expansion term gives the dynamic repulsion spot a proactive safety warning capability: the faster the movement speed, the larger the spot expands, and the farther the warning trigger distance. This relationship naturally matches the laws of physical inertia, ensuring that the warning timing can adaptively compensate for braking requirements at different speeds. When the moving speed decreases or approaches zero, the speed expansion term automatically contracts, and the radius of the dynamically repelling light spot returns to its static size, avoiding unnecessary compression of the normal working space due to excessive expansion, thus achieving a dynamic balance between safety and work efficiency.

[0065] See Figure 3 In step S23, within the same control cycle, traverse the dynamic repulsion light spots corresponding to all robotic arms in the multi-robotic arm system, perform position determination and trend determination on any two dynamic repulsion light spots, and generate a collision warning flag when the position determination result and the trend determination result simultaneously meet the collision conditions, and set the collision warning flag position to the valid state.

[0066] Traversing the dynamic repulsion spots corresponding to all robotic arms in a multi-arm system within the same control cycle means: at each fixed control cycle, synchronously acquiring the dynamic repulsion spot status data of all robotic arms in the system on the virtual safety monitoring plane, and checking each possible interference spot combination one by one. The duration of the control cycle is consistent with the control cycle for acquiring joint encoder values ​​in step S12, and is usually set to the millisecond level to meet the response requirements of real-time collision detection. The traversal operation adopts a pairwise pairing method: when the multi-arm system contains multiple robotic arms, the traversal process sequentially selects all non-repeating robotic arm pairings, and performs collision risk judgment on the two dynamic repulsion spots in each pairing. For example, when the multi-arm system contains four robotic arms, the number of spot pairings to be checked is six groups, namely the dynamic repulsion spot combinations of the first robotic arm and the second robotic arm, the first robotic arm and the third robotic arm, the first robotic arm and the fourth robotic arm, the second robotic arm and the third robotic arm, the second robotic arm and the fourth robotic arm, and the third robotic arm and the fourth robotic arm.

[0067] The method for determining the position is as follows: Calculate the Euclidean distance between the centers of any two dynamic repulsion spots and compare it with the sum of the radii of the two dynamic repulsion spots. When the Euclidean distance is less than or equal to the sum of the radii, it is determined that the two dynamic repulsion spots have geometric interference. The Euclidean distance between the centers of the two dynamic repulsion spots is calculated using a two-dimensional planar distance formula. The coordinates of the two dynamic projection centers on the virtual safety monitoring sectional plane are used as input, and the straight-line distance between the two coordinate points is calculated as the center distance value. The sum of the radii of the two dynamic repulsion spots is equal to the arithmetic sum of the dynamic radius of the first dynamic repulsion spot and the dynamic radius of the second dynamic repulsion spot, where each dynamic radius is a time-varying radius value including a velocity expansion term calculated in step S22. The geometric significance of position determination lies in the following: In a two-dimensional plane, whether two circular regions contact or overlap depends on the distance between their centers, specifically the relationship between the Euclidean distance between their centers and the sum of their radii. When the distance between their centers is greater than the sum of their radii, the two circles are separate and not in contact. When the distance between their centers is equal to the sum of their radii, the two circles are externally tangent at a point. When the distance between their centers is less than the sum of their radii, the two circles intersect and overlap. Position determination uniformly classifies the externally tangent state (where the distance between their centers is equal to the sum of their radii) and the intersecting state (where the distance between their centers is less than the sum of their radii) as having geometric interference, ensuring that collision warnings are triggered as soon as the two light spots come into contact, avoiding the omission of critical collision scenarios due to lenient boundary condition judgments.

[0068] The method for trend determination is as follows: The approach rate is obtained by calculating the time derivative of the Euclidean distance between the centers of any two dynamic repulsion spots. When the approach rate is negative, the two dynamic repulsion spots are determined to be in a continuous approaching state. The derivative of the center distance with respect to time is achieved using a difference approximation method: the center distance value is recorded within a continuous control cycle; the distance change is obtained by subtracting the center distance value of the previous control cycle from the current control cycle's center distance value; and the approach rate is obtained by dividing the distance change by the control cycle duration. The sign of the approach rate represents the relative motion direction of the two spots: when the approach rate is negative, the center distance is decreasing, and the two spots are moving towards each other and continuously approaching; when the approach rate is positive, the center distance is increasing, and the two spots are moving away from each other and moving away from each other; when the approach rate is zero, the center distance remains unchanged, and the two spots are in a relatively stationary state or moving tangentially. The physical significance of trend determination lies in the fact that the static positional relationship at the current moment cannot distinguish between two completely opposite motion trends of "approaching" and "moving away". Introducing time differential information can capture the changing direction of the distance between the center and the circle, and incorporate the motion trend into the collision risk assessment, so that the determination result has the ability to predict the future state.

[0069] The collision condition is met when the position determination result satisfies geometric interference, and when the collision condition is met when the trend determination result satisfies continuous approach. Simultaneous collision conditions mean that both geometric interference and continuous approach are met at the same time. The composite determination logic combining position and trend determination is as follows: While position determination alone can detect whether geometric interference exists between two light spots at the current moment, it cannot distinguish between scenarios of "brief contact followed by separation" and "continuous approach and impending deep overlap." The former has a lower collision risk and requires no intervention, while the latter has a higher collision risk and requires immediate warning. Similarly, while trend determination alone can identify whether two light spots are moving towards each other, it cannot determine whether the current distance between the two light spots has entered a dangerous range; the relative movement of two light spots at a considerable distance does not pose an imminent threat. The composite determination logic requires both geometric interference and continuous approach to be met simultaneously to generate a collision warning flag. This requirement means that the system only determines that the outer driving domain is in a critical virtual collision state requiring intervention when two dynamically repulsive light spots have already contacted or overlapped and are still moving closer to each other. The composite judgment logic can effectively eliminate invalid static approach interference: In actual operation, the outer drive linkages of the two robotic arms may briefly approach each other and then move away from each other due to the operation task. If the warning is triggered when the two light spots come into contact based solely on the position judgment, a large number of situations that will not develop into actual collisions will be falsely alarmed. Frequent erroneous interventions will affect the smoothness of the operation. By introducing trend judgment as a second screening condition, it is possible to distinguish between the two scenarios of "approaching and about to move away" and "approaching and continuing to move deeper". The warning is triggered only for the latter, which greatly reduces the false alarm rate of the system.

[0070] The collision warning flag is a Boolean state variable generated internally by the control system. When both the position determination result and the trend determination result meet the collision conditions, the collision warning flag is set to an active state to trigger the force intervention process in subsequent step S30. Otherwise, the collision warning flag is in an inactive state. The generation of the collision warning flag is strictly synchronized with the determination result. The flag state is updated immediately after the determination calculation is completed in each control cycle to ensure that subsequent torque mapping and tactile feedback can respond promptly in the next control cycle. The design of the collision warning flag decouples the collision determination result from the force intervention action: the determination logic focuses on risk identification, while the force logic focuses on intervention execution. The two communicate asynchronously through the flag, facilitating independent debugging and parameter tuning.

[0071] Step S20 achieves a dimensionality reduction transformation from three-dimensional spatial interference detection to two-dimensional plane collision determination by constructing a virtual safety monitoring section within the outer drive domain and projecting the three-dimensional spatial attitude of the outer drive link onto the dynamic repulsion spot on the two-dimensional section. This dimensionality reduction transformation simplifies the computational object of the collision detection algorithm from the judgment of the intersection of the cylindrical envelope in three-dimensional space to the judgment of the tangency of the circle in two-dimensional plane, greatly reducing the computational complexity. When running in real time with a millisecond-level control cycle on a standard industrial controller, it can ensure the fast response performance in multi-robotic arm collaborative scenarios, so that collision detection calculation will not become a performance bottleneck of the control system. Step S20 establishes a dynamic correlation between the spot size and the linkage speed by introducing a velocity expansion term proportional to the moving speed of the dynamic projection center into the radius of the dynamic repulsion spot. This correlation enables the dynamic repulsion spot to automatically expand when driving the linkage to move at high speed on the outside, and automatically contract and return when moving at low speed or stationary. This establishes an adaptive match between the collision warning trigger distance and mechanical inertia and servo response delay. In high-speed scenarios, the warning distance increases to reserve sufficient space for braking operations, while in low-speed scenarios, the warning distance shrinks to avoid excessive intervention and compression of the working space. This achieves a balance between forward-looking dynamic safety warning and work efficiency. By mapping the spatial state of the outer drive link to a dynamic repulsion spot on a virtual safety monitoring surface and performing collision determination at the spot level, the link interference risk that originally occurred in three-dimensional physical space is transformed into a spot tangency determination in a two-dimensional virtual plane. This virtualization process allows the collision risk to be identified by the system before actual physical contact occurs, thus shifting collision protection from "post-event detection" to "pre-event warning." This provides an early intervention window for the force perception intervention in step S30, enabling operators to perceive the risk and proactively adjust their operations before the outer drive link experiences an actual collision, avoiding consequences such as motor overload protection triggering, robotic arm movement locking, and damage to the guide channel device caused by the collision.

[0072] Step S30: Calculate the virtual repulsion force vector in response to the collision warning flag, map the virtual repulsion force vector to the equivalent joint damping torque, and convert the equivalent joint damping torque into a current command compensation value and superimpose it into the current control loop of the servo driver of each joint of the robotic arm.

[0073] Further, see Figure 4 Step S30 includes:

[0074] Step S31: When the collision warning flag is in an active state, calculate the intrusion depth of the two dynamic repulsion spots with geometric interference. The intrusion depth is the difference between the sum of the radii of the two dynamic repulsion spots with geometric interference and the Euclidean distance between the centers of the two dynamic repulsion spots. Calculate the virtual repulsion force vector based on the intrusion depth. The direction of the virtual repulsion force vector is along the line connecting the centers of the two dynamic repulsion spots, pointing from the center of the other spot to the center of the dynamic projection of the robotic arm.

[0075] The collision warning flag is a Boolean state variable. When both the position determination result and the trend determination result simultaneously meet the collision conditions, the collision warning flag is set to an active state. Step S30, responding to the collision warning flag, means: within each control cycle, the state of the collision warning flag is detected. If and only if the collision warning flag is active, the virtual repulsion force calculation, torque mapping, and tactile feedback overlay operations of steps S31 to S33 are executed. When the collision warning flag is inactive, the subsequent operations of step S30 are not executed, and the robotic arm moves normally according to the operator's original control commands without applying any additional damping torque. This response mechanism ensures that force intervention is triggered only when a collision risk actually exists, avoiding unnecessary operational resistance interference during normal operation.

[0076] See Figure 5 This is a schematic diagram of the virtual repulsive force vector provided in the embodiments of this application. Figure 5 As shown, within the virtual safety monitoring section, the dynamic repulsion spot of this robotic arm partially overlaps with the dynamic repulsion spot of the adjacent robotic arm. The intrusion depth is a geometric parameter that quantifies the degree of overlap between the two dynamic repulsion spots. The calculation method is as follows: subtract the Euclidean distance between the centers of the two spots from the sum of the radii of the two dynamic repulsion spots that have geometric interference. The difference obtained is the intrusion depth. Figure 5 The radial width of the area indicated by the bidirectional arrow at the intersection of the two light spots visually demonstrates the intrusion depth. It characterizes the virtual overlap of the two outer drive links in the spatial layer of the virtual safety monitoring plane: a zero intrusion depth indicates that the two light spots are in tangential contact; a positive intrusion depth indicates an overlapping area, with larger positive values ​​indicating more severe overlap; and a negative intrusion depth indicates that the two light spots have not yet made contact. However, since the composite judgment logic in step S23 uses geometric interference as a trigger condition, the intrusion depth in the scenario entering step S31 must be non-negative. As the core input variable for subsequent virtual repulsion force calculation, the intrusion depth transforms the severity of collision risk into a quantifiable value, providing a quantitative basis for adjusting the intensity of force feedback.

[0077] The virtual repulsive force vector is a mathematical quantity generated by the control system based on intrusion depth calculations. This force vector does not actually exist in physical space, but rather is a virtual mechanical model constructed by the system to achieve force feedback. The virtual repulsive force vector includes two elements: magnitude and direction. Figure 5 As shown, the method for determining the direction of the virtual repulsive force vector is as follows: on the virtual safety monitoring section, Figure 5 The diagram illustrates a line connecting the two dynamic projection centers. For this robotic arm, the virtual repulsive force vector is represented by an arrow along the connecting line away from the center of the adjacent robotic arm. That is, its direction is from the dynamic projection center of the adjacent robotic arm that is geometrically interfering with this robotic arm to the dynamic projection center of this robotic arm. This direction represents the tendency for the dynamic repulsive light spot of this robotic arm to move away from the dynamic repulsive light spot of the adjacent robotic arm. For the adjacent robotic arm, as... Figure 5 As shown by the arrow on the other side, the direction of the virtual repulsion force vector is opposite, pointing from the dynamic projection center of this robotic arm to the dynamic projection center of the adjacent robotic arm. This direction is normalized to obtain the unit vector of the repulsion direction. The magnitude of the virtual repulsion force vector is calculated using the virtual impedance model. The virtual impedance model is a mathematical model that transforms collision risk into a virtual mechanical response. Its core idea is to simulate the contact mechanics behavior in real physical collisions: when two objects come into contact, the contact force increases with the depth of penetration. The virtual impedance model includes a virtual stiffness parameter, which characterizes the response intensity of the virtual repulsion force relative to the depth of penetration. To provide operators with an intuitive tactile experience, the virtual stiffness is designed nonlinearly: when the penetration depth is small, the virtual stiffness is low, and the virtual repulsion force increases slowly with the depth of penetration, resulting in a gentler resistance perceived by the operator; when the penetration depth increases, the virtual stiffness increases exponentially, and the virtual repulsion force increases sharply with the depth of penetration, rapidly increasing the resistance perceived by the operator until it approaches a rigid barrier. The virtual repulsion force is calculated using a nonlinear product of the virtual stiffness and the depth of penetration. The calculation relationship can be expressed as: ,in Indicates the foundation stiffness coefficient. Indicates the depth of intrusion. Represented by natural constant An exponential function with base 0. The characteristic length parameter is represented. Nonlinear virtual stiffness can be expressed as the basic stiffness coefficient and the exponential amplification factor. The product of the two factors, the basic stiffness coefficient, is configured based on the structural stiffness of the robotic arm, the rated torque of the servo motor, and the range of resistance that the operator expects to perceive. Specifically, the basic stiffness coefficient can be determined based on the ratio of the rated torque of the servo motor to the expected maximum feedback torque: First, determine the maximum feedback torque value that the operator expects to perceive at the maximum intrusion depth in the system design. This maximum feedback torque is usually taken as 30% to 50% of the rated torque of the servo motor to balance the safety feedback strength and the motor output margin; then, calculate the relationship based on the virtual repulsion force. Substituting the maximum intrusion depth (usually twice the static radius of the dynamic repulsion spot) and the corresponding maximum feedback torque, the inverse solution yields the foundation stiffness coefficient. ,in The force value after lever arm conversion for the expected maximum feedback torque. The maximum penetration depth is defined as the characteristic length parameter, which is configured based on the static radius of the dynamic repulsion spot. It is typically set to a value within the range of 50% to 100% of the static radius; that is, the characteristic length parameter can be calibrated based on 50% to 100% of the static radius of the dynamic repulsion spot. Alternatively, it can be determined through simulation experiments: different characteristic length parameter values ​​are set in the simulation environment to simulate the change curve of the virtual repulsion force as the penetration depth gradually increases from zero to its maximum value. The characteristic length parameter value is selected to give the force feedback a gradual transition characteristic of "gradually increasing flexibility at low penetration depths and rapid saturation at high penetration depths." The smaller the characteristic length parameter, the steeper the increase in nonlinear stiffness with penetration depth.

[0078] The principle behind nonlinear virtual stiffness design is that while linear stiffness models are simple to calculate, they cannot simultaneously meet the dual requirements of gentle warnings at low risk and rigid protection at high risk. If the linear stiffness is set too low, the repulsive force generated at high intrusion depths is insufficient to stop the operator's forceful advance; if the linear stiffness is set too high, significant resistance is generated at low intrusion depths, affecting the smoothness of normal operations. Nonlinear stiffness, through its exponential growth characteristic, provides gentle warning resistance in the low intrusion depth range, allowing the operator to perceive the risk of collision without hindering their adjustment of operating direction; in the high intrusion depth range, it provides sharply increased blocking resistance, making it difficult for the operator to continue moving in the direction of collision even if they attempt to forcefully advance, thus establishing a continuous transition from gentle warning to rigid protection at the tactile level. The virtual repulsive force vector is equal to the product of the magnitude of the virtual repulsive force and the unit vector of the repulsive direction. This vector is defined in the two-dimensional plane where the virtual safety monitoring section is located. Since the virtual safety monitoring section is parallel to the mounting plane of the robotic arm base and perpendicular to the longitudinal axis of the system, the two components of the virtual repulsive force vector correspond to the force components in the horizontal and vertical directions in the three-dimensional Cartesian coordinate system. By setting the force component in the vertical direction to zero, the two-dimensional virtual repulsive force vector can be extended into a force vector in the three-dimensional Cartesian space. The subsequent step S32 performs torque mapping on this three-dimensional force vector through the Jacobian transpose matrix.

[0079] Step S32: Construct a kinematic Jacobian matrix describing the mapping relationship between the angular velocity of each joint module and the Cartesian velocity at the outer virtual node. Map the virtual repulsive force vector to the torque vector corresponding to each joint module through the transpose of the kinematic Jacobian matrix, which serves as the equivalent joint damping torque.

[0080] The kinematic Jacobian matrix is ​​a fundamental mathematical tool in robot kinematics, used to describe the differential mapping relationship between the joint space and Cartesian space of a robotic arm. Each element of the kinematic Jacobian matrix represents the degree of influence of a small change in the corresponding joint angle on the Cartesian velocity of the end effector or a specific link point. The number of rows in the matrix equals the number of degrees of freedom in the Cartesian space, and the number of columns equals the number of joints in the robotic arm. The method for constructing the kinematic Jacobian matrix is ​​as follows: based on the Denavitt-Hartenberg parameter table of the robotic arm, the partial derivatives of each joint angle in the forward kinematic equations are calculated using the principles of differential kinematics, and the partial derivatives are arranged in rows and columns to form the Jacobian matrix. Since the virtual repulsive force vector generated in step S31 is defined on the virtual safety monitoring plane of the outer drive domain, step S32 needs to construct a mapping relationship from the angular velocity of each joint module to the Cartesian velocity at the outer virtual node, rather than the traditional mapping relationship from the joint angular velocity to the end effector velocity. The coordinates of the outer virtual node have been calculated in step S13 using rigid body geometric constraints. The kinematic Jacobian matrix is ​​constructed with the outer virtual node as the reference point, and the influence of the joint angle changes on the Cartesian velocity of that point is calculated. Since the main drive axis of the robotic arm is a rigid straight line, the coordinates of the outer virtual node, the kinematic pivot point, and the end effector are always collinear. The Jacobian matrix with the outer virtual node as the reference point can be constructed as follows: First, a Jacobian matrix describing the mapping relationship between the angular velocities of each joint and the Cartesian velocity at the kinematic pivot point is established according to the standard De Navet-Hartenberg method. Then, the product of the unit vector of the axis direction and the length of the outer drive link is used as the rigid offset vector from the kinematic pivot point to the outer virtual node. Based on the velocity transmission relationship between different points on the same rigid body in rigid body kinematics, the velocity reference point is translated from the kinematic pivot point to the outer virtual node. The physical principle behind this velocity transfer relationship is as follows: When a rigid body undergoes normal motion, the linear velocity of any point on the rigid body is equal to the linear velocity of another reference point on the rigid body plus the additional linear velocity generated by the rotation of the rigid body relative to that reference point. The direction of the additional linear velocity is perpendicular to the plane determined by the rigid body's angular velocity vector and the position vector between the two points. The magnitude of the additional linear velocity is equal to the product of the rigid body's angular velocity and the distance between the two points. When applying the above velocity transfer relationship to the transformation of the Jacobian matrix, the specific implementation is as follows: the cross product term of the rigid offset vector and the angular velocity component is superimposed on the linear velocity component of the original Jacobian matrix. This cross product term is the decomposition of the additional linear velocity generated by the rigid body's rotation of the outer virtual node relative to the kinematic pivot point in the direction of the joint angular velocity.

[0081] The transpose of the kinematic Jacobian matrix is ​​used to map the force vector in Cartesian space to the torque vector in joint space. This mapping is based on the principle of virtual work: under static equilibrium conditions, the virtual work done by the external force acting on a point in the link in Cartesian space and the virtual displacement of that point is equal to the sum of the virtual work done by the torques of each joint and the corresponding virtual angular displacements in joint space. Since a linear mapping is established between the virtual displacement in Cartesian space and the virtual angular displacement in joint space through the kinematic Jacobian matrix, according to the principle of virtual work equivalence, a dual mapping is established between the force vector in Cartesian space and the torque vector in joint space through the transpose of the kinematic Jacobian matrix. By transforming the virtual repulsive force vector generated in step S31 through the transpose of the kinematic Jacobian matrix, the equivalent torque vector acting on each relevant joint can be calculated. This torque vector is the equivalent joint damping torque.

[0082] The physical meaning of the equivalent joint damping torque is that, although the virtual repulsive force vector is defined in the outer drive domain and does not actually exist, through the mapping of the Jacobian transpose matrix, this virtual force is mechanically equivalent to the resisting torque applied at each joint. Since the outer drive link and the end effector of the inner working domain form a rigid lever relationship through the kinematic pivot point, the equivalent joint damping torque generated by the virtual repulsive force acting on the outside will manifest as operating resistance when the operator controls the inner end effector. For example, when the operator controls the inner end effector to converge toward the center of the working area, the outer drive link diverges outward. If the dynamic repulsive light spots of the two robotic arms undergo geometric interference at this time and satisfy the trend determination of continuous approach, the virtual repulsive force vector calculated in step S31 points in the direction that causes the light spots to separate. The equivalent joint damping torque obtained after mapping through the Jacobian transpose matrix will resist the operator's convergence operation command, causing the operator to perceive an increase in operating resistance. The principle behind force space mapping using the Jacobi transpose matrix is ​​that it accurately converts force vectors in Cartesian space into torque vectors in joint space, while adhering to the principle of energy conservation, ensuring the mechanical equivalence between virtual forces and equivalent torques. Compared to other torque allocation methods, the Jacobi transpose mapping has clear physical meaning and a rigorous mathematical derivation, eliminating the need for manually setting the weights of torque allocation for each joint. The mapping result naturally reflects the kinematic transmission characteristics of the robotic arm. For the robotic arm in different postures, the Jacobi matrix is ​​updated in real time with the posture change, and the equivalent joint damping torque obtained through the Jacobi transpose matrix mapping is also adaptively adjusted accordingly, ensuring that the force feedback remains consistent with the kinematic relationship of the current posture.

[0083] Step S33: Convert the equivalent joint damping torque into the current command compensation value of the servo motor, and superimpose the current command compensation value into the current control loop of the servo driver of each joint of the robotic arm. When the equivalent joint damping torque exceeds the preset safety threshold, the corresponding joint with the equivalent joint damping torque exceeding the safety threshold is forcibly locked to prevent the robotic arm from continuing to move in the collision direction.

[0084] Step S33 converts the equivalent joint damping torque into the current command compensation value of the servo motor. There is an approximately linear correspondence between the output torque of the servo motor and the drive current, characterized by a torque constant, defined as the ratio of the servo motor's rated torque to its rated current. The torque constant is obtained by: reading the rated torque and rated current parameters from the servo motor's technical data sheet and dividing the rated torque by the rated current; or obtaining a more accurate torque constant value through actual measurement and calibration during system debugging. Dividing the equivalent joint damping torque by the torque constant of the corresponding joint servo motor yields the current value required to generate that damping torque, which is the current command compensation value.

[0085] The process of superimposing the current command compensation value onto the current control loop of the servo driver for each joint of the robotic arm is as follows: The control architecture of the servo driver typically adopts a three-loop nested structure of position control loop, speed control loop, and current control loop. The current control loop, as the innermost control loop, directly controls the drive current of the motor windings. Under normal operating conditions, the reference input of the current control loop comes from the output of the speed control loop, the reference input of the speed control loop comes from the output of the position control loop, and the reference input of the position control loop comes from the operator's motion control command. Step S33 linearly superimposes the current command compensation value as an additional quantity onto the reference input of the current control loop. The superimposed current reference value is equal to the original current reference value output by the speed control loop plus the current command compensation value. Since the direction of the current command compensation value is opposite to the direction of the motion command issued by the operator that may cause a collision risk, the net torque output by the servo motor after superposition will generate a resistance effect against the operating direction. When the operator attempts to continue pushing in the direction of the collision, the motor drive current output by the current control loop includes a compensation component that is opposite to the operation command. The net torque after subtracting the damping torque from the actual output torque of the motor decreases, and the operator's hand feels an increase in operating resistance through the operating handle.

[0086] The principle behind superimposing the current command compensation value onto the current control loop instead of the position or speed control loop is as follows: the current control loop is the fastest responding control loop in a servo system, with a control cycle typically in the microsecond range, far faster than the millisecond-level control cycles of the position and speed control loops. Injecting the compensation value into the current control loop achieves the fastest force response speed, allowing the operator to perceive the resistance change almost simultaneously with the collision warning flag. Furthermore, the current control loop directly controls the motor torque; converting torque compensation into current compensation before injecting it into the current control loop results in the shortest signal path, avoiding phase delays and amplitude attenuation caused by passing through the position or speed control loop, ensuring timely and accurate force feedback. By superimposing the compensation value onto the current control loop instead of cutting off power output or triggering emergency stop protection, the robotic arm's motion control system continues to operate normally. The operator can still adjust the robotic arm's position within a certain range by increasing the operating force to overcome resistance. The system will not forcibly interrupt operation due to a collision warning, ensuring the continuity and smoothness of the operation.

[0087] The process of forcibly locking the relevant joints to prevent the robotic arm from continuing to move in the collision direction when the equivalent joint damping torque exceeds a preset safety threshold is as follows: The system presets a safety threshold, which represents the maximum allowable cumulative damping torque. The safety threshold is determined by comprehensively evaluating the rated torque of the servo motor, the load-bearing capacity of the robotic arm structure, and the operator's maximum operating force. The minimum limit value among the above factors is multiplied by a safety margin coefficient to obtain the safety threshold. For example, the safety margin coefficient can be a value in the range of 0.8 to 0.9. In each control cycle, the equivalent joint damping torque is compared with the safety threshold: when the equivalent joint damping torque is less than the safety threshold, the current command compensation value is superimposed on the current control loop according to the normal process of step S33; when the equivalent joint damping torque reaches or exceeds the safety threshold, it is determined that the collision risk has been upgraded to a dangerous level, and the system immediately switches to the forced locking mode. The position control loop of the corresponding joint whose equivalent joint damping torque exceeds the safety threshold is set to a position holding state, prohibiting the joint from continuing to move in the collision direction, and only allowing movement in the direction away from the collision. The method for determining whether a joint's movement direction is "towards the collision direction" is as follows: This is determined by calculating the projection of the joint's angular velocity onto the current virtual repulsion force direction. Specifically, the current angular velocity of each joint is mapped to a Cartesian velocity vector at the outer virtual node using the kinematic Jacobian matrix constructed in step S32. The dot product of this Cartesian velocity vector and the virtual repulsion force vector is calculated. If the dot product is negative, meaning the Cartesian velocity direction is opposite to the repulsion force direction, it indicates that the joint's movement trend is towards the collision direction (angular velocity direction is opposite to the repulsion force direction), and the joint is locked to prevent further movement. If the dot product is positive or zero, meaning the Cartesian velocity direction is the same as or orthogonal to the repulsion force direction, it indicates that the joint's movement trend is away from or unrelated to the collision direction (angular velocity direction is the same as or orthogonal to the repulsion force direction), and the joint is allowed to continue moving. This direction determination method ensures that forced locking only restricts the movement direction that intensifies the collision, without hindering the operator's ability to actively move away from the collision area in the opposite direction. The exit condition for the forced locking mode is: the operator actively reverses the operation to reduce the equivalent joint damping torque to below the safety threshold, and the lock is automatically released to restore the normal control mode.

[0088] The technical principle behind the graded safety strategy is that relying solely on flexible damping feedback may not prevent strong operator misoperation. If the operator continues to move towards the collision direction due to distraction or misjudgment, even with increasing resistance, the outer drive link may still experience an actual physical collision. By setting a safety threshold and initiating mandatory locking upon triggering the threshold, a rigid locking protection layer is established on top of the flexible warning layer. This ensures that even if the operator fails to respond to the flexible damping prompts in time, mandatory intervention can prevent an actual collision. The graded transition from flexible damping to rigid locking avoids operational interruptions caused by emergency stop protection: in the early stages of collision risk, flexible damping guides the operator to avoid collisions autonomously, ensuring smooth operation; when the risk level rises to a dangerous level, rigid locking forcibly prevents dangerous operations, ensuring inherent safety.

[0089] Step S30 establishes a non-visual tactile feedback channel, transforming the virtually calculated collision risk in the outer drive domain into a perceptible operational resistance in the inner working domain. This allows the operator to perceive the collision risk on the outside without shifting their visual attention from the internal working field of view to the external monitoring screen, relying solely on their instinctive tactile sense. This tactile feedback mechanism utilizes the human body's instinctive force perception: when the operator controls the robotic arm's movement via the control handle, the change in resistance transmitted by the handle can be immediately perceived by the operator's hand muscles and tactile nervous system, without the delay of visual information processing and cognitive judgment. By employing a nonlinear virtual impedance model to calculate the virtual repulsive force, step S30 achieves continuous tactile feedback from flexible viscosity to rigid saturation locking: when the collision risk is low, the resistance perceived by the operator is similar to the gentle resistance felt when pushing an object in a viscous fluid, indicating a potential risk in the current operating direction without hindering adjustment; when the collision risk is high, the resistance perceived by the operator is similar to the strong resistance felt when touching an elastic barrier or a rigid wall, preventing the operator from continuing in the dangerous direction. By mapping the force space of the Jacobian transpose matrix, step S30 accurately converts the two-dimensional virtual repulsive force defined on the virtual safety monitoring section of the outer drive domain into an equivalent damping torque acting on each joint. This ensures that the force feedback is consistent with the kinematic relationship of the current posture of the robotic arm, and that the direction of resistance perceived by the operator precisely corresponds to the direction of operation that leads to the collision risk. By superimposing the current command compensation value onto the current control loop of the servo driver instead of triggering emergency stop protection, step S30 provides a collision risk warning while maintaining the normal operation of the robotic arm motion control system. The operator can still adjust the position of the robotic arm within a certain range, and the operation will not be forcibly interrupted due to the collision warning. This ensures both safety and the continuity and smoothness of the operation. By setting a safety threshold and initiating forced locking when the threshold is triggered, step S30 establishes a rigid protection layer on top of the flexible damping warning. This ensures that even if the operator fails to respond to the flexible prompt in time, the system can still prevent actual physical collisions of the outer drive linkage through forced intervention, avoiding consequences such as motor overload protection triggering, robotic arm motion locking, and damage to the guide channel device caused by the collision.

[0090] Example 2:

[0091] This embodiment, based on Embodiment 1, provides a robot multi-arm collaborative collision avoidance control system, such as... Figure 6 As shown, it includes:

[0092] The outer state reconstruction module is used to establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin and to divide the inner working domain and the outer driving domain. It also defines the outer virtual node in the three-dimensional Cartesian global reference coordinate system.

[0093] Collision Trend Determination Module: This module is used to construct a virtual safety monitoring section within the outer drive domain, connect the outer virtual nodes with the kinematic pivot points to form the physical axis of the outer drive link, calculate the intersection of the physical axis of the outer drive link and the virtual safety monitoring section as the dynamic projection center, generate a dynamic repulsion spot with the dynamic projection center as the center, traverse the dynamic repulsion spots of all robotic arms to perform a collision trend composite determination that combines position determination and trend determination, and generate a collision warning flag.

[0094] Force feedback control module: Calculates virtual repulsion force vector in response to collision warning flag, maps virtual repulsion force vector to equivalent joint damping torque, and converts equivalent joint damping torque into current command compensation value and superimposes it into the current control loop of the servo driver of each joint of the robotic arm.

[0095] Furthermore, in the outer state reconstruction module, the kinematic pivot point refers to the position coordinates of the multi-manipulator system passing through the single-point constraint entrance;

[0096] The method for dividing the inner working domain and the outer driving domain is as follows: the positive direction of the vertical axis of the three-dimensional Cartesian global reference coordinate system is defined as the insertion direction, the spatial region located downstream of the kinematic pivot point along the insertion direction is defined as the inner working domain, and the spatial region located upstream of the kinematic pivot point along the insertion direction is defined as the outer driving domain.

[0097] The method for defining outer virtual nodes in a three-dimensional Cartesian global reference coordinate system includes:

[0098] Obtain the end-effector coordinates of the robotic arm, construct a unit vector along the axis direction based on the end-effector coordinates and the kinematic pivot point, and extend the unit vector along the axis direction to obtain the outer virtual node.

[0099] The method for obtaining the end position coordinates includes: real-time acquisition of absolute position encoder values ​​of each robotic arm joint module, and calculation of the spatial coordinates of the robotic arm end in the inner working domain using the absolute position encoder values ​​as the end position coordinates;

[0100] The method for constructing the axial direction unit vector is as follows: construct a direction vector from the end position coordinates to the kinematic pivot point based on the end position coordinates and the kinematic pivot point, and then normalize the direction vector to obtain the axial direction unit vector.

[0101] Furthermore, in the collision trend determination module, the method for composite collision trend determination includes:

[0102] Within the same control cycle, the dynamic repulsion light spots corresponding to all robotic arms in the multi-robotic arm system are traversed. The position and trend of any two dynamic repulsion light spots are determined. When the position determination result and the trend determination result simultaneously meet the collision condition, a collision warning flag is generated and the collision warning flag is set to an effective state.

[0103] The method for determining the location is as follows:

[0104] Calculate the Euclidean distance between the centers of any two dynamic repulsion spots and compare it with the sum of the radii of the two dynamic repulsion spots. When the Euclidean distance is less than or equal to the sum of the radii, it is determined that there is geometric interference between the two dynamic repulsion spots.

[0105] The method for determining the trend is as follows: calculate the time derivative of the Euclidean distance between the centers of any two dynamic repulsion spots to obtain the approach rate. When the approach rate is negative, it is determined that the two dynamic repulsion spots are in a state of continuous approach.

[0106] The simultaneous fulfillment of the collision condition by the position determination result and the trend determination result means that both geometric interference and the continuous approach state are simultaneously established.

[0107] The methods and systems of this application may be implemented in many ways. For example, they may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above-described order of steps for the method is for illustrative purposes only, and the steps of the method of this application are not limited to the order specifically described above, unless otherwise specifically stated.

[0108] In addition, the parts of the technical solutions provided in the embodiments of this application that are consistent with the implementation principles of the corresponding technical solutions in the prior art have not been described in detail, so as to avoid excessive elaboration.

[0109] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the invention. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for collaborative collision avoidance control of multiple robotic arms in a robot, characterized in that, The method includes: A three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin is established and the inner working domain and the outer driving domain are divided. In the three-dimensional Cartesian global reference coordinate system, the outer virtual node is defined, including obtaining the end position coordinates of the robot arm, constructing the axial direction unit vector based on the end position coordinates and the kinematic pivot point, and extending along the axial direction unit vector to obtain the outer virtual node. A virtual safety monitoring section is constructed within the outer drive domain. The outer virtual nodes are connected to the kinematic pivot points to form the physical axis of the outer drive link. The intersection of the physical axis of the outer drive link and the virtual safety monitoring section is calculated as the dynamic projection center. A dynamic repulsion spot is generated with the dynamic projection center as the center. The radius of the dynamic repulsion spot is dynamically adjusted according to the moving speed of the dynamic projection center on the virtual safety monitoring section. The collision trend composite judgment combining position determination and trend determination is performed on all the dynamic repulsion spots of the robotic arm and a collision warning flag is generated. The method for composite collision trend determination includes: traversing the dynamic repulsion light spots corresponding to all robotic arms in the multi-robotic arm system within the same control cycle, performing position determination and trend determination on any two dynamic repulsion light spots, generating a collision warning flag when the position determination result and the trend determination result simultaneously meet the collision conditions, and setting the collision warning flag to an effective state. The method for determining the position is as follows: calculate the Euclidean distance between the centers of any two dynamic repulsion spots and compare it with the sum of the radii of the two dynamic repulsion spots. When the Euclidean distance is less than or equal to the sum of the radii, it is determined that the two dynamic repulsion spots have geometric interference. The method for determining the trend is as follows: calculate the time derivative of the Euclidean distance between the centers of any two dynamic repulsion spots to obtain the approach rate. When the approach rate is negative, it is determined that the two dynamic repulsion spots are in a continuous approaching state. The collision condition that the position determination result and the trend determination result simultaneously satisfy means that both geometric interference and the continuous approaching state are simultaneously established. When the collision warning flag is active, the intrusion depth of the two dynamic repulsion light spots with geometric interference is calculated. The virtual repulsion force vector is calculated based on the intrusion depth. The virtual repulsion force vector is mapped to the equivalent joint damping torque. The equivalent joint damping torque is converted into a current command compensation value and superimposed on the current control loop of the servo driver of each joint of the robotic arm.

2. The robot multi-arm cooperative collision avoidance control method according to claim 1, characterized in that, The kinematic pivot point refers to the position coordinates of the multi-manipulator system passing through the single-point constraint entrance; The method for dividing the inner working domain and the outer driving domain is as follows: the positive direction of the vertical axis of the three-dimensional Cartesian global reference coordinate system is defined as the insertion direction, the spatial region located downstream of the kinematic pivot point along the insertion direction is defined as the inner working domain, and the spatial region located upstream of the kinematic pivot point along the insertion direction is defined as the outer driving domain.

3. The robot multi-arm cooperative collision avoidance control method according to claim 2, characterized in that, The method for obtaining the end position coordinates includes: real-time acquisition of absolute position encoder values ​​of each robotic arm joint module, and calculation of the spatial coordinates of the robotic arm end in the inner working domain using the absolute position encoder values ​​as the end position coordinates; The method for constructing the axial direction unit vector is as follows: construct a direction vector from the end position coordinates to the kinematic pivot point based on the end position coordinates and the kinematic pivot point, and then normalize the direction vector to obtain the axial direction unit vector.

4. The robot multi-arm cooperative collision avoidance control method according to claim 3, characterized in that, The method for obtaining the outer virtual node by extending a unit vector along the axial direction includes: The length of the rigid link in the outer drive domain is obtained as the length of the outer drive link. The distance of the outer drive link length is extended from the kinematic pivot point to the outer drive domain direction by a unit vector along the axis. The position point corresponding to the end point of the extension in the three-dimensional Cartesian global reference coordinate system is defined as the outer virtual node.

5. A robot multi-arm cooperative collision avoidance control system, used to implement the robot multi-arm cooperative collision avoidance control method according to any one of claims 1-4, characterized in that, The system includes: The outer state reconstruction module is used to establish a three-dimensional Cartesian global reference coordinate system with the kinematic pivot point as the origin and to divide the inner working domain and the outer driving domain. It also defines the outer virtual node in the three-dimensional Cartesian global reference coordinate system. Collision Trend Determination Module: This module is used to construct a virtual safety monitoring section within the outer drive domain, connect the outer virtual nodes with the kinematic pivot points to form the physical axis of the outer drive link, calculate the intersection of the physical axis of the outer drive link and the virtual safety monitoring section as the dynamic projection center, generate a dynamic repulsion spot with the dynamic projection center as the center, traverse the dynamic repulsion spots of all robotic arms to perform a collision trend composite determination that combines position determination and trend determination, and generate a collision warning flag. Force feedback control module: Calculates virtual repulsion force vector in response to collision warning flag, maps virtual repulsion force vector to equivalent joint damping torque, and converts equivalent joint damping torque into current command compensation value and superimposes it into the current control loop of the servo driver of each joint of the robotic arm.

Images