Stress-free docking and distributed force control for multi-robot cooperative work

Abstract

The application discloses a stress-free docking and distributed force control method for multi-robot cooperative work and belongs to the technical field of multi-robot cooperative control. The method comprises the following steps: a host robot is docked at the end through visual guidance, a weak electric communication link is established, and safety handshake verification is completed; after the verification, an adaptive admittance control law is constructed based on six-dimensional force sensor data, a compliant search is executed, and mechanical locking stress-free closure is triggered when the force in each direction meets the zero force condition and is stable; after locking, a connection topology matrix is generated, each end force and motion data are fused by using an extended Kalman filter, the virtual combined mass center and inertia parameters are identified online, and the robot end force compensation is calculated and issued for execution with joint torque distribution as the optimization target under the constraint of the required resultant force and moment for the desired motion. The application solves the problems of large stress in existing rigid docking and instability caused by internal force coupling in cooperative handling, and realizes stress-free docking and compliant cooperative handling.

Description

Technical Field

[0001] This invention relates to the field of multi-robot collaborative control technology, and in particular to a stress-free docking and distributed force control method for multi-robot collaborative operation. Background Technology

[0002] In industrial settings such as aerospace assembly, shipbuilding, large wind turbine blade installation, and heavy machinery manufacturing, multiple industrial robots are often required to collaboratively handle large, heavy, or weakly rigid objects. These operations typically require each robot's end effector to first physically connect with the object being handled or its auxiliary tooling to form a stable mechanical interface. Then, multiple robots work together to complete a six-degree-of-freedom spatial handling trajectory. Due to manufacturing errors, installation errors, and limitations in the robots' absolute positioning accuracy, the initial pose of each robot's end effector before connection is unavoidable. Forcibly locking the mechanical interface while these pose deviations exist will introduce significant assembly stress into the assembled structure, potentially damaging the object being handled or the robot's end effector.

[0003] In existing technologies, control methods for multi-robot collaborative docking and handling mainly fall into two categories. One category is based on rigid position servo control, which pre-plans the motion trajectories of each robot through master-slave kinematic inverse kinematics, causing each end effector to move to a predetermined docking position and then mechanically lock. This method has extremely low tolerance for end effector pose errors. When the actual pose deviates from the predetermined position due to calibration errors or elastic deformation, the mechanical interface will forcibly close under stress, generating non-negligible contact forces and assembly stresses. The other category employs a force-position hybrid control approach, but it primarily relies on centralized planning, lacking a decoupling mechanism for internal force coupling between the robotic arms. During collaborative handling, the end effectors of each robot are rigidly connected to the object being handled, forming a closed-loop system. Even minor pose disturbances or external interference from any robot will be transmitted to other robots through structural coupling, causing fluctuations in internal compressive or tensile stresses. Due to the lack of real-time internal force sensing and decoupling compensation methods based on force feedback, such internal force fluctuations are difficult to be effectively identified and suppressed by each robot controller, which can easily lead to instability of the combined dynamic model, affect the tracking accuracy of the transport trajectory, and even damage the object or robot joints due to excessive accumulated internal force when transporting large inertia objects.

[0004] Furthermore, during the docking phase, existing force-controlled docking methods largely rely on impedance control strategies with fixed parameters. Fixed impedance parameters struggle to balance rapid compliance with large deviations and stable convergence with small deviations. When the force converges to near zero, repeated collisions can occur due to inertial overshoot or sensor noise, making it difficult to maintain a stable zero-force state. During the transport phase, existing methods typically rely on offline theoretical calculations to obtain the position of the virtual assembly's center of mass and inertial parameters. This fails to track parameter perturbations caused by changes in the object's internal mass distribution or load deformation online, thus limiting the long-term accuracy of force distribution strategies.

[0005] Therefore, existing technologies have problems in multi-robot collaborative docking and handling processes, such as incomplete elimination of docking stress, lack of internal force coupling decoupling mechanism, and insufficient online sensing capability of combined dynamic parameters. A method needs to be designed to achieve stress-free docking and force control between robots. Summary of the Invention

[0006] This invention overcomes the problems of high stress during rigid docking of robots and instability caused by internal force coupling during collaborative transport in the prior art, and realizes stress-free docking and compliant collaborative transport.

[0007] To achieve the above objectives, the present invention adopts the following solution: A stress-free docking and distributed force control method for multi-robot collaborative operation, wherein the robots include a master robot and slave robots connected by mechanical locking, and the method includes the following steps: S1: The main robot acquires images of visual marker points at the robot's end via the vision system, calculates the relative pose deviation, and drives the robot's end to move until the low-voltage communication interface set at the end is connected, thus establishing a low-voltage communication link. S2: Perform master-slave authentication and force sensor status verification through the weak current communication link. If the authentication and verification are successful, the handshake success flag is set as the compliant search enable signal. If the handshake is unsuccessful, the mechanical lock closure and high power connection are prohibited. S3: When the handshake success flag is set, the master robot acquires the six-dimensional force sensor data of itself and each slave robot end effector, constructs the admittance control law, and adjusts the impedance parameters of the admittance control law online according to the six-dimensional force sensor data, generates the end effector pose fine-tuning amount, and drives each robot end effector to perform six-degree-of-freedom compliant motion. S4: When the data from the six-dimensional force sensor indicates that the force in each direction meets the preset zero force condition and remains stable for a preset duration, the main robot sends a mechanical latch locking enable signal to drive the electromagnetic latch to close, so that the ends of the multiple robots are rigidly connected to form a combined body. S5: After locking is completed, the main robot generates a connection topology matrix based on the electrical connectivity status of each interface, and uses an extended Kalman filter to fuse the six-dimensional force sensor data and end motion state of each robot end based on the connection topology matrix, and identifies the center of mass position and inertial parameters of the virtual assembly online. S6: Based on the identified center of mass position and inertial parameters, under the constraints of the resultant force and resultant torque required for the desired motion of the combined body, the desired end force compensation of each robot is calculated with the torque distribution of each robot joint as the optimization objective, and the desired end force compensation is sent to the local force-position hybrid controller of each robot for execution.

[0008] Preferably, step S1 includes: The vision system acquires images of at least three visual markers set at the end of the robot, uses a multi-point perspective imaging algorithm to calculate the relative pose deviation of the end of the robot relative to the end of the main robot, and converts the relative pose deviation to the base coordinate system of the main robot. During the motion of the robot's end effector, a filter is used to smooth the estimated sequence of relative pose deviations to generate continuous motion control commands. Simultaneously, the remaining distance between the end effector and the target is calculated in real time based on the relative pose deviation. When the remaining distance is greater than a preset first distance threshold, the end effector is controlled to move at a first speed. When the remaining distance is less than or equal to the first distance threshold, the end effector is controlled to move at a second speed, which is lower than the first speed. When the low-voltage circuit inside the low-voltage communication interface is detected to be connected, the low-voltage communication interface is determined to be connected, the end effector motion is immediately stopped, and a low-voltage communication link is established.

[0009] Preferably, step S2 includes: Through a low-voltage communication link, the master robot sends a challenge message containing a dynamic random number to each slave robot. Each slave robot encrypts the dynamic random number using a preset shared key, generates a response message, and sends it back. The master robot decrypts and verifies the response message to complete the master-slave authentication. During the identity authentication process, the force sensor status verification is performed simultaneously. The force sensor status verification includes: obtaining the zero-point offset value, working temperature parameter, and bit error rate of the weak current communication link of each six-dimensional force sensor from the robot end. If the zero-point offset value exceeds the preset offset tolerance, or the working temperature parameter exceeds the preset working temperature range, or the bit error rate exceeds the preset bit error rate threshold, the force sensor status is determined to be abnormal. When authentication is successful and all force sensors are functioning normally, the main robot sets a handshake success flag as a compliant search enable signal. When authentication fails or any force sensor is abnormal, the main robot broadcasts a lock prohibition command through the low-voltage communication link, locking the mechanical latch's drive circuit in the open state and cutting off the high-voltage connection control circuit. At the same time, the main robot records abnormal event information including the failure type and timestamp, and triggers an alarm indication.

[0010] Preferably, step S3 includes: An admittance control law is constructed using a decoupled six-degree-of-freedom second-order admittance model. The impedance parameters of the admittance control law include a virtual inertia matrix, a virtual damping matrix, and a virtual stiffness matrix. Online adjustment of the parameters includes: The acquired six-dimensional force sensor data is low-pass filtered to obtain filtered force vectors and torque vectors. Based on the filtered force vectors and torque vectors, the magnitude of the force component in each coordinate axis direction is calculated. The magnitude of the force component is input into a preset monotonically decreasing function to generate a corresponding virtual stiffness adjustment coefficient. The virtual stiffness adjustment coefficient is then multiplied by the diagonal element in the virtual stiffness matrix corresponding to that coordinate axis, so that the virtual stiffness is lower in the direction with greater force. At the same time, the rate of change of the filtered force vector is calculated. For coordinate axes with a negative rate of change and a force component magnitude greater than a preset overshoot suppression threshold, the damping coefficient in the virtual damping matrix corresponding to that coordinate axis is increased. Substitute the adjusted virtual damping matrix and virtual stiffness matrix into the admittance control law to solve for the end-effector pose fine-tuning amount, so as to drive each robot end to perform six-degree-of-freedom compliant motion until the force components in each direction converge to the range allowed by the preset zero force condition.

[0011] Preferably, when the force component amplitudes in each coordinate axis direction are all lower than the preset mode switching threshold, and the mode switching threshold is less than the preset overshoot suppression threshold, the online adjustment also includes: Set all diagonal elements of the virtual stiffness matrix to the preset minimum stiffness value and set all diagonal elements of the virtual damping matrix to the preset maximum damping value, so that the robot end effector enters the isotropic fine compliant attitude adjustment mode; in the fine compliant attitude adjustment mode, the end effector pose fine adjustment amount solved by the admittance control law is limited, and the limiting threshold is set according to the allowable assembly gap of the mechanical latch. The system monitors six-dimensional force sensor data in real time. If the amplitude fluctuation of the force component in any direction exceeds the preset fluctuation tolerance, the corresponding virtual damping coefficient is temporarily increased according to the fluctuation amplitude in that direction until the force component fluctuation in that direction returns to within the fluctuation tolerance. Then, the virtual damping coefficient is restored to the preset maximum damping value.

[0012] Preferably, step S4 includes: The preset zero-force condition includes force thresholds and torque thresholds set for each channel of the six-dimensional force sensor. When the absolute value of the force component of each channel is lower than the corresponding force threshold and the absolute value of the torque component of each channel is lower than the corresponding torque threshold, it is determined that the force in each direction meets the preset zero-force condition. A timer is set to count the duration for which the preset zero-force condition is continuously met. The timer is reset when the absolute value of the force component or the absolute value of the torque component in any channel exceeds the corresponding threshold. During the timer's counting process, a fault-tolerant counter is set. If the force component or torque component momentarily exceeds the tolerance within a single sampling period, but the magnitude of the exceedance is lower than the preset tolerance margin, and the number of channels exceeding the tolerance does not exceed the preset upper limit of the number of channels, the fault-tolerant counter is incremented without resetting the timer. The timer is reset and the fault-tolerant counter is cleared only when the accumulated value of the fault-tolerant counter reaches the preset upper limit of the number of tolerances. When the timer reaches the preset stable duration, the main robot sends a mechanical latch locking enable signal to drive the electromagnetic latch to close. At the same time, it stops the operation of the admittance control law and latches the final impedance parameter adjusted online by the admittance control law as the initial impedance reference value for the collaborative transport stage of the combined body.

[0013] Preferably, step S5 includes: The main robot detects the electrical connectivity of the weak current communication links in each interface, records the interface connection as the first logic value, and records the interface disconnection as the second logic value, and generates a connection topology matrix, which represents the connection relationship between each robot end effector. The six-dimensional force sensor data of each robot end effector are transformed into a global coordinate system with the estimated center of mass of the virtual assembly as the origin through a force transformation matrix. The force transformation matrix is ​​jointly determined by the current pose of each robot end effector and the connection topology matrix. The extended Kalman filter is initialized, with the estimated center of mass position as the initial value of the center of mass position, the theoretical inertial parameters of the virtual assembly as the initial values ​​of the inertial parameters, and the initial state covariance matrix is ​​set. Within each filtering cycle, a state prediction model is constructed based on the acceleration signal of the desired motion trajectory of the combined body, the state prediction value is calculated, and the resultant force and torque of the six-dimensional force sensor after being transformed to the global coordinate system by the force transformation matrix, as well as the synthetic motion state of each robot end effector obtained by the joint encoder and the forward kinematic solution, are used as joint observations to correct the state prediction value. The online identification results of the centroid position and inertial parameters of the virtual combined body are then output.

[0014] As a preferred embodiment, during the operation of the extended Kalman filter, residual detection is performed on the resultant force and resultant torque of the six-dimensional force sensor in the joint observation. When the residual exceeds the preset residual abnormality threshold, it is determined that the corresponding six-dimensional force sensor data has a measurement abnormality. The observation noise covariance matrix corresponding to the six-dimensional force sensor data is temporarily amplified, and the amplified observation noise covariance matrix is ​​used for state correction within the filtering cycle. When the residual exceeds the residual abnormal threshold for multiple consecutive filtering cycles, the master robot sends a force sensor re-inspection command to the corresponding slave robot through a weak current communication link. If the re-inspection result returned by the slave robot confirms that the force sensor is abnormal, the master robot removes the six-dimensional force sensor data corresponding to the slave robot from the joint observation and regenerates the connection topology matrix. At the same time, it updates the force transformation matrix according to the regenerated connection topology matrix to maintain the continuity of the online identification of the centroid position and inertial parameters of the virtual assembly.

[0015] Preferably, step S6 includes: Based on the identified center of mass position and inertial parameters of the virtual assembly, a rigid body dynamics equation is constructed for the virtual assembly. The desired end force of each robot is mapped to the resultant force and resultant torque at the center of mass of the virtual assembly through the rigid body dynamics equation. The desired end force of each robot is decomposed into kinematic force components and internal force components. The kinematic force components are used to synthesize the resultant force and resultant torque required for the desired motion of the assembly. The internal force components are located in the zero space of the spinor of the end contact force of each robot, satisfying the constraint that the sum of the internal force components is zero, and their contribution to the resultant force and resultant torque at the center of mass is zero. The optimization objective is the weighted sum of squares of the joint torques of each robot. The weight coefficient of each joint is set according to the reciprocal of the rated torque of the corresponding joint. The joint torque is obtained by mapping the desired end effector force to the joint space using the force Jacobian matrix of each robot. The constraints include the equality constraint that the motion force components satisfy the required resultant force and resultant torque, and the friction cone inequality constraint applied to the desired end effector force of each robot. The friction cone inequality constraint is used to prevent relative sliding between the end effector and the object contact surface. A quadratic programming solver is used to solve the optimization objective under the constraints and output the desired end effector force compensation amount of each robot.

[0016] Preferably, in each control cycle, the mass matrix and Coriolis force terms in the rigid body dynamics equations are updated based on the current inertial parameters identified online by the extended Kalman filter. The system monitors the desired end-effector force compensation for each robot. If any force or torque component of the desired end-effector force compensation exceeds the end-effector force range corresponding to the torque output limit of the robot, the joint weight coefficient of the robot is multiplied by a preset amplification factor, and the quadratic programming solver is retried until all desired end-effector force compensations are within the end-effector force range. A penalty term is added to the objective function of the quadratic programming solver to penalize the difference between the desired end-effector force compensation in the current control cycle and the previous control cycle. If the limit is still exceeded after a preset number of re-solutions, the acceleration of the desired motion of the assembly is scaled proportionally, and the scaled acceleration is re-input into the rigid body dynamics equations.

[0017] The present invention includes at least the following beneficial effects: (1) By combining visual guidance with compliant search based on adaptive impedance, active force search and zero force determination are performed before the mechanical latch closes, so that the locking action is triggered after the contact stress is reduced to the preset zero force condition and is stably maintained, thus eliminating the docking impact force and assembly stress caused by posture deviation from the root. Compared with the rigid position servo docking method, it significantly reduces the risk of stress damage to the object and robot end during the connection process; (2) By introducing the interlocking mechanism of master-slave identity authentication and force sensor status verification in the weak current communication stage, the drive circuit of the strong current and mechanical latch is physically isolated before the system confirms that all nodes are legal and the sensor link is normal. This effectively avoids the risk of control link failure and malfunction caused by sensor failure or unauthorized node access, and improves the operational safety of the system in complex industrial network environments; (3) By constructing a decoupled six-degree-of-freedom admittance model and adjusting the virtual stiffness and virtual damping online in the direction driven by force amplitude and force change rate, the robotic arm exhibits high compliance in the direction of large contact force to avoid force confrontation, and increases compliance in the direction of rapid force release. Damping is used to suppress overshoot, and when approaching zero force, it switches to a fine-tuning mode with low stiffness and high damping, achieving smooth compliance from initial contact to zero force convergence, avoiding repeated collisions and convergence delays near zero force; (4) By mapping the distributed six-dimensional force sensing data of each robot end to the global coordinate system through the connection topology matrix, and using the extended Kalman filter to fuse the force and motion data with rigid body dynamics as constraints, the centroid position and inertial parameters of the virtual assembly are identified online, enabling the multi-robot system to perceive the overall dynamic state in real time like a single controlled body, overcoming the shortcomings of traditional methods that rely on offline models and cannot adapt to online parameter changes; (5) By decomposing the desired end force into motion force components and internal force components located in the null space of the grasping matrix, and using the minimization of the weighted square sum of torques of each robot joint as the objective, the internal force decoupling and load balance distribution are achieved while meeting the desired motion requirements of the assembly, actively suppressing the internal stress fluctuations of the assembly caused by single-arm disturbance or external force, improving the motion stability of collaborative handling and the load balance of each robot joint. Attached Figure Description

[0018] Figure 1 This is a schematic diagram illustrating the principle of the method of the present invention. Detailed Implementation

[0019] The present invention will now be described in further detail with reference to the accompanying drawings, so that those skilled in the art can implement it based on the description.

[0020] like Figure 1As shown, this invention provides a stress-free docking and distributed force control method for multi-robot collaborative operation, applied to a system consisting of a master robot and multiple slave robots. Each robot's end effector is equipped with a standardized interface that can be interconnected via mechanical latches. The interface integrates low-voltage communication contacts, a six-dimensional force sensor, and an electromagnetic latch drive mechanism. Before the collaborative transport operation begins, the robot end effectors must first complete physical docking to form a rigid assembly, and then enter the collaborative transport phase. The entire process is coordinated by the master robot, which achieves stress-free docking and optimized force distribution through distributed control. The robot system includes a master robot and slave robots connected via mechanical latches, and the method includes the following steps: S1: The main robot acquires images of visual markers on its end effector via its vision system, calculates the relative pose deviation, and drives the end effector to move until the low-voltage communication interface on the end effector is connected, establishing a low-voltage communication link. The main robot uses its onboard vision system to acquire images of visual markers on its end effector. Each visual marker contains at least three non-collinear feature points. A multi-point perspective imaging algorithm is used to calculate the six-degree-of-freedom pose deviation of the end effector relative to the main robot's end effector. This pose deviation is converted to the main robot's base coordinate system and used as the input for coarse alignment motion control. The main robot drives the end effector to move based on this deviation. To reduce docking impact, a speed grading strategy is introduced in the control: when the calculated remaining distance is greater than a preset distance threshold, the end effector moves at a higher speed; when the remaining distance is less than or equal to the distance threshold, it switches to a lower speed to approach. The distance threshold can be set based on the travel of the elastic contact points of the mechanical interface, and the ratio of high to low speeds is typically 5-10 times. The end effector movement continues until the low-voltage communication contacts on the interface surface make contact, establishing a low-voltage circuit. At this point, by detecting the jump in current or voltage in the weak current circuit, the system immediately stops the end movement and establishes a weak current communication link, which is based on low voltage differential signaling.

[0021] S2: Master-slave authentication and force sensor status verification are performed via the low-voltage communication link. If authentication and verification pass, a handshake success flag is set as a compliant search enable signal. If they fail, mechanical latch closure and high-voltage connection are prohibited. After the low-voltage communication link is established, the system enforces a secure handshake sequence before physical connection. The master robot initiates challenge-response authentication via this link, for example, by sending a one-time random number. Each slave robot encrypts the number using a preset shared key and returns it. The master robot decrypts and verifies the number, thus preventing replay attacks and unauthorized node access. Simultaneously, the master robot collects self-test data from the six-dimensional force sensors of each slave robot, including zero-point offset, internal temperature, and bit error rate of the low-voltage communication link, and compares it with preset allowable ranges. If authentication is successful and all force sensor parameters are within the normal range, the main robot sets a handshake success flag, which serves as an enable signal for the compliant search phase. If any step fails, the main robot broadcasts a lock prohibition command, locking the electromagnetic latch's drive circuit in the open state and cutting off the high-voltage control circuit. At the same time, it records the abnormal event log and triggers an alarm, thereby blocking faulty or unauthorized physical connections at the source.

[0022] S3: After the handshake success flag is set, the master robot acquires six-dimensional force sensor data from its own end effector and that of each slave robot, constructs an admittance control law, and adjusts the impedance parameters of the admittance control law online based on the six-dimensional force sensor data to generate end effector pose fine-tuning amounts, driving each robot end effector to perform six-degree-of-freedom compliant motion. The master robot collects six-dimensional force sensor data from its own end effector and that of each slave robot in real time. Each dimension of the data contains three force components and three torque components. The admittance control law is constructed based on this data. Formally, this control law can be a decoupled second-order mass-spring-damped dynamics model, mapping the contact force to a correction amount for the end effector pose. The impedance parameters in the admittance control law, namely the virtual inertia, virtual damping, and virtual stiffness matrix, are not fixed values ​​but are dynamically adjusted online based on real-time force sensor data. One adjustment strategy involves low-pass filtering the force sensor data to extract steady-state force components. Then, using the force amplitude along each coordinate axis as the independent variable, a virtual stiffness adjustment coefficient is generated through a monotonically decreasing function. This results in lower stiffness in directions with greater force, encouraging the end effector to move in the direction of stress. Simultaneously, the rate of force change is monitored. When a rapidly decreasing force occurs in a certain direction and its amplitude exceeds the overshoot suppression threshold, the virtual damping in that direction is appropriately increased to suppress potential velocity overshoot and oscillations. The pose fine-tuning calculated by the admittance control law is converted into joint commands via inverse kinematics, driving each robot end effector to perform compliant micro-motions in a six-degree-of-freedom space. This process constitutes a closed-loop force control search, with the contact force gradually decreasing and converging to near zero.

[0023] S4: When the six-dimensional force sensor data indicates that the force in each direction meets the preset zero-force condition and remains stable for a preset duration, the main robot sends a mechanical latch locking enable signal, driving the electromagnetic latch to close, thus rigidly connecting the multiple robot end effectors to form a combined unit. During the compliant search process, the system continuously judges whether the force in each direction meets the preset zero-force condition. This condition involves setting independent force and torque thresholds for each channel of the six-dimensional force sensor. When the absolute values ​​of all channels are simultaneously lower than the corresponding thresholds, a zero-force state is considered to have been reached. To avoid misjudgments caused by instantaneous sensor noise or random vibration, this zero-force state must be maintained continuously for more than the preset stable duration. During the timing process, a fault-tolerant mechanism can be introduced, allowing individual channels to have very short-term, slight deviations without immediately resetting the timer, to adapt to non-ideal working conditions. Once the continuous stable condition is met, the main controller sends a mechanical latch locking enable signal, driving the electromagnetic latch to close in a state of almost no stress, rigidly connecting the robot end effectors into a single unit. At this point, the admittance controller stops operating and latches the finally adjusted impedance parameters as the initial reference value for impedance control in the subsequent handling stage.

[0024] S5: After locking is complete, the main robot generates a connection topology matrix based on the electrical connectivity status of each interface. Using an extended Kalman filter, it fuses the six-dimensional force sensor data and end-effector motion states of each robot's end effector based on the connection topology matrix, identifying the centroid position and inertial parameters of the virtual assembly online. The connection topology matrix uses binary logic to represent whether there is a physical connection between any two end effectors, clarifying the spatial adjacency relationships of each robot within the assembly. The six-dimensional force sensor data of each robot's end effector is transformed to a global coordinate system with the estimated centroid as the origin using a force transformation matrix. The force transformation matrix integrates the current end-effector pose and topology matrix information, achieving spatial synchronization of distributed force perception. Based on this, an extended Kalman filter is constructed with the centroid position, velocity, and inertial parameters of the virtual assembly as state variables. During filter initialization, the geometric center or estimated centroid of the assembly is used as the initial position value, the theoretically designed inertial parameters are used as the initial inertial value, and an appropriate covariance matrix is ​​set. Within each filtering cycle, the state is predicted using the acceleration command of the desired motion trajectory. The resultant force and torque in the global coordinate system, along with the end-effector motion synthesized from the encoders of each robot joint using forward kinematics, are used as joint observations. Through recursive correction, the real-time center of mass position and inertial parameters of the virtual assembly are identified online. This filter can still provide accurate dynamic estimates even when the center of mass of the transported object is unknown or changes.

[0025] S6: Based on the identified center of mass position and inertial parameters, and under the constraint of satisfying the resultant force and resultant torque required for the desired motion of the assembly, the desired end-effector force compensation of each robot is calculated with the torque distribution of each robot joint as the optimization objective. This desired end-effector force compensation is then sent to the local force-position hybrid controller of each robot for execution. In the collaborative transport stage, based on the center of mass position and inertial parameters obtained online through the extended Kalman filter, the rigid body dynamics equations of the virtual assembly are constructed, linking the desired end-effector forces of each robot with the resultant force and resultant torque at the center of mass of the assembly. To address the problem of multiple solutions in force distribution caused by redundant drives, the desired end-effector force is decomposed into motion force components and internal force components: the motion force components synthesize the resultant force and resultant torque required for the desired motion, while the internal force components reside in the zero space of the end-effector contact force spinor, with their resultant force and resultant torque being zero, only adjusting the internal clamping stress distribution. The optimization objective is to minimize the weighted sum of squares of the joint torques of each robot, with the weights set according to the reciprocal of the joint's rated torque, ensuring a balanced load distribution based on capacity. In addition to satisfying the equality constraints of resultant force and resultant moment, the constraints also include the friction cone inequality constraint to ensure no slippage at the contact surface between the end effector and the object. A quadratic programming solver is used to solve this constrained optimization problem online, calculating the optimal desired end effector force compensation for each robot and sending it to the corresponding local force-position hybrid controller for execution. The force-position hybrid controller adjusts the joint torques accordingly, enabling each robot to drive the assembly to complete the desired motion while maintaining precise force distribution. If, during the optimization process, a robot's output force exceeds its end effector force capability range, the physical feasibility of the force commands can be ensured by iteratively adjusting the weights or proportionally scaling the overall acceleration.

[0026] This method employs a compliant search mechanism combining visual guidance and adaptive admittance force control, enabling mechanical latches to close under near-zero stress conditions. This fundamentally eliminates docking impact and assembly stress caused by pose errors, significantly reducing the risk of damage to objects and robots and improving the safety margin of connection operations. Through distributed force fusion and extended Kalman filtering, the method identifies the centroid and inertial parameters of the virtual assembly online, allowing multi-robot systems to perceive and respond to the overall dynamic state like a single controlled object. This effectively overcomes the clamping instability and internal force resistance problems caused by the lack of internal force decoupling in traditional methods, significantly improving the robustness of the handling process to load changes and external disturbances. The force distribution strategy based on optimization solutions balances the load on each robot joint while meeting motion requirements, avoiding single-arm overload and actively suppressing assembly vibration caused by single-arm shaking or external interference, resulting in a qualitative improvement in the stability of collaborative handling. This method is particularly suitable for large-scale flexible assembly and handling scenarios involving large-size, weakly rigid, or high-precision objects.

[0027] In another technical solution, step S1 includes: The vision system acquires images of at least three visual markers set at the end of the robot, uses a multi-point perspective imaging algorithm to calculate the relative pose deviation of the end of the robot relative to the end of the main robot, and converts the relative pose deviation to the base coordinate system of the main robot. During the motion of the robot's end effector, a filter is used to smooth the estimated sequence of relative pose deviations to generate continuous motion control commands. Simultaneously, the remaining distance between the end effector and the target is calculated in real time based on the relative pose deviation. When the remaining distance is greater than a preset first distance threshold, the end effector is controlled to move at a first speed. When the remaining distance is less than or equal to the first distance threshold, the end effector is controlled to move at a second speed, which is lower than the first speed. When the low-voltage circuit inside the low-voltage communication interface is detected to be connected, the low-voltage communication interface is determined to be connected, the end effector motion is immediately stopped, and a low-voltage communication link is established.

[0028] Visual markers refer to at least three optical feature points pre-fixed around the periphery of the robot's end effector interface. These markers can be made of passive reflective materials or active light-emitting elements, and their three-dimensional coordinates in space have been recorded in the main robot control system through offline calibration. The multi-point perspective imaging algorithm, or PnP algorithm, calculates the three-dimensional rotation matrix and translation vector of the robot's end effector coordinate system relative to the main robot's visual coordinate system by solving the mapping relationship between the projection of the markers on the two-dimensional image plane and the known three-dimensional spatial coordinates. These two elements together constitute a six-degree-of-freedom relative pose deviation. This deviation is then transformed to the main robot's base coordinate system via a transformation matrix obtained from hand-eye calibration, serving as a reference input for motion control. A filter performs temporal smoothing on the visual pose estimation sequence, using a Kalman filter or moving average filter to suppress image noise and jitter in the estimated values ​​caused by illumination fluctuations, resulting in smoother continuous motion control commands. Speed ​​graded control is a key mechanism for reducing docking impact: the first distance threshold is set based on the compression stroke of the elastic contact, for example, 30mm as a reference value; when the remaining distance exceeds this threshold, the end effector moves at a first speed v. high Movement, v high The speed can be selected within the range of 50mm / s-100mm / s; when the remaining distance is less than or equal to this threshold, it switches to the second speed v. low Movement, v lowThe speed can be selected within the range of 5mm / s-10mm / s, with a ratio of 5-10. The weak current communication interface contains a weak current circuit, consisting of a pair of elastic contacts and a weak current power supply. When the two contacts physically contact, the circuit is connected. The main robot detects a signal in the circuit where the current jumps from zero to a preset value or the voltage is pulled low, thus determining that the weak current communication interface is connected. It then immediately triggers a motion stop command and establishes a weak current communication link using the electrical path formed by the connection. In the complete docking process, from long-distance visual coarse alignment to close-range low-speed approach and then contact connection, the PnP algorithm first provides pose deviation to drive the robot's movement; a filter smooths the movement command to suppress jitter; a speed grading strategy reduces the approach speed to a low level within the close-range range to prevent hard collisions; and finally, the weak current circuit detection achieves a rapid stop-on-contact response, thus reliably establishing a communication link without impacting the mechanical interface.

[0029] This solution combines visual pose closed-loop and graded deceleration to maintain high motion efficiency at long distances, reducing kinetic energy to an extremely low level at the moment of contact with the elastic contact point, significantly mitigating the risk of contact point bounce and hard collisions. The PnP algorithm combined with filtering smoothing effectively suppresses visual noise disturbances to motion control stability, resulting in a smoother approach trajectory. The weak current circuit continuity detection method allows the system to capture the event and stop motion within the same sampling period of physical contact, with a response speed significantly superior to indirect detection methods that rely on threshold judgments from position or force sensors. This fundamentally avoids continuous pushing and pose deviations caused by communication establishment lag.

[0030] In another technical solution, step S2 includes: Through a low-voltage communication link, the master robot sends a challenge message containing a dynamic random number to each slave robot. Each slave robot encrypts the dynamic random number using a preset shared key, generates a response message, and sends it back. The master robot decrypts and verifies the response message to complete the master-slave authentication. During the identity authentication process, the force sensor status verification is performed simultaneously. The force sensor status verification includes: obtaining the zero-point offset value, working temperature parameter, and bit error rate of the weak current communication link of each six-dimensional force sensor from the robot end. If the zero-point offset value exceeds the preset offset tolerance, or the working temperature parameter exceeds the preset working temperature range, or the bit error rate exceeds the preset bit error rate threshold, the force sensor status is determined to be abnormal. When authentication is successful and all force sensors are functioning normally, the main robot sets a handshake success flag as a compliant search enable signal. When authentication fails or any force sensor is abnormal, the main robot broadcasts a lock prohibition command through the low-voltage communication link, locking the mechanical latch's drive circuit in the open state and cutting off the high-voltage connection control circuit. At the same time, the main robot records abnormal event information including the failure type and timestamp, and triggers an alarm indication.

[0031] After the weak current communication link is established, the master robot first performs challenge-response authentication based on dynamic random numbers. The dynamic random number is an unpredictable string of binary data generated by the master robot in each authentication session; its length can be selected as 128 bits or 256 bits. The pre-set shared key is a set of private keys in a symmetric or asymmetric key pair that are burned into the secure storage areas of the master robot and each slave robot via a secure channel before system deployment. After receiving the challenge message, each slave robot uses the pre-set shared key to perform encryption on the dynamic random number. The encryption algorithm can be AES-256-GCM or SM4, etc., generating a response message containing ciphertext and an authentication tag to send back. The master robot uses the corresponding key to decrypt and verify the integrity. If the decrypted random number matches the original sent number, the authentication is successful. This mechanism prevents replay attacks, ensuring that even if an attacker steals data on the weak current communication link, they cannot impersonate the user in subsequent sessions. Simultaneously with identity authentication, the force sensor status is verified. The verification parameters include three parts: zero-point offset value (the output value of each channel of the six-dimensional force sensor in no-load condition, obtained by the difference between the offline calibrated reference zero point and the currently acquired value; the preset offset tolerance is typically 0.5%-2% of the sensor's full scale, for example, a force sensor with a full scale of 500N can have a tolerance of 5N); operating temperature parameter, obtained through the sensor's built-in thermistor; the preset operating temperature range is set according to the sensor datasheet; and bit error rate (BER), which refers to the ratio of errors occurring per unit time or per bit in a weak current communication link, calculated in real-time by counting the number of error frames using cyclic redundancy check (CRC) codes. The preset BER threshold can be set to 10. -6When all verification items are within the normal range and authentication is successful, the main robot sets a handshake success flag. This flag is a Boolean variable stored in the shared memory area of ​​the main robot controller, which is read by the compliant search algorithm before startup, serving as an algorithm-level interlock. If authentication fails or any sensor malfunctions, the main robot broadcasts a locking prohibition command via a low-voltage communication link. First, it pulls the enable signal in the mechanical latch drive circuit low and latches this low-level state to prevent the electromagnetic latch from malfunctioning. Simultaneously, it cuts off the coil drive signal of the relay in the high-voltage connection control circuit, keeping the high-voltage interface physically disconnected. In addition, the main robot records abnormal event information containing the failure type and UTC timestamp in its local non-volatile memory and triggers alarm indications through the audible and visual alarm on the control cabinet or the network message interface, allowing operators to troubleshoot promptly. This secure handshake sequence binds identity verification, sensor health self-check, and physical connection permissions into an indivisible atomic decision process. It ensures that any high-energy-level physical connection is prohibited at the hardware level before the system is certain that all nodes are trustworthy and the sensors are functioning properly, thus forming a defense-in-depth security architecture.

[0032] Compared to conventional security mechanisms that rely solely on electrical interlocks or single sensor status assessments, challenge-response authentication effectively defends against identity spoofing and replay attacks after weak current communication links are eavesdropped, ensuring secure network access for multi-robot systems in untrusted industrial network environments. By incorporating force sensor zero-point offset, temperature drift, and communication link error rate into the handshake judgment criteria, the system can terminate the docking process at the nascent stage of potential faults such as sensor accuracy degradation, exceeding environmental temperature limits, or communication quality deterioration, avoiding the introduction of abnormal sensor data into the subsequent force control closed loop and fundamentally eliminating force control divergence or locking misjudgment caused by sensor failure. The locking prohibition command simultaneously locks the mechanical latch and the high-voltage interface at the hardware level, achieving dual electrical and mechanical isolation. Even if the main controller software experiences logical anomalies, the electromagnetic latch will not close unexpectedly, significantly improving the intrinsic safety level in human-robot collaboration scenarios.

[0033] In another technical solution, step S3 includes: An admittance control law is constructed using a decoupled six-degree-of-freedom second-order admittance model. The impedance parameters of the admittance control law include a virtual inertia matrix, a virtual damping matrix, and a virtual stiffness matrix. Online adjustment of the parameters includes: The acquired six-dimensional force sensor data is low-pass filtered to obtain filtered force vectors and torque vectors. Based on the filtered force vectors and torque vectors, the magnitude of the force component in each coordinate axis direction is calculated. The magnitude of the force component is input into a preset monotonically decreasing function to generate a corresponding virtual stiffness adjustment coefficient. The virtual stiffness adjustment coefficient is then multiplied by the diagonal element in the virtual stiffness matrix corresponding to that coordinate axis, so that the virtual stiffness is lower in the direction with greater force. At the same time, the rate of change of the filtered force vector is calculated. For coordinate axes with a negative rate of change and a force component magnitude greater than a preset overshoot suppression threshold, the damping coefficient in the virtual damping matrix corresponding to that coordinate axis is increased. Substitute the adjusted virtual damping matrix and virtual stiffness matrix into the admittance control law to solve for the end-effector pose fine-tuning amount, so as to drive each robot end to perform six-degree-of-freedom compliant motion until the force components in each direction converge to the range allowed by the preset zero force condition.

[0034] The admittance control law is an outer-loop control strategy that maps the contact force to the desired end-effector motion. Its core is a virtual second-order mass-spring-damped dynamic system. This scheme adopts a decoupled six-degree-of-freedom form, establishing independent admittance relationships in the three translational and three rotational directions in Cartesian space. The admittance model for each degree of freedom can be expressed as:

[0035] in, For the degree of freedom index, m i Let b be the virtual inertia of the i-th degree of freedom. i k is the virtual damping coefficient. i x is the virtual stiffness coefficient. i f is the desired position correction for this degree of freedom. i This refers to the contact force or torque in the corresponding direction. The force *f* represents the three translational degrees of freedom. i Take the force components, in N; f for the three rotational degrees of freedom i The torque component is taken, with units of Nm. The parameters of the six degrees of freedom are arranged to form a virtual inertia matrix M, a virtual damping matrix B, and a virtual stiffness matrix K, all of which are 6×6 diagonal matrices. The first step in online adjustment is to perform low-pass filtering on the original six-dimensional force sensor data. The filter cutoff frequency can be selected based on the sensor noise characteristics and the natural frequency of the mechanical structure, with a reference range of 5Hz-20Hz. After filtering, the force vector f, with high-frequency noise removed, is obtained. filt With the torque vector τ filt Low-pass filtering can use a first-order or second-order Butterworth filter to ensure passband flatness and stopband attenuation characteristics, and to avoid excessive filter phase lag affecting control response speed.

[0036] The core strategy for online adjustment is adaptive stiffness amplitude and overshoot suppression of damping. For each coordinate axis direction, the force component amplitude is extracted from the filtered force vector and moment vector. .Will Input a preset monotonically decreasing function, which maps the force amplitude to a virtual stiffness adjustment coefficient. The value ranges from 0 to 1. An alternative function form is a linear saturated function: when... Less than the lower threshold f low hour, Maintain original stiffness; when Greater than the upper limit threshold f high At that time, α i =α min Take the lowest adjustment factor; when When it is in between, α i Decreasing linearly from 1 to α min As an example, where f low It can be set to 5N, f high It can be set to 30N, α min It can be set to 0.1. Then use α. i Multiply by the diagonal element k of the corresponding degree of freedom in the virtual stiffness matrix i Updated to This mechanism results in lower virtual stiffness in directions with greater force, meaning the end effector behaves more compliantly in that direction, tending to follow the direction of the external force to release contact stress. Simultaneously, the rate of change of the filtered force components is monitored. This is calculated using a difference approximation. For a negative rate of change, meaning the force is decreasing, and the current force component amplitude... For coordinate axes where the overshoot suppression threshold is greater than the preset threshold, the damping coefficient in that direction is increased according to the following rules: ,in The damping amplification factor is determined based on the rate of force reduction; the faster the force decreases, the more significant the amplification. The overshoot suppression threshold can be set to 10N, slightly higher than the force threshold under zero-force conditions, to suppress velocity overshoot before the force rapidly decays and crosses the zero-force zone. This damping amplification mechanism can inject additional energy dissipation in the direction of rapid force release, preventing the end effector from generating a reverse collision force due to inertia when crossing the zero-force position. The virtual damping matrix will be updated in real time. and virtual stiffness matrix Substituting the admittance control law, the six-degree-of-freedom end-effector pose fine-tuning is solved in each control cycle. The inverse kinematics is converted into joint angle increments and then sent to the servo actuators to drive each robot end effector to perform compliant motion. This process is a closed-loop iterative process, in which the force components in each direction gradually decay under the guidance of adaptive impedance and converge to the range allowed by the zero-force condition.

[0037] This solution employs a mechanism where stiffness adaptively decreases with force amplitude, enabling the robotic arm to exhibit high compliance under large contact forces. This avoids sensor overload or mechanical deformation caused by strong force resistance. When the contact force decreases, it can restore a certain degree of stiffness to ensure positioning accuracy, achieving a dynamic balance between compliance and positioning stiffness. The damping overshoot suppression mechanism effectively eliminates velocity overshoot and residual oscillations during the rapid release of force, solving the problem that single stiffness adjustment is prone to repeated collisions due to inertia when the force crosses the zero force zone, making the force convergence process more stable and efficient.

[0038] In another technical solution, when the force component amplitudes in each coordinate axis direction are all lower than a preset mode switching threshold, and the mode switching threshold is less than a preset overshoot suppression threshold, the online adjustment also includes: Set all diagonal elements of the virtual stiffness matrix to the preset minimum stiffness value and set all diagonal elements of the virtual damping matrix to the preset maximum damping value, so that the robot end effector enters the isotropic fine compliant attitude adjustment mode; in the fine compliant attitude adjustment mode, the end effector pose fine adjustment amount solved by the admittance control law is limited, and the limiting threshold is set according to the allowable assembly gap of the mechanical latch. The system monitors six-dimensional force sensor data in real time. If the amplitude fluctuation of the force component in any direction exceeds the preset fluctuation tolerance, the corresponding virtual damping coefficient is temporarily increased according to the fluctuation amplitude in that direction until the force component fluctuation in that direction returns to within the fluctuation tolerance. Then, the virtual damping coefficient is restored to the preset maximum damping value.

[0039] When the force component amplitude in each coordinate axis direction When all contact forces decrease to below the preset mode switching threshold, the system determines that it has entered the fine-tuning stage, which is close to zero force, from the initial high contact force search stage. The mode switching threshold is less than the overshoot suppression threshold, and its value can be set in the range of 2-5N, depending on the sensor noise level, to ensure that the contact force has been sufficiently reduced and the sensor signal-to-noise ratio still meets the fine-tuning control requirements when the switching occurs. After the switching is triggered, the online adjustment strategy enters the fine-tuning and compliant attitude adjustment mode. In this mode, all diagonal elements of the virtual stiffness matrix are uniformly set to the preset minimum stiffness value, which should be lower than the lower limit of stiffness in the normal search stage, and can be set to 50N / m-200N / m; all diagonal elements of the virtual damping matrix are uniformly set to the preset maximum damping value, which should be higher than the upper limit of damping in the normal search stage, and can be set to 500Ns / m-2000Ns / m. The combination of high damping and low stiffness enables the end to exhibit isotropic critical damping or overdamping characteristics, which can produce slow and stable compliant displacement in the face of small contact force disturbances in any direction, avoiding the micro-amplitude oscillations that may occur in the underdamped state, and approaching the final zero force posture at an extremely low speed.

[0040] In the fine-grained compliant attitude adjustment mode, the end-effector pose fine-tuning amount obtained from the admittance control law is limited. The limiting threshold is set based on the allowable assembly clearance of the mechanical latch, which refers to the maximum allowable radial and axial fit deviation of the latch during closure, typically between 0.5mm and 3mm. The selected value of the limiting threshold should be slightly smaller than this clearance to prevent the end-effector pose adjustment amplitude from exceeding the latch tolerance during zero-force search, thus preventing jamming or damage. The real-time monitoring stage continuously analyzes the data from the six-dimensional force sensor. If the amplitude fluctuation of the force component in any direction exceeds the preset fluctuation tolerance, it is determined that there is a force impact caused by external disturbance or mechanical clearance during the pose adjustment process. The fluctuation tolerance can be set to 30%-50% of the force threshold in that direction. For example, when the force threshold is 3N, the fluctuation tolerance is 1N. In response, the system temporarily increases the virtual damping coefficient in that direction, with the increase proportional to the fluctuation amplitude. This further reduces the response speed to force disturbances in that direction until the force component fluctuation returns to within the fluctuation tolerance. Then, the virtual damping coefficient is restored to the preset maximum damping value. This dynamic damping enhancement mechanism, as a supplementary means of anti-disturbance in fine-tuning mode, enhances the ability to suppress external transient disturbances without changing the compliance level. By employing a phase division based on force amplitude, the system automatically switches to a fine and compliant attitude adjustment mode with high damping and low stiffness when approaching zero force. This effectively overcomes the problems of weak force signals, sluggish motion response, or over-adjustment caused by sensor resolution and mechanical friction near zero force in a single impedance strategy, achieving ultra-low-speed, oscillatory convergence to the zero force position. Limiting the attitude fine-tuning ensures that the attitude adjustment trajectory is always within the allowable assembly gap envelope of the mechanical latch, eliminating the risk of mechanical interference at the interface due to over-adjustment. The wave-triggered damping adaptive enhancement provides targeted suppression of external transient force interference while maintaining attitude adjustment compliance, ensuring the anti-disturbance robustness of the fine attitude adjustment process.

[0041] In another technical solution, step S4 includes: The preset zero-force condition includes force thresholds and torque thresholds set for each channel of the six-dimensional force sensor. When the absolute value of the force component of each channel is lower than the corresponding force threshold and the absolute value of the torque component of each channel is lower than the corresponding torque threshold, it is determined that the force in each direction meets the preset zero-force condition. A timer is set to count the duration for which the preset zero-force condition is continuously met. The timer is reset when the absolute value of the force component or the absolute value of the torque component in any channel exceeds the corresponding threshold. During the timer's counting process, a fault-tolerant counter is set. If the force component or torque component momentarily exceeds the tolerance within a single sampling period, but the magnitude of the exceedance is lower than the preset tolerance margin, and the number of channels exceeding the tolerance does not exceed the preset upper limit of the number of channels, the fault-tolerant counter is incremented without resetting the timer. The timer is reset and the fault-tolerant counter is cleared only when the accumulated value of the fault-tolerant counter reaches the preset upper limit of the number of tolerances. When the timer reaches the preset stable duration, the main robot sends a mechanical latch locking enable signal to drive the electromagnetic latch to close. At the same time, it stops the operation of the admittance control law and latches the final impedance parameter adjusted online by the admittance control law as the initial impedance reference value for the collaborative transport stage of the combined body.

[0042] A six-dimensional force sensor typically outputs force components in three orthogonal directions. and torque components about the three axes The six channels have different physical meanings and ranges, therefore the threshold settings must be configured independently for each channel. The force threshold can be selected based on 1%-3% of the full scale of each sensor channel. For example, for a force channel with a 2000N range, the force threshold can be set to 20N-60N. The 2000N range is commonly used in medium-load industrial robots. The torque threshold can be calculated by referring to the product of the force threshold and the end effector lever arm. Taking a 100Nm range as an example, the torque threshold can be set to 1Nm-3Nm. The determination logic is: if and only if... and When all conditions are met simultaneously, the forces in each direction are determined to satisfy the preset zero-force condition. to There are three force channel thresholds. to There are three torque channel thresholds. This independent threshold design for each channel can accommodate differences in the geometric constraints of the interface in different directions. For example, the axial direction may allow for larger force deviations due to the longer travel of the elastic contact, while the radial direction requires more stringent force thresholds due to the smaller fit clearance of the locking guide groove.

[0043] Upon determining that the zero-force condition is met, locking is not immediately triggered; instead, a continuous stabilization time verification is performed. A timer is set up, which starts accumulating the sampling period from zero when the zero-force condition is first met. The sampling period is consistent with the data update rate of the force sensor. During the timing process, if the absolute value of the force component or torque component of any channel exceeds the corresponding threshold, the timer is reset to zero and the timing restarts. Simultaneously, a fault-tolerant counter is added to handle instantaneous spike interference. In industrial environments, sensor signals may experience brief deviations within a single sampling period due to electrical noise, mechanical micro-vibration, or electromagnetic interference. Frequent timer resets due to this would make it difficult to accumulate stabilization time. The fault-tolerant counter mechanism is as follows: if a force component or torque component experiences a momentary deviation within a single sampling period, but the deviation amplitude is lower than the preset fault tolerance margin... And the number of channels with out-of-tolerance conditions does not exceed the preset upper limit N of the number of channels. max If the error occurs, increment the fault-tolerant counter without resetting the timer. Fault tolerance margin. It can be set to 10%-20% of the corresponding channel threshold, for example, when the force threshold is 40N, the fault tolerance margin is 4N-8N; the upper limit of the number of channels N maxIt can be set to 1-2, allowing only a few channels to experience slight deviations simultaneously. When the accumulated value of the fault tolerance counter reaches the preset maximum number of fault tolerance attempts C... max When this occurs, it indicates that the frequent short-term spikes are no longer accidental interference. At this point, the timer should be reset and the fault-tolerant counter cleared. C max The timer can be set according to statistical significance, for example, 3-5 times, to achieve a balance between false positive suppression and true anomaly detection. When the timer value reaches the preset stable duration (generally 0.5s-1.5s), the main robot sends a mechanical latch locking enable signal. This signal is a digital logic level signal that directly drives the coil drive circuit of the electromagnetic latch, causing the latch to perform a closing action. Simultaneously with sending the locking enable signal, the main robot stops running the admittance control law, stops sending pose fine-tuning commands to other robots, and displays the virtual damping matrix B obtained during the online adjustment of the admittance control law. final and virtual stiffness matrix K final It is latched into the shared memory region and used as the initial impedance reference value for the subsequent collaborative transport phase of the assembly.

[0044] This scheme considers the impact of continuous settling time, confirms the zero-force state based on statistical stability, and eliminates the interference of sensor noise peaks. The fault-tolerant counter mechanism provides a flexible range between timer reset and no action, allowing a small amount of minute instantaneous deviation without interrupting the timing, which significantly improves the robustness of judgment in harsh industrial electromagnetic environments and avoids infinite delay in settling time due to random disturbances at individual sampling points. The converged impedance parameters are latched and reused in the transportation stage, eliminating the step jump of impedance parameters between the two stages of compliant search and cooperative transportation, so that the robot has the same force control characteristics as the docking end state at the moment of transportation start, avoiding force shocks during stage switching.

[0045] In another technical solution, step S5 includes: The main robot detects the electrical connectivity of the weak current communication links in each interface, records the interface connection as the first logic value, and records the interface disconnection as the second logic value, and generates a connection topology matrix, which represents the connection relationship between each robot end effector. The six-dimensional force sensor data of each robot end effector are transformed into a global coordinate system with the estimated center of mass of the virtual assembly as the origin through a force transformation matrix. The force transformation matrix is ​​jointly determined by the current pose of each robot end effector and the connection topology matrix. The extended Kalman filter is initialized, with the estimated center of mass position as the initial value of the center of mass position, the theoretical inertial parameters of the virtual assembly as the initial values ​​of the inertial parameters, and the initial state covariance matrix is ​​set. Within each filtering cycle, a state prediction model is constructed based on the acceleration signal of the desired motion trajectory of the combined body, the state prediction value is calculated, and the resultant force and torque of the six-dimensional force sensor after being transformed to the global coordinate system by the force transformation matrix, as well as the synthetic motion state of each robot end effector obtained by the joint encoder and the forward kinematic solution, are used as joint observations to correct the state prediction value. The online identification results of the centroid position and inertial parameters of the virtual combined body are then output.

[0046] After the main robot completes the mechanical locking, it checks the electrical connectivity of the low-voltage communication links in each interface one by one. This can be achieved through physical layer signal detection of the low-voltage communication links, such as reading the terminal matching resistance value of the low-voltage differential signal lines of each interface or sending a link test frame and detecting the response frame. The interface connectivity status is recorded as the first logical value, such as binary 1, and the interface disconnection status is recorded as the second logical value, such as binary 0, thereby generating a connection topology matrix. , where N is the total number of robots participating in the collaboration. Matrix elements This indicates that there is a physical connection between the end effectors of robot i and robot j. This indicates that there is no connection between the two elements; diagonal element. This matrix is ​​an undirected symmetric matrix that fully represents the connection relationship diagram structure between the robot ends.

[0047] The force transformation matrix uniformly transforms the six-dimensional force sensor data measured in the end-effector coordinate systems of each robot to a global coordinate system with the estimated center of mass of the virtual assembly as the origin. Let the homogeneous transformation matrix of the robot j end-effector coordinate system relative to the global coordinate system be denoted as... The transformation matrix is ​​calculated in real time from the forward kinematics of robot j, and includes the rotation matrix. Translation vector The force vector f measured at the end effector of robot j j and torque vector τ j The expression in the global coordinate system is: However, the conversion of torque must also take into account the additional torque generated by the force translation, that is... ,in This provides the estimated centroid position of the virtual assembly in the global coordinate system. The connectivity topology matrix C provides the filtering criteria, only including... The associated robot data is incorporated into the fusion. When initializing the extended Kalman filter, the estimated centroid position is used as the initial value for the centroid position. The estimated center of mass can be taken as the centroid of the polygon enclosed by the geometric centers of the robot's end-effectors, or calculated offline based on the volumetric center of mass of the 3D model of the object being transported. Initial values ​​of inertial parameters. The theoretical inertial parameters of the virtual assembly are used, including the total mass m and the inertial tensor. The initial state covariance matrix P0 is synthesized from the theoretical values ​​of each robot body, end effector, and transported object through homogeneous transformation. The initial state covariance matrix P0 reflects the uncertainty of the initial estimate and can be set as a diagonal matrix. The magnitude of the diagonal elements is set according to the initial deviation range of the predicted center-of-mass position and inertial parameters; for example, the variance of the center-of-mass position can be set to (0.1m). 2 The magnitude and mass variance are set to (1 kg). 2 Magnitude.

[0048] Within each filtering cycle, the extended Kalman filter performs a two-step recursive process of state prediction and observation correction. The state prediction model utilizes the acceleration signal of the desired trajectory of the combined body. As input, it includes the centroidal acceleration and angular acceleration. State variables. The state transition equations, including the center of mass position, center of mass velocity, and inertial parameter vectors, can be established based on rigid body dynamics and the assumption of invariant inertial parameters. The prediction step outputs the predicted state values. and predict covariance matrix The observation correction step uses a joint observation z, which consists of two parts: the first part is the resultant force of the six-dimensional force sensors after transformation to the global coordinate system via a force transformation matrix. With resultant torque ,in The first part is a six-dimensional generalized force vector containing force and torque; the second part is the synthesized motion state of each robot end effector, which is obtained by taking the kinematic forward kinematics solution from the encoder readings of each robot joint to obtain the velocity and acceleration of each end effector in the global coordinate system, and then synthesizing them according to the connection topology matrix to obtain the estimated motion state at the center of mass of the combined body. The observation equation correlates the state variables with the joint observations, and uses the Kalman gain K k The predicted state values ​​are corrected to obtain the posterior estimate. It outputs the online identification results of the center of mass position and inertial parameters of the virtual assembly.

[0049] Compared to the traditional approach of processing force data from each robot end effector independently or simply summing them, this scheme establishes a spatial correlation of force transformation by connecting topological matrices. This allows for geometrically consistent spatial fusion of force sensing data distributed across various end effectsors within a unified global coordinate system, eliminating information fragmentation caused by inconsistencies in the coordinate systems of different sensors. The extended Kalman filter, using rigid body dynamics as a link, jointly fuses force and motion data, enabling online identification of the center of mass position and inertial parameters to no longer rely on precise prior models. This allows for adaptation to parameter perturbations caused by changes in the internal mass distribution of objects or external load disturbances during handling, significantly improving identification accuracy and convergence speed. The fused virtual composite perception model provides an accurate and real-time dynamic reference benchmark for subsequent force optimization and allocation.

[0050] In another technical solution, during the operation of the extended Kalman filter, residual detection is performed on the resultant force and resultant torque of the six-dimensional force sensor in the joint observation. When the residual exceeds the preset residual abnormality threshold, it is determined that the corresponding six-dimensional force sensor data has a measurement abnormality. The observation noise covariance matrix corresponding to the six-dimensional force sensor data is temporarily amplified, and the amplified observation noise covariance matrix is ​​used for state correction within the filtering cycle. When the residual exceeds the residual abnormal threshold for multiple consecutive filtering cycles, the master robot sends a force sensor re-inspection command to the corresponding slave robot through a weak current communication link. If the re-inspection result returned by the slave robot confirms that the force sensor is abnormal, the master robot removes the six-dimensional force sensor data corresponding to the slave robot from the joint observation and regenerates the connection topology matrix. At the same time, it updates the force transformation matrix according to the regenerated connection topology matrix to maintain the continuity of the online identification of the centroid position and inertial parameters of the virtual assembly.

[0051] Within each filtering cycle of the extended Kalman filter, after obtaining the posterior estimate after observation correction, residual detection is performed on the resultant force and resultant torque of the six-dimensional force sensors in the joint observation. The residual r... k Defined as actual observation z k Compared with the predicted value based on the state Predicted observations obtained by calculation using the observation equation The difference between them, i.e. ,in Let be the observation function. The physical meaning of the residuals is the portion of the observed data that cannot be explained by the current dynamic model. Under normal circumstances, the residuals should appear as a white noise sequence, and their statistical characteristics should be consistent with the preset observation noise covariance matrix R. Set the residual abnormality threshold r. thres Each component can be set by multiplying the steady-state noise standard deviation of the corresponding observation by a coefficient of 3-5, i.e., using the 3-sigma or 5-sigma criterion. When the residual r k When the absolute value of a certain component exceeds the corresponding residual anomaly threshold, the six-dimensional force sensor data associated with that component is determined to have a measurement anomaly. The sources of this anomaly may include transient electrical interference from the sensor, force signal distortion caused by localized stress concentration at the end interface, or erroneous data introduced by occasional packet loss in the communication link. For the sensor data determined to be abnormal, the diagonal elements corresponding to the sensor data in the observation noise covariance matrix R are temporarily amplified within this filtering period. The amplification factor λ can be set to a value between 10 and 100. The amplified observation noise covariance matrix... Using the magnified version By performing Kalman gain calculation and state correction, the trust weight of anomalous observation data in state updates is reduced, making the filter rely more on dynamic model predictions rather than anomalous observations, thereby achieving soft isolation of instantaneous anomalous data within a single cycle.

[0052] When the residual exceeds the residual abnormality threshold for M consecutive filtering cycles, the number of consecutive abnormal cycles M can be set to 3-5, indicating that the problem is not instantaneous interference but rather a continuous sensor failure or long-term degradation of the communication link. At this time, the master robot sends a force sensor re-inspection command to the corresponding slave robot via the weak current communication link. The re-inspection command requires the slave robot to execute the force sensor self-test program, including re-acquiring the zero-point offset, reading the current operating temperature, executing the built-in test stimulus, and verifying the response output, and then transmits the re-inspection results back to the master robot via the weak current communication link. If the re-inspection results returned by the slave robot confirm an abnormal force sensor status, the master robot performs fault isolation and topology reconstruction. The fault isolation operation completely removes the six-dimensional force sensor data corresponding to the slave robot from the joint observation, meaning its force data is no longer included in the calculation of resultant force and resultant moment. The topology reconstruction operation includes regenerating the connection topology matrix and marking the connection relationships in the rows and columns of the adjacency matrix corresponding to the slave robot as disconnected, i.e., setting the corresponding elements to the second logical value. Simultaneously, the force transformation matrix is ​​updated based on the regenerated connection topology matrix, coordinate system transformation terms related to the faulty node are removed, and the weighted calculation method for resultant force and resultant moment is adjusted. After reconstruction, the extended Kalman filter continues to operate in a reduced-order form, utilizing the force sensing data and motion state data of the remaining normal nodes to maintain online identification of the centroid position and inertial parameters of the virtual assembly, ensuring that the identification process is not interrupted by the faulty node. When the faulty node recovers, it can be incorporated into the sensing network by re-executing topology identification and filter dimension expansion operations.

[0053] The single-cycle observation noise covariance adaptive amplification mechanism adopted in this scheme provides millisecond-level soft isolation for instantaneous abnormal data, avoiding the pollution of state estimation by occasional spike data. The combination of continuous anomaly discrimination and re-inspection instructions effectively distinguishes between instantaneous interference and persistent faults, reducing the probability of unnecessary topology reconstruction triggered by occasional interference. The automatic removal of faulty nodes and the online reconstruction mechanism of the topology matrix enable the multi-robot perception network to continue degraded operation even when some sensors fail, maintaining the continuity of online identification of the centroid position and inertial parameters of the virtual assembly, avoiding the interruption of the entire collaborative handling task due to a single point of failure, and significantly improving the task reliability and robustness of the system.

[0054] In another technical solution, step S6 includes: Based on the identified center of mass position and inertial parameters of the virtual assembly, a rigid body dynamics equation is constructed for the virtual assembly. The desired end force of each robot is mapped to the resultant force and resultant torque at the center of mass of the virtual assembly through the rigid body dynamics equation. The desired end force of each robot is decomposed into kinematic force components and internal force components. The kinematic force components are used to synthesize the resultant force and resultant torque required for the desired motion of the assembly. The internal force components are located in the zero space of the spinor of the end contact force of each robot, satisfying the constraint that the sum of the internal force components is zero, and their contribution to the resultant force and resultant torque at the center of mass is zero. The optimization objective is the weighted sum of squares of the joint torques of each robot. The weight coefficient of each joint is set according to the reciprocal of the rated torque of the corresponding joint. The joint torque is obtained by mapping the desired end effector force to the joint space using the force Jacobian matrix of each robot. The constraints include the equality constraint that the motion force components satisfy the required resultant force and resultant torque, and the friction cone inequality constraint applied to the desired end effector force of each robot. The friction cone inequality constraint is used to prevent relative sliding between the end effector and the object contact surface. A quadratic programming solver is used to solve the optimization objective under the constraints and output the desired end effector force compensation amount of each robot.

[0055] In the collaborative transport phase, the virtual assembly formed by multiple robot end effectors and the object being transported, rigidly connected, moves according to the laws of rigid body dynamics. The centroid position p is obtained online based on the extended Kalman filter. CoM and inertial parameters, the inertial parameters including the total mass m of the assembly and the inertial tensor in the center-of-mass coordinate system. This allows us to establish the rigid body dynamics equations for the virtual assembly. These equations map the desired end-effector forces of each robot to the resultant force and resultant torque at the center of mass of the virtual assembly, and their expression is:

[0056] in, To achieve the desired center of mass, The net torque at the desired center of mass is given by the second derivative of the desired trajectory of the combined body, i.e., the linear acceleration. and angular acceleration Sure; and Let be the linear velocity and angular velocity of the center of mass, respectively; g be the acceleration due to gravity; I3 be the 3×3 identity matrix; × denotes the cross product of vectors. The third term on the right-hand side of the equation is the gravity compensation term, ensuring that the robot's end effector force generates the desired motion while counteracting the combined weight. In this equation, the unit of force is N, the unit of torque is Nm, the unit of mass is kg, and the unit of length is m.

[0057] After obtaining the required resultant force and torque at the center of mass, the problem of force distribution in a redundant drive system arises: multiple robot end effectors jointly grasp the same object, and there are various ways to combine the end effector forces. This solution adopts a decoupling strategy that decomposes the desired end effector force into kinematic force components and internal force components. The kinematic force components are the force components necessary to generate the resultant force and torque at the center of mass of the combined body, while the internal force components are the force components that compress or stretch the end effectors against each other but do not contribute to the motion of the center of mass. (Note: The last sentence appears to be a separate, unrelated statement about robots.) The desired end force helix in the global coordinate system is The desired end force spirals of all robots can be stacked as follows: Where N is the number of robots involved in the transport. The motion force components must satisfy the following equation constraint: ,in To capture the matrix, the resultant force and resultant torque of each end force are helically mapped to the center of mass. The desired force spiral. The internal force components lie in the null space of the grasping matrix G, i.e., satisfying... A basis of the null space can be obtained by singular value decomposition of G or by directly calculating its null space projection matrix. It is obtained that any internal force component can be expressed as ,in This is a zero-space coefficient vector. Internal force components are considered, allowing each robot end effector to adjust the distribution of clamping forces among itself while meeting motion requirements.

[0058] The optimization objective is to minimize the weighted sum of squared moments of the joint torques of each robot, and to express the desired end effector force helix H through the force Jacobian matrix of each robot. Mapping to joint space yields the joint torque vector of robot j. , where n j Let be the number of joints of robot j. For N robots, the total weighted sum of squared joint moments is... ,in Let be the weight coefficient of the i-th joint of the j-th robot. According to the rated torque of the corresponding joint The reciprocal setting, that is This allows joints with larger rated torques to bear more load, while protecting joints with smaller rated torques. Constraints include those based on the equations governing kinematic force components. In addition, it includes friction cone inequality constraints. Friction cone constraints are used to prevent relative sliding between the end and the contact surface of the object; specifically, the ratio of the tangential force to the normal force at the contact point does not exceed the static friction coefficient μ. s For each endpoint, the constraint can be linearized as follows: C fric The linearized friction cone matrix is ​​formed by the contact surface normal direction vector and the friction coefficient μ. s Confirmed, μs Typical values ​​range from 0.3 to 0.8, depending on the material of the end-contact pad; for example, 0.6 is used for rubber pads and 0.3 for direct metal contact. The quadratic programming solver solves the above-mentioned constrained optimization problem within each control cycle based on the currently identified inertial parameters, desired motion acceleration, and current robot configuration. This optimization problem is a standard convex quadratic programming problem, which can be solved online using interior-point methods or effective set methods. The control cycle is typically 1ms-10ms, meeting real-time requirements.

[0059] This solution, based on an optimized force distribution strategy, achieves weighted and balanced force distribution according to the actual load capacity of each robot joint while ensuring motion tracking accuracy. This avoids overloading of weaker joints and significantly improves the overall load margin of the multi-robot system. The decoupling of motion force and internal force allows the system to independently adjust the internal clamping force without affecting the transport motion, achieving orthogonality between motion control and internal force control. This fundamentally solves the oscillation problem caused by the coupling of motion and internal force in redundant drive systems. The friction cone constraint ensures the reliability of contact between each end and the object during transport, effectively suppressing micro-slippage of the contact surface caused by acceleration changes or external disturbances, thus qualitatively improving the gripping stability in high-speed transport and emergency braking scenarios.

[0060] In another technical solution, within each control cycle, the mass matrix and Coriolis force terms in the rigid body dynamics equations are updated based on the current inertial parameters identified online by the extended Kalman filter. The system monitors the desired end-effector force compensation for each robot. If any force or torque component of the desired end-effector force compensation exceeds the end-effector force range corresponding to the torque output limit of the robot, the joint weight coefficient of the robot is multiplied by a preset amplification factor, and the quadratic programming solver is retried until all desired end-effector force compensations are within the end-effector force range. A penalty term is added to the objective function of the quadratic programming solver to penalize the difference between the desired end-effector force compensation in the current control cycle and the previous control cycle. If the limit is still exceeded after a preset number of re-solutions, the acceleration of the desired motion of the assembly is scaled proportionally, and the scaled acceleration is re-input into the rigid body dynamics equations.

[0061] Within each control cycle, the rigid body dynamics equations upon which the optimization problem depends need to be updated based on the current inertial parameters identified online by the extended Kalman filter. Specifically, the identification results output by the extended Kalman filter include the total mass of the virtual assembly at the current moment. and inertial tensor Both update the mass matrix in the rigid body dynamics equations. and the inertial tensor matrix I C Meanwhile, the Coriolis force term The gravity term mg is also updated synchronously with the mass estimate. The online parameter update mechanism enables the dynamic model to track the slow changes in inertial parameters caused by conditions such as liquid sloshing, deformation of flexible components, or partial load shedding, ensuring that the optimization constraints remain consistent with the actual physical system in real time.

[0062] After optimization, the system performs a physical feasibility check on the solution results. Each robot joint motor and end effector has an upper limit on force output. This upper limit is determined by the rated torque of the joint motor, the reduction ratio of the reducer, and the end effector lever arm, and is converted into the end effector force range. Monitor the calculated desired end-effector force compensation for each robot. If any desired end-effector force compensation... If the force or torque components exceed the end-effector force range of the corresponding robot, a redistribution mechanism is initiated. First, the joint weight coefficient corresponding to the overloaded robot is multiplied by a preset amplification factor γ > 1. The amplification factor γ is typically in the range of 1.5-3, which increases the penalty weight of the overloaded robot in the objective function, transferring the load from the robot with saturated capacity to other robots with remaining capacity. After the weight update, the quadratic programming solver is retried for solving. This iterative redistribution process can be repeated until all desired end-effector force compensations are within the corresponding end-effector force range. If overload still exists after a preset number of re-solutions (3-5 times), it indicates that at the current desired motion acceleration level, the force output capability of some robots is insufficient to support the assembly's movement along the predetermined trajectory. In this case, the desired motion acceleration of the assembly is scaled proportionally, with a scaling factor of [missing value]. Scaled acceleration and Re-enter the rigid body dynamics equations, recalculate the desired resultant force and resultant moment, and trigger the optimization solution. Acceleration scaling slows down the motion rhythm of the assembly to adapt to the upper limit of the current force output capability of each robot, prioritizing the feasibility of force distribution and handling safety.

[0063] Simultaneously, a penalty term is added to the objective function of the quadratic programming solver to suppress step changes in force commands between adjacent control cycles. The penalty term takes the form of... H k H is the desired end-force helical vector for the current control cycle. k-1The desired end-effector force helical vector actually issued in the previous control cycle is ρ, where ρ>0 is the penalty weight coefficient. This penalty term suppresses the rate of change of the force command, introducing a first-order low-pass filter characteristic into the force command channel to ensure a smooth transition of the desired force at each robot end, avoiding joint torque shocks and mechanical vibrations caused by large jumps in the optimization solution between adjacent cycles. The selection of the penalty weight ρ needs to balance the optimality of force distribution and the smoothness of the command. A smaller value, such as 0.01-0.1, can be taken when the parameter identification converges well and the operating conditions are stable, while a larger value, such as 0.5-1.0, can be taken when the parameters change drastically or external disturbances are frequent.

[0064] This method employs a closed-loop verification mechanism involving iterative weight amplification and acceleration scaling to ensure that the force allocation results remain within the physical capabilities of each robot, avoiding the accumulation of force tracking errors and motion trajectory distortion caused by actuator saturation. Online updates of inertial parameters enable the dynamic model to adapt to the gradual changes in the parameters of the assembly during handling, maintaining the stability of the optimized allocation accuracy over long-term tasks. The force command variation penalty term allows the optimization solver to pursue optimal torque allocation while also considering the temporal smoothness of the force command, significantly reducing joint torque shocks and mechanical resonances caused by jumps in the optimization solution. This results in smoother force control output during handling and extends the service life of the robot's transmission components.

[0065] It should be noted that although the steps are described in a specific order above, this does not mean that they must be performed in that order. In fact, some of these steps can be executed concurrently, or even in a different order, as long as the required functionality is achieved. The number of devices and processing scale described herein are for simplification of the invention; applications, modifications, and variations of this invention will be readily apparent to those skilled in the art.

[0066] Although embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, other modifications can be easily made. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and illustrations shown and described herein.

Claims

1. A stress-free docking and distributed force control method for multi-robot collaborative operation, characterized in that, The robot includes a master robot and a slave robot connected by a mechanical lock, and the method includes the following steps: S1: The main robot acquires images of visual marker points at the robot's end via the vision system, calculates the relative pose deviation, and drives the robot's end to move until the low-voltage communication interface set at the end is connected, thus establishing a low-voltage communication link. S2: Perform master-slave authentication and force sensor status verification through the weak current communication link. If the authentication and verification are successful, the handshake success flag is set as the compliant search enable signal. If the handshake is unsuccessful, the mechanical lock closure and high power connection are prohibited. S3: When the handshake success flag is set, the master robot acquires the six-dimensional force sensor data of itself and each slave robot end effector, constructs the admittance control law, and adjusts the impedance parameters of the admittance control law online according to the six-dimensional force sensor data, generates the end effector pose fine-tuning amount, and drives each robot end effector to perform six-degree-of-freedom compliant motion. S4: When the data from the six-dimensional force sensor indicates that the force in each direction meets the preset zero force condition and remains stable for a preset duration, the main robot sends a mechanical latch locking enable signal to drive the electromagnetic latch to close, so that the ends of the multiple robots are rigidly connected to form a combined body. S5: After locking is completed, the main robot generates a connection topology matrix based on the electrical connectivity status of each interface, and uses an extended Kalman filter to fuse the six-dimensional force sensor data and end motion state of each robot end based on the connection topology matrix, and identifies the center of mass position and inertial parameters of the virtual assembly online. S6: Based on the identified center of mass position and inertial parameters, under the constraints of the resultant force and resultant torque required for the desired motion of the combined body, the desired end force compensation of each robot is calculated with the torque distribution of each robot joint as the optimization objective, and the desired end force compensation is sent to the local force-position hybrid controller of each robot for execution.

2. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 1, characterized in that, Step S1 includes: The vision system acquires images of at least three visual markers set at the end of the robot, uses a multi-point perspective imaging algorithm to calculate the relative pose deviation of the end of the robot relative to the end of the main robot, and converts the relative pose deviation to the base coordinate system of the main robot. During the motion of the robot's end effector, a filter is used to smooth the estimated sequence of relative pose deviations to generate continuous motion control commands. Simultaneously, the remaining distance between the end effector and the target is calculated in real time based on the relative pose deviation. When the remaining distance is greater than a preset first distance threshold, the end effector is controlled to move at a first speed. When the remaining distance is less than or equal to the first distance threshold, the end effector is controlled to move at a second speed, which is lower than the first speed. When the low-voltage circuit inside the low-voltage communication interface is detected to be connected, the low-voltage communication interface is determined to be connected, the end effector motion is immediately stopped, and a low-voltage communication link is established.

3. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 1, characterized in that, Step S2 includes: Through a low-voltage communication link, the master robot sends a challenge message containing a dynamic random number to each slave robot. Each slave robot encrypts the dynamic random number using a preset shared key, generates a response message, and sends it back. The master robot decrypts and verifies the response message to complete the master-slave authentication. During the identity authentication process, the force sensor status verification is performed simultaneously. The force sensor status verification includes: obtaining the zero-point offset value, working temperature parameter, and bit error rate of the weak current communication link of each six-dimensional force sensor from the robot end. If the zero-point offset value exceeds the preset offset tolerance, or the working temperature parameter exceeds the preset working temperature range, or the bit error rate exceeds the preset bit error rate threshold, the force sensor status is determined to be abnormal. When authentication is successful and all force sensors are functioning normally, the main robot sets a handshake success flag as a compliant search enable signal. When authentication fails or any force sensor is abnormal, the main robot broadcasts a lock prohibition command through the low-voltage communication link, locking the mechanical latch's drive circuit in the open state and cutting off the high-voltage connection control circuit. At the same time, the main robot records abnormal event information including the failure type and timestamp, and triggers an alarm indication.

4. The stress-free docking and distributed force control method for multi-robot collaborative operation according to claim 1, characterized in that, Step S3 includes: An admittance control law is constructed using a decoupled six-degree-of-freedom second-order admittance model. The impedance parameters of the admittance control law include a virtual inertia matrix, a virtual damping matrix, and a virtual stiffness matrix. Online adjustment of the parameters includes: The acquired six-dimensional force sensor data is low-pass filtered to obtain filtered force vectors and torque vectors. Based on the filtered force vectors and torque vectors, the magnitude of the force component in each coordinate axis direction is calculated. The magnitude of the force component is input into a preset monotonically decreasing function to generate a corresponding virtual stiffness adjustment coefficient. The virtual stiffness adjustment coefficient is then multiplied by the diagonal element in the virtual stiffness matrix corresponding to that coordinate axis, so that the virtual stiffness is lower in the direction with greater force. At the same time, the rate of change of the filtered force vector is calculated. For coordinate axes with a negative rate of change and a force component magnitude greater than a preset overshoot suppression threshold, the damping coefficient in the virtual damping matrix corresponding to that coordinate axis is increased. Substitute the adjusted virtual damping matrix and virtual stiffness matrix into the admittance control law to solve for the end-effector pose fine-tuning amount, so as to drive each robot end to perform six-degree-of-freedom compliant motion until the force components in each direction converge to the range allowed by the preset zero force condition.

5. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 4, characterized in that, When the force component amplitudes in each coordinate axis direction are all lower than the preset mode switching threshold, and the mode switching threshold is less than the preset overshoot suppression threshold, the online adjustment also includes: Set all diagonal elements of the virtual stiffness matrix to the preset minimum stiffness value and set all diagonal elements of the virtual damping matrix to the preset maximum damping value, so that the robot end effector enters the isotropic fine compliant attitude adjustment mode; in the fine compliant attitude adjustment mode, the end effector pose fine adjustment amount solved by the admittance control law is limited, and the limiting threshold is set according to the allowable assembly gap of the mechanical latch. The system monitors six-dimensional force sensor data in real time. If the amplitude fluctuation of the force component in any direction exceeds the preset fluctuation tolerance, the corresponding virtual damping coefficient is temporarily increased according to the fluctuation amplitude in that direction until the force component fluctuation in that direction returns to within the fluctuation tolerance. Then, the virtual damping coefficient is restored to the preset maximum damping value.

6. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 1, characterized in that, Step S4 includes: The preset zero-force condition includes force thresholds and torque thresholds set for each channel of the six-dimensional force sensor. When the absolute value of the force component of each channel is lower than the corresponding force threshold and the absolute value of the torque component of each channel is lower than the corresponding torque threshold, it is determined that the force in each direction meets the preset zero-force condition. A timer is set to count the duration for which the preset zero-force condition is continuously met. The timer is reset when the absolute value of the force component or the absolute value of the torque component in any channel exceeds the corresponding threshold. During the timer's counting process, a fault-tolerant counter is set. If the force component or torque component momentarily exceeds the tolerance within a single sampling period, but the magnitude of the exceedance is lower than the preset tolerance margin, and the number of channels exceeding the tolerance does not exceed the preset upper limit of the number of channels, the fault-tolerant counter is incremented without resetting the timer. The timer is reset and the fault-tolerant counter is cleared only when the accumulated value of the fault-tolerant counter reaches the preset upper limit of the number of tolerances. When the timer reaches the preset stable duration, the main robot sends a mechanical latch locking enable signal to drive the electromagnetic latch to close. At the same time, it stops the operation of the admittance control law and latches the final impedance parameter adjusted online by the admittance control law as the initial impedance reference value for the collaborative transport stage of the combined body.

7. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 1, characterized in that, Step S5 includes: The main robot detects the electrical connectivity of the weak current communication links in each interface, records the interface connection as the first logic value, and records the interface disconnection as the second logic value, and generates a connection topology matrix, which represents the connection relationship between each robot end effector. The six-dimensional force sensor data of each robot end effector are transformed into a global coordinate system with the estimated center of mass of the virtual assembly as the origin through a force transformation matrix. The force transformation matrix is ​​jointly determined by the current pose of each robot end effector and the connection topology matrix. The extended Kalman filter is initialized, with the estimated center of mass position as the initial value of the center of mass position, the theoretical inertial parameters of the virtual assembly as the initial values ​​of the inertial parameters, and the initial state covariance matrix is ​​set. Within each filtering cycle, a state prediction model is constructed based on the acceleration signal of the desired motion trajectory of the combined body, the state prediction value is calculated, and the resultant force and torque of the six-dimensional force sensor after being transformed to the global coordinate system by the force transformation matrix, as well as the synthetic motion state of each robot end effector obtained by the joint encoder and the forward kinematic solution, are used as joint observations to correct the state prediction value. The online identification results of the centroid position and inertial parameters of the virtual combined body are then output.

8. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 7, characterized in that, During the operation of the extended Kalman filter, residual detection is performed on the resultant force and resultant torque of the six-dimensional force sensor in the joint observation. When the residual exceeds the preset residual abnormality threshold, it is determined that the corresponding six-dimensional force sensor data has a measurement abnormality. The observation noise covariance matrix corresponding to the six-dimensional force sensor data is temporarily amplified, and the amplified observation noise covariance matrix is ​​used for state correction within the filtering cycle. When the residual exceeds the residual abnormal threshold for multiple consecutive filtering cycles, the master robot sends a force sensor re-inspection command to the corresponding slave robot through a weak current communication link. If the re-inspection result returned by the slave robot confirms that the force sensor is abnormal, the master robot removes the six-dimensional force sensor data corresponding to the slave robot from the joint observation and regenerates the connection topology matrix. At the same time, it updates the force transformation matrix according to the regenerated connection topology matrix to maintain the continuity of the online identification of the centroid position and inertial parameters of the virtual assembly.

9. The method for stress-free docking and distributed force control in multi-robot collaborative operation according to claim 1, characterized in that, Step S6 includes: Based on the identified center of mass position and inertial parameters of the virtual assembly, a rigid body dynamics equation is constructed for the virtual assembly. The desired end force of each robot is mapped to the resultant force and resultant torque at the center of mass of the virtual assembly through the rigid body dynamics equation. The desired end force of each robot is decomposed into kinematic force components and internal force components. The kinematic force components are used to synthesize the resultant force and resultant torque required for the desired motion of the assembly. The internal force components are located in the zero space of the spinor of the end contact force of each robot, satisfying the constraint that the sum of the internal force components is zero, and their contribution to the resultant force and resultant torque at the center of mass is zero. The optimization objective is the weighted sum of squares of the joint torques of each robot. The weight coefficient of each joint is set according to the reciprocal of the rated torque of the corresponding joint. The joint torque is obtained by mapping the desired end effector force to the joint space using the force Jacobian matrix of each robot. The constraints include the equality constraint that the motion force components satisfy the required resultant force and resultant torque, and the friction cone inequality constraint applied to the desired end effector force of each robot. The friction cone inequality constraint is used to prevent relative sliding between the end effector and the object contact surface. A quadratic programming solver is used to solve the optimization objective under the constraints and output the desired end effector force compensation amount of each robot.

10. A stress-free docking and distributed force control method for multi-robot collaborative operation according to claim 9, characterized in that, Within each control cycle, the mass matrix and Coriolis force terms in the rigid body dynamics equations are updated based on the current inertial parameters identified online by the extended Kalman filter. The system monitors the desired end-effector force compensation for each robot. If any force or torque component of the desired end-effector force compensation exceeds the end-effector force range corresponding to the torque output limit of the robot, the joint weight coefficient of the robot is multiplied by a preset amplification factor, and the quadratic programming solver is retried until all desired end-effector force compensations are within the end-effector force range. A penalty term is added to the objective function of the quadratic programming solver to penalize the difference between the desired end-effector force compensation in the current control cycle and the previous control cycle. If the limit is still exceeded after a preset number of re-solutions, the acceleration of the desired motion of the assembly is scaled proportionally, and the scaled acceleration is re-input into the rigid body dynamics equations.

Images