A heterogeneous robot cluster cooperative control method and system based on digital twinning

Abstract

This invention discloses a collaborative control method and system for heterogeneous robot swarms based on digital twins, belonging to the field of robot swarm control technology. It addresses the problems of instruction mapping deviation, difficulty in online correction of coupling disturbances, and lack of safety boundary constraints. This invention constructs an online calibration and adaptive correction mechanism driven by digital twins. First, it collects instruction-response data pairs and fits individual mapping parameters. Second, it simulates and obtains coupling-disturbance parameters in the digital twin and iteratively optimizes them through gradient descent. Then, it generates a multidimensional ellipsoidal safety boundary based on the covariance matrix and monitors the status in real time during execution. If the boundary is exceeded, a local adjustment of the buffer trajectory is triggered, thereby achieving proactive perception of uncertainty and dynamic safety constraints. This avoids the computational overhead caused by global replanning, and the system forms a closed-loop collaborative control process, improving the accuracy, robustness, and task completion efficiency of swarm collaboration in complex manufacturing scenarios.

Description

Technical Field

[0001] This invention relates to the field of robot swarm control technology, and more specifically, to a method and system for collaborative control of heterogeneous robot swarms based on digital twins. Background Technology

[0002] In the fields of intelligent manufacturing, logistics warehousing, and collaborative operations of unmanned systems, heterogeneous robot swarm collaborative control technology has been widely applied. These systems typically use a central controller or distributed scheduling algorithm to issue pre-planned task instructions to each robot, enabling them to complete collaborative tasks according to predetermined paths and sequences. Existing solutions focus on optimizing path planning efficiency and communication synchronization, allocating tasks and adjusting trajectories through kinematic models or reinforcement learning methods to achieve consistent and collaborative control of swarm motion. These systems generally generate control instructions based on model prediction or rule matching, and their technical implementation involves the cross-integration of robot kinematics, multi-agent systems, and real-time communication networks. However, due to significant differences in the individual dynamic parameters, response latency, and execution accuracy of heterogeneous robots, and the often complex and variable collaborative operating environment, traditional models struggle to accurately depict the actual physical motion process, leading to a non-negligible mapping error between planned instructions and execution results.

[0003] Existing technologies have significant limitations in control precision and robustness: differences in dynamic parameters, response delays, and execution precision among heterogeneous robots result in a substantial mapping deviation between issued commands and physical execution. Traditional methods typically employ fixed motion models for planning, lacking the ability to identify and dynamically correct individual uncertainties and coupled disturbances online, making it difficult to maintain precise spatial relative positions and motion consistency in complex collaborative tasks. Furthermore, existing systems lack quantitative representation of motion state uncertainties and proactive constraints on risk boundaries. If individual robots deviate from the planned path due to disturbances or execution errors, it can easily trigger cascading collisions or task interruptions, severely weakening the safety and reliability of swarm collaboration. In summary, existing technologies struggle to achieve refined dynamic control and adaptive constraints on safety boundaries for heterogeneous robot swarms, necessitating the introduction of digital twins and online identification mechanisms to improve system performance. Summary of the Invention

[0004] To overcome the aforementioned deficiencies of the prior art, embodiments of the present invention provide a method and system for collaborative control of heterogeneous robot clusters based on digital twins to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: A method for cooperative control of heterogeneous robot swarms based on digital twins, characterized by the following steps: When heterogeneous robots are connected to the collaborative control platform, the platform sends a predefined standardized motion excitation command sequence to each heterogeneous robot and simultaneously collects the motion response data of each heterogeneous robot to form a command-response data pair. Based on command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on a preset cooperative task in a digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. Before executing the collaborative task, the instructions for the pre-set micro-task trajectory are issued to all heterogeneous robots and the actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the instruction-motion response mapping parameters and the spatial coupling-perturbation correction parameters is calculated. With the goal of minimizing the error, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. Based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, the collaborative task is simulated and path planning is performed in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the calculated multidimensional ellipsoid boundary representing the uncertainty of the motion state is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The planned task instructions are sent to the heterogeneous robots for execution, and the motion state data of the heterogeneous robots are collected. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

[0006] In a preferred embodiment, when heterogeneous robots are connected to the collaborative control platform, the platform sends a predefined standardized motion excitation command sequence to each heterogeneous robot and simultaneously collects motion response data from each robot to form command-response data pairs. Specific steps include: When heterogeneous robots are connected to the collaborative control platform, a standardized motion excitation command sequence covering basic motion modes is generated. The standardized motion excitation command sequence includes four basic command modes: uniform linear motion, uniformly accelerated motion, uniform circular motion, and fixed-point rotational motion. The standardized motion excitation command sequence is sequentially sent to each heterogeneous robot connected to the collaborative control platform at preset time intervals.

[0007] In a preferred embodiment, motion response data of each heterogeneous robot are collected synchronously to form command-response data pairs. Specific steps include: For each standardized motion excitation command in the sequence of standardized motion excitation commands, starting from the moment the standardized motion excitation command is issued, the end pose data, joint angle data and motion speed data of the corresponding heterogeneous robot are collected as motion response data until the moment the next standardized motion excitation command is issued. Each standardized motion excitation command is aligned and bound with all corresponding motion response data to form an initial command-response data pair; The initial command-response data pairs are validated, and incomplete data pairs caused by communication interruption or data packet loss are removed to obtain the command-response data pair set.

[0008] In a preferred embodiment, based on command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot to calculate the command-motion response mapping parameters. In a digital twin environment, a cluster robot cooperative motion simulation is performed on a preset cooperative task to obtain spatial relative position and motion disturbance data. Then, spatial coupling-disturbance correction parameters are calculated through linear regression analysis. Specific steps include: For each heterogeneous robot, extract the instruction feature vector from the standardized motion excitation instructions of the robot contained in the instruction-response data pair, and extract the response feature vector from the corresponding motion response data. The least squares method is used to fit the command feature vector and response feature vector of each heterogeneous robot, and the mapping coefficient matrix that maps the command feature vector of the heterogeneous robot to the predicted motion response is obtained, which serves as the command-motion response mapping parameter of the heterogeneous robot. In a digital twin environment, a corresponding digital twin is constructed for each heterogeneous robot based on the command-motion response mapping parameters, and all digital twins are driven to perform cooperative motion simulation according to the preset cooperative tasks. During the execution of the cooperative motion simulation, spatial relative position data between each digital twin and motion disturbance data caused by mutual motion interference are collected. Based on the collected spatial relative position data and motion disturbance data, the spatial coupling-disturbance correction parameter, which characterizes the motion coupling and disturbance relationship between robots in the cluster, is calculated using linear regression analysis.

[0009] In a preferred embodiment, before executing the collaborative task, pre-set micro-task trajectories are issued to all heterogeneous robots, and actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the command-motion response mapping parameters and spatial coupling-perturbation correction parameters is calculated. With the goal of minimizing the error, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. The specific steps include: Before executing the collaborative task, a set of pre-set micro-task trajectory instruction sequences are issued to all heterogeneous robots, and the actual motion data generated by each heterogeneous robot during the execution of the micro-task trajectory instruction sequence is collected simultaneously. In a digital twin environment, based on the constructed heterogeneous robot digital twin, command-motion response mapping parameters, and spatial coupling-perturbation correction parameters, the micro-task trajectory command sequence is simulated to generate corresponding predicted motion data. For each heterogeneous robot, the difference between the actual motion data and the predicted motion data of the heterogeneous robot at each sampling time is calculated to obtain the motion error sequence; The sum of squares of the motion error sequences of all heterogeneous robots is used as the overall optimization objective. The gradient descent method is used to iteratively calculate the spatial coupling-perturbation correction parameters. In each iteration, the gradient of the overall optimization objective with respect to the spatial coupling-perturbation correction parameters is calculated, and the parameter values ​​of the spatial coupling-perturbation correction parameters are updated in the opposite direction of the gradient until the overall optimization objective is lower than the preset threshold or the maximum number of iterations is reached. The final updated parameter values ​​are used as the iteratively updated spatial coupling-perturbation correction parameters.

[0010] In a preferred embodiment, based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, the collaborative task is simulated and path planning is performed in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted values ​​and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the specific steps include: In a digital twin environment, simulation and path planning are performed to drive all heterogeneous robot digital twins to perform collaborative tasks based on command-motion response mapping parameters and iteratively updated spatial coupling-perturbation correction parameters. During the simulation, a task path for executing collaborative tasks is planned for each heterogeneous robot digital twin. The task path is composed of a series of planned task path points connected sequentially. For each heterogeneous robot digital twin, at each planned task path point, obtain the corresponding planning and control instructions; The planning control command is input into the command-motion response mapping parameters corresponding to the heterogeneous robot to obtain the first predicted state; Based on the state of the digital twins of other heterogeneous robots in the cluster, the first predicted state is corrected according to the spatial coupling-perturbation correction parameters to obtain the final predicted motion state value of the heterogeneous robot at the planned task path point. Simultaneously, based on the error characteristics of the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the covariance matrix corresponding to the final predicted motion state value is calculated.

[0011] In a preferred embodiment, the calculated multidimensional ellipsoidal boundary representing the uncertainty of the motion state is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task instructions are generated. The specific steps include: Based on the final predicted motion state value and covariance matrix, a multidimensional ellipsoidal space boundary characterizing the uncertainty of the motion state of the heterogeneous robot is calculated by using a preset probability confidence level and Mahalanobis distance. The multidimensional ellipsoidal spatial boundary is defined as the multidimensional safety boundary corresponding to the planned task path point of the heterogeneous robot. Based on the task path planned for all heterogeneous robot digital twins and the multidimensional safety boundary corresponding to each planned task path point, the final planned task instruction sequence is generated.

[0012] In a preferred embodiment, the planned task instructions are issued to the heterogeneous robots for execution, and the motion state data of the heterogeneous robots is collected. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the specific steps include: The task instruction sequence is sent to the heterogeneous robot cluster for execution, and motion state data of each heterogeneous robot is collected; The motion state data of each heterogeneous robot collected is compared with the multidimensional safety boundary of the corresponding planned task path point on the planned task path of the heterogeneous robot in the digital twin. When the comparison results determine that the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data.

[0013] In a preferred embodiment, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution. Specific steps include: Starting from the current actual state, calculate the planned task path point that has not been passed on the original task path of the heterogeneous robot and is closest to the starting point in the digital twin, and determine the geometric center point of the multidimensional safety boundary corresponding to the planned task path point as the closest safe point; In the digital twin, a smooth buffer trajectory is generated using a polynomial trajectory planning method, starting from the initial point and ending at the nearest safe point. The buffer trajectory is converted into a corresponding buffer trajectory instruction and sent to the corresponding heterogeneous robot for execution.

[0014] A heterogeneous robot swarm cooperative control system based on digital twins, used to implement the aforementioned heterogeneous robot swarm cooperative control method based on digital twins, includes: The instruction-response data acquisition module is used to send a predefined standardized motion excitation instruction sequence to each heterogeneous robot through the collaborative control platform when the heterogeneous robot is connected to the collaborative control platform, and simultaneously collect the motion response data of each heterogeneous robot to form an instruction-response data pair. The mapping correction parameter calculation module is used to fit the command and response data of each heterogeneous robot based on the command-response data pair using the least squares method, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on the preset cooperative task in the digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. The parameter iteration update module is used to issue pre-set micro-task trajectory instructions to all heterogeneous robots and collect actual motion data before executing collaborative tasks. It calculates the error between the actual motion data and the predicted data in the digital twin based on instruction-motion response mapping parameters and spatial coupling-perturbation correction parameters. With the goal of minimizing the error, the gradient descent method is used to iteratively update the spatial coupling-perturbation correction parameters. The simulation planning safety boundary generation module is used to simulate and plan the cooperative task in the digital twin based on the command-motion response mapping parameters and the updated spatial coupling-disturbance correction parameters. During the simulation, for each planned task path point of the heterogeneous robot, the multidimensional ellipsoid boundary that represents the uncertainty of the motion state is calculated based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-disturbance correction parameters. This multidimensional safety boundary is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The monitoring buffer trajectory generation module sends the planned task instructions to the heterogeneous robots for execution and collects the motion state data of the heterogeneous robots. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

[0015] The technical effects and advantages of this invention are as follows: This invention first systematically calibrates heterogeneous robots using standardized motion excitation command sequences, collects command-response data pairs, and fits the command-motion response mapping parameters for each robot using the least squares method. Simultaneously, it simulates cluster cooperative motion in a digital twin environment to obtain spatial relative position and motion perturbation data, and calculates spatial coupling-perturbation correction parameters through linear regression analysis. Based on this, before executing the cooperative task, it issues micro-task trajectory commands and collects actual motion data. With the goal of minimizing the error between actual and predicted data, it uses gradient descent to iteratively update the spatial coupling-perturbation correction parameters. Subsequently, based on the updated mapping and correction parameters, collaborative task simulation and path planning are performed in the digital twin. At each planned task path point, a multidimensional ellipsoid boundary is calculated based on the predicted value and covariance matrix, defined as the multidimensional safety boundary corresponding to the task path point, and planned task instructions are generated. Furthermore, motion state data is collected in real time during actual execution. Once the motion state of any heterogeneous robot is found to exceed the multidimensional safety boundary, the current actual state is determined in the digital twin, and a buffer trajectory instruction is generated and issued for execution, starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary. This series of technical means significantly improves the accuracy, real-time performance, and robustness of heterogeneous robot swarm collaborative control. It not only achieves proactive perception of uncertainties in the swarm's motion state and dynamic safety boundary constraints, but also effectively enhances motion consistency, safety, and task completion success rate under complex collaborative tasks. Attached Figure Description

[0016] Figure 1 This is a flowchart of a heterogeneous robot cluster collaborative control method based on digital twins according to the present invention.

[0017] Figure 2 This is a schematic diagram of the structure of a heterogeneous robot cluster collaborative control system based on digital twins according to the present invention. Detailed Implementation

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

[0019] Example 1: As Figure 1 As shown, this invention presents a method for cooperative control of heterogeneous robot swarms based on digital twins, which includes the following steps: When heterogeneous robots are connected to the collaborative control platform, the platform sends a predefined standardized motion excitation command sequence to each heterogeneous robot and simultaneously collects the motion response data of each heterogeneous robot to form a command-response data pair. Based on command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on a preset cooperative task in a digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. Before executing the collaborative task, the instructions for the pre-set micro-task trajectory are issued to all heterogeneous robots and the actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the instruction-motion response mapping parameters and the spatial coupling-perturbation correction parameters is calculated. With the goal of minimizing the error, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. Based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, the collaborative task is simulated and path planning is performed in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the calculated multidimensional ellipsoid boundary representing the uncertainty of the motion state is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The planned task instructions are sent to the heterogeneous robots for execution, and the motion state data of the heterogeneous robots are collected. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

[0020] Step 1: The food processor receives the fermentation program instructions set by the user. These instructions include the fermentation time range and the final fermentation status. The specific implementation is as follows: When any heterogeneous robot connects to the collaborative control platform, the platform first generates a standardized sequence of motion excitation commands for the robot. This sequence consists of four basic command modes: uniform linear motion, uniformly accelerated motion, uniform circular motion, and fixed-point rotational motion. The duration, velocity amplitude, acceleration amplitude, and rotational angular velocity of each basic command mode are preset to ensure coverage of common motion ranges for heterogeneous robots. For example, the linear velocity of uniform linear motion is set to 0.5 m / s, with a duration of 5 seconds; the initial velocity of uniformly accelerated motion is set to 0, the acceleration is set to 0.2 m / s², and the duration is 5 seconds; the linear velocity of uniform circular motion is set to 0.3 m / s, the turning radius is set to 1 m, and the duration is 10 seconds; and the angular velocity of fixed-point rotational motion is set to 0.5 radians / second, with a duration of 4 seconds.

[0021] Subsequently, the collaborative control platform sends the standardized motion excitation command sequence to the heterogeneous robot at preset time intervals. The preset time interval is 1 second, that is, after each standardized motion excitation command is sent, wait 1 second before sending the next standardized motion excitation command. This ensures that the heterogeneous robot has enough time to respond and avoids the overlap of standardized motion excitation commands. At the same time, the absolute timestamp of each standardized motion excitation command is recorded, that is, the Coordinated Universal Time obtained from the system clock.

[0022] Step 2: Synchronously collect motion response data from each heterogeneous robot to form command-response data pairs. The specific implementation is as follows: For each standardized motion excitation command issued, an absolute timestamp is recorded from the moment the standardized motion excitation command is issued, and motion response data of the corresponding heterogeneous robot is continuously collected. The motion response data includes end-effector pose data, joint angle data, and motion speed data.

[0023] The end-effector pose data is acquired through a six-degree-of-freedom inertial measurement unit and a vision positioning device installed on the robot's end effector, outputting three-dimensional spatial position coordinates and attitude Euler angles. Joint angle data is read in real time by the absolute encoder built into each joint, outputting the rotation angle of each joint. Motion velocity data, including the end-effector linear velocity and angular velocity, is calculated differentially by the IMU and the encoder, with the sampling frequency set to 100 Hz until the next standardized motion excitation command is issued.

[0024] After completing the data acquisition for all standardized motion excitation commands, each standardized motion excitation command and its corresponding complete motion response data are aligned and bound according to absolute timestamps. Specifically, using the absolute timestamp of each standardized motion excitation command as the starting index, the end-position pose data, joint angle data, and motion velocity data collected at each sampling moment after the starting index and before the absolute timestamp of the next standardized motion excitation command are uniformly classified under the name of that standardized motion excitation command, forming an initial command-response data pair.

[0025] Each initial command-response data pair consists of two parts: a command feature vector and a response feature matrix. The command feature vector is composed of the type identifier of the command, such as uniform linear motion, as well as the velocity amplitude, acceleration amplitude, angular velocity amplitude, and duration. The response feature matrix is ​​composed of the end pose data, joint angle data, and motion velocity data at all sampling times arranged in chronological order. To ensure data consistency, all timestamps are based on Coordinated Universal Time (UTC), and the response data under the same standardized motion excitation command are strictly continuous in time.

[0026] Based on the duration of each standardized motion excitation command and the sampling frequency of the motion response data, the theoretical number of sampling moments corresponding to the standardized motion excitation command is calculated. For example, the duration of the standardized motion excitation command is 5 seconds for uniform linear motion, 5 seconds for uniformly accelerated motion, 10 seconds for uniform circular motion, and 4 seconds for fixed-point rotational motion. The sampling frequency of the motion response data is 100 Hz. Therefore, the theoretical number of sampling moments for uniform linear motion is 5 seconds multiplied by 100 Hz, which equals 500 sampling moments. The same applies to uniformly accelerated motion, 500 sampling moments for uniform circular motion, 1000 sampling moments for uniform circular motion, and 400 sampling moments for fixed-point rotational motion.

[0027] Then, the actual number of sampling moments is extracted from the response feature matrix of the initial command-response data pair, and the actual number of sampling moments is compared with the theoretical number of sampling moments. If the actual number of sampling moments is less than 95% of the theoretical number of sampling moments, the initial command-response data pair is determined to be incomplete due to communication interruption or data packet loss and is discarded. At the same time, the continuity of the absolute timestamps corresponding to adjacent sampling moments in the response feature matrix is ​​checked. If there is an absolute timestamp interval greater than 11 milliseconds between any two consecutive sampling moments, that is, more than 1.1 times the sampling period of 10 milliseconds, since the sampling period is 10 milliseconds, a time interval greater than 11 milliseconds means that at least one sampling point is missing. Therefore, it is also determined that there is data packet loss and the pair is discarded. All complete initial command-response data pairs are retained to form a command-response data pair set. Each pair of data in the command-response data pair set contains a complete command feature vector and a time-continuous response feature matrix.

[0028] Step 3: Based on the command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot, calculate the command-motion response mapping parameters, and perform swarm robot cooperative motion simulation on the preset cooperative task in the digital twin environment to obtain spatial relative position and motion disturbance data. The spatial coupling-disturbance correction parameters are then calculated through linear regression analysis. The specific implementation is as follows: For each heterogeneous robot, the instruction feature vector of each standardized motion excitation instruction is read from the instruction-response data pair set corresponding to that heterogeneous robot. The instruction feature vector consists of instruction type identifier, velocity amplitude, acceleration amplitude, angular velocity amplitude, and duration.

[0029] Simultaneously, a response feature vector is extracted from the response feature matrix corresponding to the same standardized motion excitation command. This response feature vector is composed of a vector obtained by taking the arithmetic mean of the end-effector pose data, joint angle data, and motion velocity data at all sampling moments within the duration of the standardized motion excitation command over the time dimension. Furthermore, to establish a stable mapping relationship, the end-effector position coordinates in the response feature vector are further transformed into position offsets relative to the start time of the standardized motion excitation command, the Euler angles of the posture are transformed into angle offsets relative to the start posture of the standardized motion excitation command, and the joint angle data are transformed into angle offsets relative to the start time. Finally, a response feature vector with dimensions matching the command feature vector is formed, ensuring that each command under each heterogeneous robot can obtain a command feature vector and a corresponding response feature vector.

[0030] The extracted command feature vector and response feature vector for each heterogeneous robot are as follows: the command feature vector consists of command type identifier, velocity amplitude, acceleration amplitude, angular velocity amplitude and duration; the response feature vector is composed of the end pose data, joint angle data and motion velocity data of all sampling moments within the duration of the standardized motion excitation command, which are arithmetically averaged and transformed into offsets relative to the start time.

[0031] For each heterogeneous robot, a matrix is ​​formed by arranging the command feature vectors corresponding to all its standardized motion excitation commands in rows, called the command feature matrix. At the same time, another matrix is ​​formed by arranging the corresponding response feature vectors in rows, called the response feature matrix. The least squares method is used to fit these two matrices. Specifically, firstly, the product of the transpose of the command feature matrix and the command feature matrix itself is calculated to obtain the first intermediate matrix. Then, the product of the transpose of the command feature matrix and the response feature matrix is ​​calculated to obtain the second intermediate matrix. Then, the system of linear equations is solved, that is, the first intermediate matrix multiplied by the unknown mapping coefficient matrix equals the second intermediate matrix, and the solution of the mapping coefficient matrix is ​​obtained. The mapping coefficient matrix is ​​the command-motion response mapping parameter of the heterogeneous robot.

[0032] In a digital twin environment, a corresponding digital twin is constructed for each heterogeneous robot based on its command-motion response mapping parameters. During construction, the mapping coefficient matrix is ​​stored as the motion response mapping table of the digital twin. This motion response mapping table is fixed in the internal data structure of the digital twin in matrix form. When the digital twin receives a command feature vector, it performs matrix multiplication to multiply the command feature vector with the mapping coefficient matrix to directly calculate the corresponding response feature vector. Then, based on the offset in the response feature vector, it is superimposed dimension by dimension onto the end position, attitude Euler angles, and joint angle data at the starting time. The complete motion response sequence is generated by interpolation according to the sampling time interval, thereby realizing the prediction of motion response. Subsequently, according to the preset collaborative task, such as multi-robot collaborative handling or formation movement, a collaborative motion command sequence of each digital twin is generated, and all digital twins are driven to execute collaborative motion simulation in parallel in the same virtual space-time.

[0033] During the simulation, spatial relative position data between each digital twin is collected. This spatial relative position data is obtained by calculating the three-dimensional coordinate difference between the geometric centers of each pair of digital twins. At the same time, motion disturbance data generated by mutual motion interference is collected. This motion disturbance data is the deviation vector between the actual motion response of each digital twin and the predicted motion response when executing the same instruction feature vector alone. The deviation vector includes end position deviation, attitude deviation, and velocity deviation.

[0034] Using the spatial relative position data between all digital twins at each moment as independent variables and the motion perturbation data at the corresponding moment as dependent variables, a linear regression analysis method is employed. The independent variables at all moments are arranged in rows to form an independent variable matrix, and the dependent variables at all moments are arranged in rows to form a dependent variable matrix. First, the product of the transpose of the independent variable matrix and the independent variable matrix is ​​calculated to obtain the third intermediate matrix. Then, the product of the transpose of the independent variable matrix and the dependent variable matrix is ​​calculated to obtain the fourth intermediate matrix. Finally, the linear equation system is solved, i.e., the third intermediate matrix multiplied by the unknown regression coefficient matrix equals the fourth intermediate matrix. The solution to this regression coefficient matrix is ​​the spatial coupling-perturbation correction parameter.

[0035] Step 4: Before executing the collaborative task, pre-set micro-task trajectory instructions are issued to all heterogeneous robots, and actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the instruction-motion response mapping parameters and spatial coupling-perturbation correction parameters is calculated. With minimizing the error as the objective, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. The specific implementation is as follows: Before executing the collaborative task, a set of pre-set micro-task trajectory instruction sequences is first issued to all heterogeneous robots that have been connected to the collaborative control platform. This micro-task trajectory instruction sequence consists of several standardized motion excitation instructions, the instruction type, velocity amplitude, acceleration amplitude, angular velocity amplitude, and duration of which are all pre-set. Simultaneously, the actual motion data generated by each heterogeneous robot during the execution of the micro-task trajectory instruction sequence is collected in real time at a sampling frequency of 100 Hz through the six-degree-of-freedom inertial measurement unit, visual positioning device, and joint absolute encoder installed on each heterogeneous robot. This includes end-effector pose data, joint angle data, and motion velocity data, and is recorded and aligned according to absolute timestamps to form the actual motion data of each robot.

[0036] In a digital twin environment, simulations are performed on the same micro-task trajectory command sequence based on heterogeneous robot digital twins, command-motion response mapping parameters, and initial spatial coupling-perturbation correction parameters. Specifically, the command feature vector of each standardized motion excitation command in the micro-task trajectory command sequence is multiplied by the command-motion response mapping parameters to obtain the single-machine predicted motion response corresponding to the command, i.e., the motion data when the heterogeneous robot executes the same command feature vector alone. Subsequently, based on the spatial relative position data between each digital twin at the current moment, it is multiplied by the spatial coupling-perturbation correction parameters to obtain the correction amount caused by mutual motion interference. This correction amount is added element-by-element to the single-machine predicted motion response, i.e., linearly superimposed, to obtain the predicted motion data after coupling. This predicted motion data includes the end position, attitude Euler angle, linear velocity, and angular velocity of each digital twin at each moment, and its sampling frequency is consistent with the actual sampling frequency.

[0037] First, for each heterogeneous robot, predictive motion data for each digital twin is obtained. This predictive motion data includes the end-effector position, attitude Euler angles, linear velocity, and angular velocity at each sampling time. The sampling frequency is 100 Hz, consistent with the actual motion data collected. The actual motion data and predictive motion data of each heterogeneous robot are subtracted element by element at the same sampling time to obtain the motion error of the heterogeneous robot at that sampling time. The motion error includes end-effector position error, attitude Euler angle error, linear velocity error, and angular velocity error. The motion errors at all sampling times are arranged in chronological order to form the motion error sequence of the heterogeneous robot.

[0038] Subsequently, each scalar error value in the motion error sequence of all heterogeneous robots at all sampling times is squared, and then all the squared values ​​are added together to obtain a single scalar value, which is called the overall optimization objective. This overall optimization objective quantifies the overall deviation between the actual running trajectory of all heterogeneous robots and the predicted trajectory of the digital twin.

[0039] Then, with the goal of minimizing the overall optimization objective, the spatial coupling-perturbation correction parameter is iteratively updated. This spatial coupling-perturbation correction parameter is a matrix whose dimension is related to the number of columns in the independent variable matrix and the number of columns in the dependent variable matrix.

[0040] In each iteration, the partial derivative of the overall optimization objective with respect to each element of the spatial coupling-perturbation correction parameters is first calculated. Specifically, the current spatial coupling-perturbation correction parameters are substituted into the calculation process of the predicted motion data of the digital twin to obtain the current predicted motion data, and then the current motion error sequence and the current overall optimization objective are obtained. Then, a small increment is applied to each element in the spatial coupling-perturbation correction parameter matrix. This small increment is preset to 1×10^(-6). The parameter matrix after applying this small increment is substituted back into the above calculation process to obtain a new overall optimization objective. The difference between the new overall optimization objective and the current overall optimization objective is divided by the small increment to obtain the partial derivative value corresponding to the element. The same operation is performed on all elements to form a gradient matrix with the same dimension as the parameter matrix. Each element in the gradient matrix represents the sensitivity of the overall optimization objective to that element.

[0041] Next, subtract a preset learning rate multiplied by the gradient value corresponding to that element from each element in the current spatial coupling-perturbation correction parameter matrix to obtain the updated spatial coupling-perturbation correction parameter matrix. The learning rate is a positive number less than 1, for example, set to 0.001. Repeat the above iterative process. After each iteration, check whether the overall optimization objective is lower than a preset threshold, for example, 0.01 mm squared, or whether the preset maximum number of iterations, for example, has been reached. If either condition is met, stop the iteration and use the last updated spatial coupling-perturbation correction parameter as the final updated spatial coupling-perturbation correction parameter.

[0042] Step 5: Based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, simulate and plan the cooperative task in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted values ​​and covariance matrix output by the command-motion response mapping parameters and spatial coupling-perturbation correction parameters, the specific implementation is as follows: In a digital twin environment, simulation and path planning are performed to drive all heterogeneous robot digital twins to perform collaborative tasks, based on the instruction-motion response mapping parameters corresponding to each heterogeneous robot and the iteratively updated spatial coupling-perturbation correction parameters.

[0043] The collaborative task can be multi-robot cooperative handling or formation movement. The task objective is set in advance. At the start of the simulation, a task path that can complete the collaborative task is planned for each heterogeneous robot digital twin. The task path is composed of a series of planned task path points connected in the order of execution time. Each planned task path point contains the end position, attitude Euler angles and joint angle data that the heterogeneous robot digital twin should reach at a specific time. The planning process takes into account the collision avoidance and cooperative constraints between heterogeneous robots, so that all robot paths are coordinated in space and time.

[0044] For each heterogeneous robot digital twin, at each planned task path point on its task path, a planned control command is first generated according to the target state of the task path point, namely the end position, attitude Euler angles, and joint angles, as well as the current initial motion state, namely the end position, attitude Euler angles, and joint angle data at the initial moment, in accordance with a preset control strategy.

[0045] The format of the planning control command is exactly the same as the command feature vector of the standardized motion excitation command, which consists of five dimensions: command type identifier, velocity amplitude, acceleration amplitude, angular velocity amplitude, and duration. The command type identifier is an integer code, for example, 1 represents uniform linear motion, 2 represents uniform acceleration, 3 represents uniform circular motion, and 4 represents fixed-point rotation. The remaining values ​​are calculated based on the kinematic relationship between the path points of the planning task.

[0046] Subsequently, the instruction feature vector of the planning control command is input to the instruction-motion response mapping parameter corresponding to the heterogeneous robot, namely the mapping coefficient matrix. By performing matrix multiplication, specifically multiplying the instruction feature vector with the mapping coefficient matrix, a product vector is obtained. This product vector is the first predicted state, which is a vector consisting of components such as end-position offset, attitude Euler angle offset, joint angle offset, and motion velocity offset.

[0047] At the same moment during the simulation, the current state of all other heterogeneous robot digital twins in the cluster is acquired. The current state includes end position, attitude Euler angles and joint angle data. The spatial relative position data between the heterogeneous robot digital twin and each other digital twin is calculated, and all spatial relative position data are arranged into a row vector in a fixed order. The dimension of the row vector is consistent with the number of columns in the independent variable matrix.

[0048] The row vector is multiplied by the iteratively updated spatial coupling-perturbation correction parameter matrix, where the number of rows in the spatial coupling-perturbation correction parameter matrix is ​​equal to the number of columns in the spatial relative position data vector, and the number of columns is equal to the number of columns in the first predicted state. The result obtained by multiplying the row vector by the correction parameter matrix is ​​the correction amount caused by mutual motion interference.

[0049] The first predicted state is added element-wise to the correction value, i.e., linearly superimposed, to obtain the final predicted motion state value of the heterogeneous robot at the planned task path point. This final predicted motion state value includes the end-effector position offset, attitude Euler angle offset, joint angle offset, and motion velocity offset.

[0050] Simultaneously, based on the error characteristics of the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the covariance matrix corresponding to the final predicted motion state value is calculated. Specifically, firstly, based on the command-response data pair set, for each heterogeneous robot, the actual response feature vector under each standardized motion excitation command is calculated. This actual response feature vector is obtained by arithmetically averaging the end-effector pose data, joint angle data, and motion velocity data at all sampling moments within the duration of the standardized motion excitation command over the time dimension. The offset is obtained by subtracting the corresponding value at the start time from the end-effector position coordinates, attitude Euler angles, and joint angles respectively, while keeping the motion velocity data unchanged. This constitutes the actual response feature vector and the residual vector between it and the response feature vector predicted by the mapping coefficient matrix.

[0051] Then, arrange all residual vectors in rows to form a residual matrix, and calculate the covariance matrix of the residual matrix. Next, calculate the covariance matrix of the motion error sequence based on the motion error sequence. Finally, add the covariance matrix of the residual matrix to the covariance matrix of the motion error sequence to obtain the covariance matrix corresponding to the final predicted motion state value.

[0052] Step 5, characterized by defining the calculated multidimensional ellipsoidal boundary representing the uncertainty of the motion state as the multidimensional safety boundary corresponding to the task path point, and generating the planned task instructions, specifically implemented as follows: The preset probability confidence level is set to 95%, which is used to determine the threshold for the Mahalanobis distance. The squared Mahalanobis distance is defined as the distance between the final predicted motion state value and the target value. State vectors with the same dimensions First, calculate the difference vector between the state vector and the final predicted motion state value. Then, the difference vector is compared with the covariance matrix. Multiplying the inverse matrices yields the intermediate vector. Finally, the dot product of the intermediate vector and the difference vector is performed to obtain the square of the Mahalanobis distance. Specifically, it is expressed as:

[0053] in, This is the final predicted motion state value vector. The covariance matrix corresponding to the final predicted motion state value is... For any one of the following: State vectors of the same dimension. Quantile values ​​of a chi-square distribution with N degrees of freedom at a 95% confidence level. This is the threshold of the squared Mahalanobis distance.

[0054] At a 95% confidence level, the quantile value of the chi-square distribution with N degrees of freedom is the threshold of the squared Mahalanobis distance. This threshold is obtained by looking up a pre-stored chi-square distribution quantile table. For example, when N equals 15, the threshold is approximately 24.9958.

[0055] Then, among all state vectors with the same dimension as the final predicted motion state value, the set of all state vectors that satisfy the square of the Mahalanobis distance less than or equal to the threshold is defined as the multidimensional ellipsoidal space boundary of the heterogeneous robot at the planned task path point. The multidimensional ellipsoidal space boundary is a multidimensional ellipsoid centered on the final predicted motion state value, whose shape is determined by the inverse matrix of the covariance matrix and the threshold.

[0056] The multidimensional ellipsoidal spatial boundary is defined as the multidimensional safety boundary of the heterogeneous robot at the planned task path point. For each heterogeneous robot digital twin, at each planned task path point on its task path, firstly, based on the multidimensional safety boundary at the planned task path point, it is determined whether the final predicted motion state value corresponding to the currently generated planning control command (i.e., the command feature vector composed of command type identifier, velocity amplitude, acceleration amplitude, angular velocity amplitude, and duration) is located inside the multidimensional safety boundary. That is, the Mahalanobis distance of the final predicted motion state value relative to itself is calculated. If the Mahalanobis distance is zero, it means that the value is always inside the multidimensional safety boundary. The check examines whether the end position, attitude Euler angles, linear velocity, and angular velocity at each sampling moment in the predicted motion data generated by the planning and control command do not exceed the multidimensional safety boundary. The determination method is as follows: for each sampling moment, the end position, attitude Euler angles, linear velocity, and angular velocity at that moment are combined into a state vector with the same dimension as the final predicted motion state value. The square of the Mahalanobis distance between the state vector and the final predicted motion state value is calculated. If the square value is less than or equal to the chi-square distribution quantile value with a degree of freedom of less than 95% of the preset probability confidence level, the state at that sampling moment is determined to be inside the multidimensional safety boundary; otherwise, it is determined to be outside the multidimensional safety boundary.

[0057] If there is a risk of exceeding the multidimensional safety boundary, the parameters such as velocity amplitude or acceleration amplitude in the planning control command are adjusted so that the multidimensional ellipsoidal space boundary corresponding to the corrected final predicted motion state value can completely cover the motion space required around the task path point. The required motion space is defined as the set of the end position, attitude Euler angle, linear velocity and angular velocity of all sampling moments in the predicted motion data generated by the planning control command at the planned task path point.

[0058] The adjustment method adopts a binary search approach. Specifically, the adjustment object is the velocity amplitude or acceleration amplitude. The initial adjustment step size is set to 0.01 m / s or 0.01 radians / s². After each adjustment, the complete predicted motion data and the corresponding multidimensional safety boundary are recalculated. It is then determined whether the state vector at all sampling times satisfies the condition that the square of the Mahalanobis distance is less than or equal to the chi-square distribution quantile. If it is satisfied, the adjustment is terminated; otherwise, the adjustment continues in the direction of shrinkage until the condition is satisfied or the preset maximum number of iterations of 100 is reached.

[0059] Meanwhile, during the adjustment process, it is also necessary to ensure that the multidimensional safety boundary does not overlap or conflict with the multidimensional safety boundaries of other heterogeneous robots. Specifically, for each pair of digital twins of heterogeneous robots, the minimum Mahalanobis distance between the corresponding multidimensional ellipsoids at the current planned task path point is calculated. The minimum Mahalanobis distance is obtained by solving the difference vector between the centers of the two multidimensional ellipsoids and considering their respective covariance matrices. If the minimum Mahalanobis distance is less than the sum of the square roots of the threshold of the square of the Mahalanobis distances of the two multidimensional ellipsoids, it is determined that there is an overlap risk. At this time, the speed amplitude or acceleration amplitude of one or both robots is reduced at the same time to shrink the multidimensional ellipsoid until the minimum Mahalanobis distance is greater than or equal to the preset safety margin of 0.02.

[0060] After performing the aforementioned safety checks and parameter adjustments on all planned task path points, the adjusted planning control instructions corresponding to each planned task path point are arranged in chronological order of execution time to form the final planned task instruction sequence for the heterogeneous robot.

[0061] Step 6: Issue the planned task instructions to the heterogeneous robots for execution and collect their motion state data. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the specific implementation is as follows: The task command sequence is sent to the collaborative control platform, which then sends each planned control command sequentially to the corresponding heterogeneous robot according to the specified time interval in each planned control command, driving the heterogeneous robot cluster to execute collaborative tasks. During execution, the motion state data of each heterogeneous robot is collected in real time at a sampling frequency of 100 Hz through a six-degree-of-freedom inertial measurement unit, a visual positioning device, and absolute encoders for each joint installed on each heterogeneous robot. This motion state data includes the end-effector position, attitude Euler angles, linear velocity and angular velocity, and joint angle data at each sampling moment.

[0062] For each heterogeneous robot, during the execution of its task, according to the planned task path point corresponding to the currently executed planning control command, the multidimensional safety boundary at the planned task path point is obtained. The multidimensional safety boundary is a multidimensional ellipsoidal space boundary centered on the final predicted motion state value, determined by the inverse matrix of the covariance matrix and the chi-square distribution quantile values ​​with the dimension as the degree of freedom under a preset probability confidence level of 95%.

[0063] For the motion state data collected at the current sampling moment, the end position, attitude Euler angles, linear velocity, angular velocity, and joint angle data are combined to form an actual state vector with the same dimension as the final predicted motion state value. The square of the Mahalanobis distance between the actual state vector and the final predicted motion state value is calculated. That is, first, the difference vector between the actual state vector and the final predicted motion state value is calculated. Then, the difference vector is multiplied by the inverse of the covariance matrix to obtain an intermediate vector. Finally, the intermediate vector and the difference vector are multiplied by a dot product to obtain the square of the Mahalanobis distance. If the square of the Mahalanobis distance is less than or equal to the chi-square distribution quantile value with a preset probability confidence level of 95% or less, the motion state data is determined to be inside the multidimensional safety boundary. If the square of the Mahalanobis distance is greater than the chi-square distribution quantile value, the motion state data is determined to be outside the corresponding multidimensional safety boundary.

[0064] When the comparison results determine that the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, an anomaly response mechanism is immediately triggered. At this time, based on the motion state data of the heterogeneous robot, the current actual state of the heterogeneous robot is determined in the digital twin. Specifically, the motion state data of the heterogeneous robot at the current sampling moment is directly assigned to the corresponding digital twin in the digital twin as the initial motion state of the digital twin at the current moment. Then, based on this initial motion state and combined with the remaining unexecuted final planning task instruction sequence, the simulation and path planning process starting from the planning task path point is re-performed. That is, the planning control instructions at each remaining planning task path point are regenerated, and the corresponding final predicted motion state value, covariance matrix, and multidimensional safety boundary are recalculated until all remaining planning task path points satisfy the internal constraints of the multidimensional safety boundary and have no overlap or conflict with the multidimensional safety boundaries of other digital twins. Thus, a corrected final planning task instruction sequence is obtained and sent back to the actual heterogeneous robot for continued execution. This cycle continues until the collaborative task is completed.

[0065] Step 7 involves determining the current actual state of the heterogeneous robot within the digital twin based on the motion state data. Starting from this current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution. The specific implementation is as follows: Once the current actual state is determined as the initial motion state of the heterogeneous robot in the digital twin, all planned task path points that have not yet been traversed on the original task path of the heterogeneous robot are traversed in the digital twin. These planned task path points correspond to the instructions that have not yet been executed in the final planned task instruction sequence.

[0066] For each planned task path point that has not yet been visited, calculate the three-dimensional Euclidean distance between the end position in the final predicted motion state value and the end position in the initial motion state corresponding to the planned task path point. That is, calculate the square root of the sum of the squares of the differences in the three-dimensional coordinates of the two points, and select the planned task path point with the smallest Euclidean distance as the planned task path point closest to the starting point.

[0067] The geometric center point of the multidimensional safety boundary corresponding to the most recently planned task path point is determined as the nearest safe point. This geometric center point is the final predicted motion state value at the planned task path point. The final predicted motion state value includes the end position offset, attitude Euler angle offset, joint angle offset, and motion velocity offset. Its center position is obtained by superimposing the end position offset in the final predicted motion state value at the planned task path point with the end position at the start time.

[0068] Subsequently, in the digital twin, a smooth buffer trajectory is generated using a fifth-order polynomial trajectory planning method, with the starting point as the trajectory origin and the nearest safe point as the trajectory endpoint. Specifically, the end position, attitude Euler angles, joint angles, linear velocity, angular velocity, and angular velocity of each joint at the trajectory origin are set to be equal to the corresponding values ​​in the initial motion state. Similarly, the end position, attitude Euler angles, joint angles, linear velocity, angular velocity, and angular velocity of each joint at the trajectory endpoint are set to be equal to the corresponding values ​​in the nearest safe point. Simultaneously, the acceleration at both the trajectory origin and endpoint is set to zero to ensure smoothness. The total duration of the buffer trajectory is calculated based on the motion distance between the origin and safe point and a preset maximum speed limit. For example, the motion distance is divided by 0.3 m / s to obtain a time estimate, which is then rounded up to the nearest second.

[0069] Fifth-order polynomials are independently fitted to the end position, attitude Euler angles, and each joint angle. The coefficients of the fifth-order polynomials are obtained by solving a system of linear equations containing boundary conditions for position, velocity, and acceleration. The unknowns of this system of linear equations are six coefficients. By substituting the end position, velocity, and acceleration values ​​at the start and end times of the trajectory into the expression of the fifth-order polynomial and its first and second derivatives, the six equations are solved simultaneously to obtain the six coefficients. The obtained fifth-order polynomials are discretized according to a sampling time interval of 10 milliseconds to generate a sequence of end position, attitude Euler angles, joint angles, end linear velocity and angular velocity, and joint angular velocity at each sampling time from the start to the end of the trajectory, i.e., buffered trajectory data.

[0070] Each segment of motion in the buffered trajectory data is converted according to the format of standardized motion excitation commands. Specifically, the motion between two adjacent sampling times is approximated as uniform linear motion, and the velocity amplitude is calculated based on the position difference and time difference. This generates several command feature vectors with a command type identifier of 1, i.e., uniform linear motion. Each command feature vector contains a command type identifier of 1, a velocity amplitude, an acceleration amplitude of 0, an angular velocity amplitude of 0, and a duration of 10 milliseconds. All command feature vectors are arranged in chronological order to form a buffered trajectory command sequence. This buffered trajectory command sequence is sent to the collaborative control platform, which then sends each buffered trajectory command to the corresponding actual heterogeneous robot for execution according to the specified sending time interval in each command. This allows the heterogeneous robot to smoothly move from its current actual state to the nearest safe point.

[0071] Example 2: A heterogeneous robot swarm collaborative control system based on digital twins, such as Figure 2 As shown, it specifically includes: The instruction-response data acquisition module is used to send a predefined standardized motion excitation instruction sequence to each heterogeneous robot through the collaborative control platform when the heterogeneous robot is connected to the collaborative control platform, and simultaneously collect the motion response data of each heterogeneous robot to form an instruction-response data pair. The mapping correction parameter calculation module is used to fit the command and response data of each heterogeneous robot based on the command-response data pair using the least squares method, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on the preset cooperative task in the digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. The parameter iteration update module is used to issue pre-set micro-task trajectory instructions to all heterogeneous robots and collect actual motion data before executing collaborative tasks. It calculates the error between the actual motion data and the predicted data in the digital twin based on instruction-motion response mapping parameters and spatial coupling-perturbation correction parameters. With the goal of minimizing the error, the gradient descent method is used to iteratively update the spatial coupling-perturbation correction parameters. The simulation planning safety boundary generation module is used to simulate and plan the cooperative task in the digital twin based on the command-motion response mapping parameters and the updated spatial coupling-disturbance correction parameters. During the simulation, for each planned task path point of the heterogeneous robot, the multidimensional ellipsoid boundary that represents the uncertainty of the motion state is calculated based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-disturbance correction parameters. This multidimensional safety boundary is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The monitoring buffer trajectory generation module sends the planned task instructions to the heterogeneous robots for execution and collects the motion state data of the heterogeneous robots. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

[0072] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0073] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0074] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0075] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0076] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for cooperative control of heterogeneous robot swarms based on digital twins, characterized in that, Includes the following steps: When heterogeneous robots are connected to the collaborative control platform, the platform sends a predefined standardized motion excitation command sequence to each heterogeneous robot and simultaneously collects the motion response data of each heterogeneous robot to form a command-response data pair. Based on command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on a preset cooperative task in a digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. Before executing the collaborative task, the instructions for the pre-set micro-task trajectory are issued to all heterogeneous robots and the actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the instruction-motion response mapping parameters and the spatial coupling-perturbation correction parameters is calculated. With the goal of minimizing the error, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. Based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, the collaborative task is simulated and path planning is performed in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the calculated multidimensional ellipsoid boundary representing the uncertainty of the motion state is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The planned task instructions are sent to the heterogeneous robots for execution, and the motion state data of the heterogeneous robots are collected. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

2. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 1, characterized in that: When heterogeneous robots are connected to the collaborative control platform, the platform sends a predefined, standardized sequence of motion excitation commands to each robot and simultaneously collects motion response data from each robot to form command-response data pairs. Specific steps include: When heterogeneous robots are connected to the collaborative control platform, a standardized motion excitation command sequence covering basic motion modes is generated. The standardized motion excitation command sequence includes four basic command modes: uniform linear motion, uniformly accelerated motion, uniform circular motion, and fixed-point rotational motion. The standardized motion excitation command sequence is sequentially sent to each heterogeneous robot connected to the collaborative control platform at preset time intervals.

3. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 2, characterized in that: Simultaneously collect motion response data from various heterogeneous robots to form command-response data pairs. Specific steps include: For each standardized motion excitation command in the sequence of standardized motion excitation commands, starting from the moment the standardized motion excitation command is issued, the end pose data, joint angle data and motion speed data of the corresponding heterogeneous robot are collected as motion response data until the moment the next standardized motion excitation command is issued. Each standardized motion excitation command is aligned and bound with all corresponding motion response data to form an initial command-response data pair; The initial command-response data pairs are validated, and incomplete data pairs caused by communication interruption or data packet loss are removed to obtain the command-response data pair set.

4. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 1, characterized in that: Based on command-response data pairs, the least squares method is used to fit the command and response data of each heterogeneous robot, and the command-motion response mapping parameters are calculated. In a digital twin environment, a cluster robot cooperative motion simulation is performed on a preset cooperative task to obtain spatial relative position and motion perturbation data. Then, spatial coupling-perturbation correction parameters are calculated through linear regression analysis. Specific steps include: For each heterogeneous robot, extract the instruction feature vector from the standardized motion excitation instructions of the robot contained in the instruction-response data pair, and extract the response feature vector from the corresponding motion response data. The least squares method is used to fit the command feature vector and response feature vector of each heterogeneous robot, and the mapping coefficient matrix that maps the command feature vector of the heterogeneous robot to the predicted motion response is obtained, which serves as the command-motion response mapping parameter of the heterogeneous robot. In a digital twin environment, a corresponding digital twin is constructed for each heterogeneous robot based on the command-motion response mapping parameters, and all digital twins are driven to perform cooperative motion simulation according to the preset cooperative tasks. During the execution of the cooperative motion simulation, spatial relative position data between each digital twin and motion disturbance data caused by mutual motion interference are collected. Based on the collected spatial relative position data and motion disturbance data, the spatial coupling-disturbance correction parameter, which characterizes the motion coupling and disturbance relationship between robots in the cluster, is calculated using linear regression analysis.

5. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 1, characterized in that: Before executing the collaborative task, pre-set micro-task trajectories are issued to all heterogeneous robots, and actual motion data is collected. The error between the actual motion data and the predicted data in the digital twin based on the command-motion response mapping parameters and spatial coupling-perturbation correction parameters is calculated. With the goal of minimizing the error, the spatial coupling-perturbation correction parameters are iteratively updated using the gradient descent method. The specific steps include: Before executing the collaborative task, a set of pre-set micro-task trajectory instruction sequences are issued to all heterogeneous robots, and the actual motion data generated by each heterogeneous robot during the execution of the micro-task trajectory instruction sequence is collected simultaneously. In a digital twin environment, based on the constructed heterogeneous robot digital twin, command-motion response mapping parameters, and spatial coupling-perturbation correction parameters, the micro-task trajectory command sequence is simulated to generate corresponding predicted motion data. For each heterogeneous robot, the difference between the actual motion data and the predicted motion data of the heterogeneous robot at each sampling time is calculated to obtain the motion error sequence; The sum of squares of the motion error sequences of all heterogeneous robots is used as the overall optimization objective. The gradient descent method is used to iteratively calculate the spatial coupling-perturbation correction parameters. In each iteration, the gradient of the overall optimization objective with respect to the spatial coupling-perturbation correction parameters is calculated, and the parameter values ​​of the spatial coupling-perturbation correction parameters are updated in the opposite direction of the gradient until the overall optimization objective is lower than the preset threshold or the maximum number of iterations is reached. The final updated parameter values ​​are used as the iteratively updated spatial coupling-perturbation correction parameters.

6. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 1, characterized in that: Based on the command-motion response mapping parameters and the updated spatial coupling-perturbation correction parameters, the cooperative task is simulated and path planning is performed in the digital twin. During the simulation, for each planned task path point of the heterogeneous robot, based on the predicted values ​​and covariance matrix output by the command-motion response mapping parameters and spatial coupling-perturbation correction parameters, the specific steps include: In a digital twin environment, simulation and path planning are performed to drive all heterogeneous robot digital twins to perform collaborative tasks based on command-motion response mapping parameters and iteratively updated spatial coupling-perturbation correction parameters. During the simulation, a task path for executing collaborative tasks is planned for each heterogeneous robot digital twin. The task path is composed of a series of planned task path points connected sequentially. For each heterogeneous robot digital twin, at each planned task path point, obtain the corresponding planning and control instructions; The planning control command is input into the command-motion response mapping parameters corresponding to the heterogeneous robot to obtain the first predicted state; Based on the state of the digital twins of other heterogeneous robots in the cluster, the first predicted state is corrected according to the spatial coupling-perturbation correction parameters to obtain the final predicted motion state value of the heterogeneous robot at the planned task path point. Simultaneously, based on the error characteristics of the command-motion response mapping parameters and the spatial coupling-perturbation correction parameters, the covariance matrix corresponding to the final predicted motion state value is calculated.

7. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 6, characterized in that: The calculated multidimensional ellipsoidal boundary, representing the uncertainty of the motion state, is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task instructions are generated. The specific steps include: Based on the final predicted motion state value and covariance matrix, a multidimensional ellipsoidal space boundary characterizing the uncertainty of the motion state of the heterogeneous robot is calculated by using a preset probability confidence level and Mahalanobis distance. The multidimensional ellipsoidal spatial boundary is defined as the multidimensional safety boundary corresponding to the planned task path point of the heterogeneous robot. Based on the task path planned for all heterogeneous robot digital twins and the multidimensional safety boundary corresponding to each planned task path point, the final planned task instruction sequence is generated.

8. The heterogeneous robot swarm cooperative control method based on digital twins according to claim 1, characterized in that: The planned task instructions are issued to the heterogeneous robots for execution, and the motion state data of the heterogeneous robots is collected. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the specific steps include: The task instruction sequence is sent to the heterogeneous robot cluster for execution, and motion state data of each heterogeneous robot is collected; The motion state data of each heterogeneous robot collected is compared with the multidimensional safety boundary of the corresponding planned task path point on the planned task path of the heterogeneous robot in the digital twin. When the comparison results determine that the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data.

9. A heterogeneous robot swarm cooperative control method based on digital twins according to claim 8, characterized in that: In the digital twin, the current actual state of the heterogeneous robot is determined based on the motion state data. Starting from this current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution. Specific steps include: Starting from the current actual state, calculate the planned task path point that has not been passed on the original task path of the heterogeneous robot and is closest to the starting point in the digital twin, and determine the geometric center point of the multidimensional safety boundary corresponding to the planned task path point as the closest safe point; In the digital twin, a smooth buffer trajectory is generated using a polynomial trajectory planning method, starting from the initial point and ending at the nearest safe point. The buffer trajectory is converted into a corresponding buffer trajectory instruction and sent to the corresponding heterogeneous robot for execution.

10. A heterogeneous robot swarm cooperative control system based on digital twins, used to implement the heterogeneous robot swarm cooperative control method based on digital twins as described in any one of claims 1-9, characterized in that, include: The instruction-response data acquisition module is used to send a predefined standardized motion excitation instruction sequence to each heterogeneous robot through the collaborative control platform when the heterogeneous robot is connected to the collaborative control platform, and simultaneously collect the motion response data of each heterogeneous robot to form an instruction-response data pair. The mapping correction parameter calculation module is used to fit the command and response data of each heterogeneous robot based on the command-response data pair using the least squares method, calculate the command-motion response mapping parameters, perform cluster robot cooperative motion simulation on the preset cooperative task in the digital twin environment, obtain spatial relative position and motion disturbance data, and calculate the spatial coupling-disturbance correction parameters through linear regression analysis. The parameter iteration update module is used to issue pre-set micro-task trajectory instructions to all heterogeneous robots and collect actual motion data before executing collaborative tasks. It calculates the error between the actual motion data and the predicted data in the digital twin based on instruction-motion response mapping parameters and spatial coupling-perturbation correction parameters. With the goal of minimizing the error, the gradient descent method is used to iteratively update the spatial coupling-perturbation correction parameters. The simulation planning safety boundary generation module is used to simulate and plan the cooperative task in the digital twin based on the command-motion response mapping parameters and the updated spatial coupling-disturbance correction parameters. During the simulation, for each planned task path point of the heterogeneous robot, the multidimensional ellipsoid boundary that represents the uncertainty of the motion state is calculated based on the predicted value and covariance matrix output by the command-motion response mapping parameters and the spatial coupling-disturbance correction parameters. This multidimensional safety boundary is defined as the multidimensional safety boundary corresponding to the task path point, and the planned task command is generated. The monitoring buffer trajectory generation module sends the planned task instructions to the heterogeneous robots for execution and collects the motion state data of the heterogeneous robots. If the motion state data of any heterogeneous robot exceeds the corresponding multidimensional safety boundary, the current actual state of the heterogeneous robot is determined in the digital twin based on the motion state data. Starting from the current actual state and ending at the nearest safe point within the multidimensional safety boundary, a buffer trajectory instruction is generated and sent to the heterogeneous robot for execution.

Images