A ros2-based heterogeneous teleoperation virtual-real collaborative control method and system

Abstract

This invention discloses a heterogeneous teleoperation virtual-real collaborative control method and system based on ROS2, belonging to the field of robot control technology. The method includes: acquiring pose data from the master-end operating device and six-dimensional force data from the slave-end manipulator; converting the pose change of the master-end device into Cartesian pose increment commands for the slave-end manipulator through an incremental mapping control strategy combined with scaling coefficients; performing inverse kinematics calculations on the pose increment commands to generate joint angle commands and performing multi-level safety state verification; synchronously sending the verified joint commands to both the real and virtual manipulators, using a time-inverse distance weighted historical state smoothing estimation algorithm for delay compensation; and converting six-dimensional force data that does not exceed the safety threshold into driving digital quantities and feeding them back to the master-end device. This invention breaks the limitation of isomorphic master-slave devices, achieving extremely low-latency virtual-real synchronous control and high-fidelity force feedback while ensuring multi-level safety protection.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically, it relates to a heterogeneous teleoperation virtual-real collaborative control method and system based on ROS2. Background Technology

[0002] In recent years, industrial robots and collaborative robots have gradually become core equipment in fields such as intelligent manufacturing, nuclear radiation environment maintenance, deep-sea operations, and space exploration. In complex and dangerous working environments, operators need to remotely control robotic arms through teleoperation devices to complete precision tasks in order to avoid the harm to human health caused by high radiation or harsh environments.

[0003] Traditional mainstream teleoperation solutions typically employ a homogeneous master-slave control architecture, meaning the master device and the slave robotic arm must have the same or highly similar degrees of freedom (DOF) configuration and linkage parameters. The control system calculates the absolute position using forward / inverse kinematics, converting the angles of each joint of the master device into corresponding joint commands for the slave robotic arm in real time. This system suffers from the following significant problems:

[0004] (1) Poor system flexibility and low adaptability. The homogeneous architecture requires strict matching of master and slave devices. Once the model or brand of the robotic arm is changed, the master device needs to be reconfigured or complex calibration work needs to be carried out, making it difficult to quickly adapt to the production line change requirements in flexible manufacturing scenarios.

[0005] (2) High communication latency and insufficient real-time performance. Most existing systems are developed based on the ROS1 architecture. Its TCPOS communication mechanism is prone to network congestion and data packet loss in high-frequency transmission scenarios of high-frequency torque and attitude data streams, making it difficult to meet the stringent requirements of precision teleoperation for low latency.

[0006] (3) The safety protection mechanism is simple and slow to respond. Most existing safety solutions are single-layer physical boundary protection. If the robotic arm collides with an obstacle due to misoperation during operation, it often cannot realize multi-level protection of software and hardware linkage in time, resulting in equipment damage or personal injury.

[0007] (4) The virtual-real joint simulation function is imperfect and the force feedback experience is poor. Existing systems often separate the simulation environment from the real robot control, making it impossible to achieve real-time mirror synchronization between the simulation and the real machine. At the same time, the force feedback function has low integration, and operators lack real tactile perception, which reduces operating efficiency and safety. Summary of the Invention

[0008] Purpose of the invention: The purpose of this invention is to address the shortcomings of existing technologies by providing a heterogeneous teleoperation virtual-real collaborative control method and system based on ROS2, which realizes high-precision, low-latency, and high-safety teleoperation control of heterogeneous devices across platforms.

[0009] Technical Solution: The heterogeneous teleoperation virtual-real collaborative control method of the present invention is applied to a teleoperation system that includes a master-end operating device, a slave-end robotic arm, and a virtual simulation environment, and includes the following steps:

[0010] The pose data of the master-end operating device and the six-dimensional force data of the end effector of the slave-end robotic arm are collected in real time.

[0011] The pose change of the master end is calculated based on the pose data, and the pose change of the master end is converted into the Cartesian pose increment command of the slave end robot arm by using an incremental mapping control strategy combined with a preset scaling factor.

[0012] The Cartesian pose increment command is subjected to inverse kinematics calculation to generate the end joint angle command. During the calculation and command issuance process, multi-level safety status verification is performed. If the verification fails, the issuance of the end joint angle command is blocked.

[0013] The joint angle command that has passed the security verification is synchronously sent to the real slave robot and the virtual robot in the virtual simulation environment. During the synchronous control process, a historical state smoothing estimation algorithm based on time inverse distance weighting is used to smooth the target position of the slave robot caused by communication delay.

[0014] Determine whether the six-dimensional force data exceeds a preset force threshold. If it does, trigger a stop command. If it does not exceed the threshold, convert the six-dimensional force data into a driving digital quantity through a mapping function and feed it back to the master operation device to perform force haptic rendering.

[0015] This invention also provides a heterogeneous teleoperation virtual-real collaborative control system based on ROS2, comprising:

[0016] The data acquisition module is used to acquire the pose data of the master operating device in real time at a first frequency, and to acquire the six-dimensional force data of the end of the slave robotic arm at a second frequency.

[0017] The mapping control module is used to calculate the master end pose change based on pose data and convert it into Cartesian pose increment instructions through an incremental mapping control strategy.

[0018] The kinematics and safety verification module is used to perform inverse kinematics calculations on incremental instructions and execute multi-level safety status verifications.

[0019] The virtual-real collaboration synchronization module is used to synchronously issue joint angle commands and uses a time-inverse distance weighted historical state smoothing estimation algorithm to perform target position smoothing compensation.

[0020] The force feedback closed-loop module is used to determine the force threshold and, if the threshold is not exceeded, convert the force data after removing the dead zone into a driving digital quantity and feed it back to the master operating device.

[0021] Beneficial effects: Compared with the prior art, the advantages of the present invention are as follows:

[0022] This invention, based on the ROS2 architecture, reduces communication latency by employing the best_effort QoS strategy for joint_states subscriptions. It combines a sliding window weighted moving average (historical window 0.3 seconds, weights correlated with the reciprocal of time) for future pose interpolation prediction and sets position change thresholds and attitude change thresholds as dead-zone filters to avoid frequent command issuance due to minor jitter. Simultaneously, it employs a safe state machine and displacement increment limits (maximum allowable displacement every 20ms is 0.05 of the rated speed), along with smooth switching based on the initial alignment trajectory. In a gigabit LAN environment, actual measurements show an end-to-end control latency of approximately 32-48ms and a position tracking error better than 1mm, significantly improving the real-time response capability and execution accuracy of precision teleoperation tasks. This effectively solves the problem of control instability caused by operational lag in long-distance or complex network environments.

[0023] Furthermore, this invention constructs a three-layer emergency stop protection mechanism, including a safety layer distance protection (5cm threshold), MoveIt high-precision obstacle avoidance planning (5mm accuracy), and end-effector contact force protection (≥30N threshold), comprehensively ensuring safety during human-machine collaboration. In particular, the safety state function progressively degrades upon detecting abnormal speed (e.g., deceleration, pause, emergency stop), and automatically recovers after confirming the robotic arm is completely stationary (below the stationary judgment threshold), greatly improving system robustness in high-frequency remote operation scenarios. Compared to existing solutions that rely solely on hardware emergency stops or single software monitoring, this system can dynamically avoid self-collisions and environmental obstacles under dual constraints of Cartesian space and joint space, and instantly triggers a soft stop upon detecting abnormal contact force, significantly reducing the risk of equipment damage or personnel injury due to misoperation or sudden environmental changes.

[0024] Through incremental mapping control strategy and coordinate axis cross-replacement logic, the system does not need to care about the absolute physical structure differences between master and slave devices. It only needs to map hand movements as increments in the slave's Cartesian space, thereby enabling master and slave devices of different brands and degrees of freedom to pair quickly and seamlessly.

[0025] This invention achieves mirror control of the simulated (Gazebo) and real robotic arm through a unified ROS control interface (such as the Cartesian pose topic ` / lbr / command / cartesian_pose`). Upper-layer nodes do not need to distinguish between virtual and real drives at the lower level. During switching, control flow is paused, the current simulated end-effector pose is read and used as the initial pose for the real robot (smoothly moving to this point via the Cartesian motion service), and control is resumed after confirmation of arrival, achieving a seamless switch. Simultaneously, force sensor data uses the same message type, and the force threshold emergency stop logic is fully reused. The overall architecture adopts a modular functional package architecture; only the lower-level driver package differs, while upper-level strategies (such as safety monitoring and collision detection) require no modification. This achieves a seamless transition from simulation verification to real robot migration, significantly shortening the on-site debugging cycle and avoiding the high costs associated with repeated trial and error in hazardous or expensive environments.

[0026] For the force feedback loop, this system not only introduces a compensation mechanism that includes the deduction of the initial zero bias, but also uses a precise digital-to-analog conversion formula (combining the device's DAC full scale, maximum output force and independent gain of each axis) to directly map the real contact force after eliminating the dead zone into the digital quantity of the motor drive, so that the operator can obtain an extremely delicate and realistic tactile perception. Attached Figure Description

[0027] Figure 1 The system architecture diagram provided for embodiments of the present invention;

[0028] Figure 2 This is a flowchart of data acquisition and processing for a touch-operated device provided in an embodiment of the present invention;

[0029] Figure 3 A safety detection flowchart for a robotic arm executing Cartesian commands provided in an embodiment of the present invention;

[0030] Figure 4 A flowchart illustrating the safety obstacle avoidance and emergency stop protection mechanism provided in this embodiment of the invention;

[0031] Figure 5 This is a flowchart of the simulation gripper control logic provided in an embodiment of the present invention;

[0032] Figure 6 This is a flowchart of the actual gripper control logic provided in an embodiment of the present invention;

[0033] Figure 7 This is a diagram illustrating the force feedback function module architecture provided in an embodiment of the present invention.

[0034] Figure 8 This is a system operation status diagram provided for an embodiment of the present invention. Detailed Implementation

[0035] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the embodiments described.

[0036] Example 1: This example is based on the Ubuntu 22.04 operating system and the ROS2 DDS distributed communication architecture, relying on OpenHaptics, KDL, MoveIt2, and Gazebo simulation libraries. The hardware environment uses a master actuator (e.g., a Geomagic Touch haptic force feedback device with six degrees of freedom), force sensors, and a real collaborative robotic arm with seven degrees of freedom as slave actuators. The system relies on the ROS2 DDS distributed communication mechanism to ensure efficient communication between nodes.

[0037] During the system deployment and initialization phase, relevant module nodes such as data acquisition, master-slave control, kinematic calculation, and force feedback rendering are started, and the simulation environment is launched. The system obtains the sensor's network communication parameters (such as IP address and port number), force threshold (such as 30.0N), position scaling factor, and safety zone boundary by loading configuration parameters to ensure the system's safety and stability during the virtual-to-real transition process.

[0038] like Figure 1 As shown, this embodiment provides a complete layered architecture for a robot teleoperation control system. The system is divided into four layers from top to bottom: the main operating terminal, the control layer, the execution layer, and the device layer, clearly presenting the closed-loop data flow and control logic of "perception-decision-execution-feedback".

[0039] Main control unit (perception layer): This unit collects multimodal operation commands through three heterogeneous input devices: GeomagicTouch for acquiring six-DOF hand pose, an ATI force sensor for acquiring force / torque information applied by the operator, and a keyboard for assisting in controlling gripping actions. This data is then integrated into a unified multimodal operation command by the main control unit's data aggregation module.

[0040] Control Layer (Decision Layer): Makes intelligent decisions based on input signals. Button1 and Button2 trigger motion control and gripping control respectively. Keyboard input can independently control the gripping. The system integrates an "incremental mapping control + safety area detection" module, which maps the tiny displacements of the tactile device to the Cartesian space motion of the robotic arm to ensure that the operation is within a safe range. At the same time, the "force threshold detection" module monitors force feedback in real time to prevent overload.

[0041] Execution layer: Invokes robot kinematics algorithm, verifies the current pose through FK (forward kinematics), solves the Cartesian target pose into joint angles through IK (inverse kinematics, such as LMA algorithm), and uses MoveIt for collision detection to ensure path safety; finally, generates "joint commands".

[0042] Equipment layer: Supports dual-mode operation, which can be used for virtual testing through the Gazebo simulation environment, or to control a real robotic arm through the FRI interface, achieving seamless transfer from simulation to physical object.

[0043] Figure 2 This demonstrates the complete workflow based on ROS 2 and DDS communication. The user inputs pose information via the Geomagic Touch device, which is converted into standard data format by the ROS 2 driver node. Subsequently, the data preprocessing module performs filtering and calibration (by setting a position change threshold (min_position_change_=0.0005 rad) and an orientation change threshold (orientation_threshold_≈0.0017 rad) as dead-zone filtering to avoid frequent command issuance due to minor hand tremors). The processed data enters the master control strategy module, generating Cartesian incremental commands, which are then published to the ROS 2 distributed network via the DDS communication layer. Finally, the slave robotic arm control node receives and converts these commands into joint-level commands, driving the real or simulated robotic arm to complete the corresponding actions, thus realizing an end-to-end control closed loop from human-machine interaction to mechanical execution.

[0044] This embodiment provides a heterogeneous teleoperation virtual-real collaborative control method, which specifically includes the following steps:

[0045] S1: Collect operator's hand motion data, including 3D position information, posture quaternions and button status, through the Geomagic Touch haptic device, and publish it to the ROS2 topic at a frequency of 100Hz. At the same time, collect force / torque data of the robotic arm end effector through a six-dimensional force sensor and publish it to the force feedback topic at a frequency of 20Hz.

[0046] S2: Based on ROS2 node subscription to Touch state data, button1 uses edge triggering to control the start / stop of robot movement, and button2 also uses edge triggering to control the opening and closing of the simulated gripper and the real gripper. At the same time, keyboard control of gripping function is set. The system sets up a four-layer safety state mechanism and force feedback threshold detection. When the force sensor data exceeds the threshold, the movement is automatically stopped. The control method adopts incremental mapping control, which converts the master end pose change into the robot arm Cartesian pose increment command through position scaling factor and attitude scaling factor.

[0047] S3: Performs forward kinematics (FK) and inverse kinematics (IK) calculations through kinematics solution nodes, converting Cartesian pose commands into 7 joint angle commands. Simultaneously, it integrates a MoveIt collision detection module, triggering an alarm and preventing joint command issuance when a collision with an obstacle is detected. Furthermore, it sets up safe zone boundary detection, triggering a warning log when the robotic arm approaches the safe boundary.

[0048] S4: Deploy a virtual-real joint control synchronization mechanism. The simulation environment loads the robotic arm model through Gazebo and configures the ROS2 Control plugin. The real robotic arm is connected through the FRI communication protocol. The real robotic arm and the simulated robotic arm subscribe to the same joint state topic to achieve mirror real-time control. The system includes a speed limiting mechanism, an emergency stop mechanism, and a static detection mechanism. At the same time, the joint command monitoring timer monitors the joint command reception status in real time.

[0049] S5: Complete system deployment and operation. Start the Touch data acquisition node, force sensor data acquisition node, force feedback rendering node, master-slave control node and kinematics calculation node in sequence through the ROS2 startup file, load the parameter configuration file, and ensure the system security and stability during the virtual-real switching process.

[0050] Specifically, step S1: multi-band asynchronous data acquisition and preprocessing.

[0051] The pose acquisition of the master device requires high real-time performance. The system uses a scheduling frequency of 100Hz (up to 1000Hz underlying hardware interrupt can be set through hdSetSchedulerRate) to call the callback function to obtain the three-dimensional position information position[x, y, z], 4×4 transformation matrix transform

[16] and button status of the master device, and publishes them to the high-frequency topics of ROS2.

[0052] S11: Acquire pose information from the main operating terminal via the Geomagic Touch haptic device, call hdInitDevice to initialize the device handle, enable hdEnable force feedback output mode, set the hdSetSchedulerRate scheduler frequency to 1000Hz, and call a callback function via a 10ms periodic timer to acquire 3D position information. The 4×4 transformation matrix transform

[16] and button states nButtons are published to the / touch / state topic;

[0053] S12: Extract quaternion pose information from the 4×4 transformation matrix. Call the extractQuaternionFromTransform function to perform matrix-to-quaternion conversion. The quaternion calculation formula is:

[0054]

[0055] Among them, when hour , Represents the elements of the transformation matrix. for Row and column indices of a homogeneous transformation matrix The intermediate scaling factor in the quaternion transformation algorithm is used to extract the quaternion. After normalization.

[0056] S13: Six-dimensional force data acquisition and conversion. Force / torque information is acquired at the end effector via a six-dimensional force sensor. The system calls the Socket interface to create a TCP client, connects to the sensor's IP address and port (49152), constructs and sends a ReadFTCommand structure containing system command words (after big-endian conversion), and then receives the ReadFTResponse response data packet. The data packet is parsed to obtain the 6-channel 16-bit raw count value. The network byte order is converted to host byte order using the `be16toh` function. To improve the realism of the system's environmental perception, this embodiment introduces a zero-point offset compensation strategy during data conversion: When the robotic arm is initially stationary and unloaded, the average of historical six-dimensional force data within a preset time window is calculated as the initial zero-bias value. During teleoperation, this initial zero-bias value is subtracted from the real-time force data, and then the data after zero-bias deduction is converted to physical units using a preset calibration coefficient. The physical unit conversion formula is as follows:

[0057]

[0058] in, , Let N and M represent the force (N) and torque (N·m) of the i-th axis, respectively. This is the converted 16-bit original count value. or The calibration coefficients for the stress / torque axes are determined by the scale_factors vector passed in during node initialization. The final generated six-dimensional force data is encapsulated as an ati_force_sensor::msg::ForceTorqueStamped message and published to the / wrench topic at a frequency of 20Hz. Step S2: Heterogeneous incremental mapping control.

[0059] In this embodiment, the heterogeneity lies in the different coordinate system definitions, motion directions, and axis correspondences between the tactile device and the robotic arm. Intuitive operation is achieved through axis swapping (e.g., mapping the Z-axis of the tactile sensor to the X-axis of the robotic arm), direction reversal, and scaling. Compared to existing typical heterogeneous solutions, this embodiment lacks advanced functions such as zero-space obstacle avoidance, anisotropic scaling, posture singularity avoidance, and bidirectional force perception mapping utilizing redundant degrees of freedom. It primarily employs static linear mapping and a safety emergency stop mechanism.

[0060] S21: Button1 uses edge triggering to control the robot's motion start / stop, and button2 uses edge triggering to control the opening and closing of the simulated gripper and the real gripper. Its edge triggering logic is as follows: The system periodically compares the button state at the current moment with the previous moment. When it detects that the button state has changed from not pressed to pressed, the system reverses the Boolean value of the underlying motion enable flag (send_pose_flag) to realize the start and stop control of the motion.

[0061] S22: Keyboard-controlled gripper function. The default key 'g' controls the opening and closing of the gripper, and the key 'q' exits the program. Non-blocking keyboard input can be configured through the checkKeyboardInput function.

[0062] For virtual environments, such as Figure 5 As shown, the simulated gripper control begins by receiving gripper opening / closing commands, setting the target position, and then starting an interpolation timer. Subsequently, it periodically updates the current position and issues gripper commands, forming a loop by judging whether the target has been reached. If the result is "no," the position continues to be updated; if the result is "yes," the status is updated and a notification is sent.

[0063] For real-world environments, such as Figure 6 As shown, the actual gripper uses asynchronous service calls. After receiving the instruction, a MoveTo service request is constructed, and then the judgment node "Is the service ready?" is entered. If it is "no", an error is reported and the process ends; if it is "yes", the asynchronous call / moveto is executed in sequence, the action start timestamp is recorded, the service callback is waited for, and finally the success / failure log of the response result is processed.

[0064] S23: Incremental mapping control, which converts the master end pose change into the robotic arm's Cartesian pose increment command through position scaling factor and attitude scaling factor. The increment calculation formula is:

[0065]

[0066] in, This is the position increment vector in the arm coordinate system; , , The three positional information of the robotic arm's coordinate system; , , The current position value of Touch; , , This represents the historical position of the previous sampling period of Touch. This is the scaling factor for movement.

[0067]

[0068] in, :Touch current gesture quaternion; The four components of the current pose quaternion for Touch are (x, y, z, three imaginary parts and one real part w).

[0069]

[0070] in, :Touch last gesture quaternion; Let x, y, z be the four components of the quaternion of the previous pose of Touch (three imaginary parts and one real part w).

[0071] Similarly, calculate the attitude increment quaternion of the master end in its own coordinate system. , for The inverse of the coordinate system is obtained, and the attitude increment quaternion in the robot arm coordinate system is obtained through the coordinate system transformation matrix. :

[0072]

[0073] in, These represent the y-axis (imaginary part), x-axis (imaginary part), z-axis (imaginary part) negative value, and real part of the master attitude increment quaternion, respectively.

[0074] Ultimately, the increment will be Superimposed onto the current actual position of the robotic arm Get the target location and target posture :

[0075] ;

[0076] .

[0077] Step S3: Inverse kinematics solution and triple collision avoidance safety boundary constraints.

[0078] like Figure 3The safety detection process for the robotic arm executing Cartesian commands is as follows: First, the spatial pose command (Cartesian command) of the robotic arm's end effector is input, followed by the FK / IK calculation stage. To address the issue of inconsistencies between the physical robotic arm's underlying drive joint sequence and kinematic chain (as seen in the KUKA series), the system forcibly cross-references specific indices (e.g., indices 2 and 3) in callbacks to ensure correct calculation. After calculation, the system proceeds to the "Is it safe?" collision detection decision stage: The MoveIt collision detection module is used to detect whether the robotic arm will collide with the environment or itself when executing the command. If the result is "no" (potential hazard exists), no joint command is issued to avoid danger; if the result is "yes" (safe), the joint command is issued to the actuator.

[0079] S31: Forward kinematics (FK) calculation, constructing the robot arm kinematics chain based on the KDL library, obtaining 7 joint angles from the / joint_states topic. The Cartesian pose of the end effector is calculated using the ChainFkSolverPos_recursive solver and published to the / lbr / state / cartesian_pose topic;

[0080] S32: Inverse Kinematics (IK) calculation. The target Cartesian pose is obtained by subscribing to the ` / lbr / command / cartesian_pose` topic. The 7-joint angles are solved using the LMA (Levenberg-Marquardt) algorithm. The A3 / A4 joint indices are automatically swapped to match the URDF model. No joint commands are issued if the IK solution fails. In `joint_position_sub_callback`, indices 2 and 3 of the joint position array are unconditionally swapped (executed every time a ` / joint_states` message is received). This is because, when used with KUKA LBR iiwa series robots, the joint order published by the ROS driver (usually sorted by name) is inconsistent with the joint order expected by the KDL kinematic chain; specifically, the index positions of physical joints A3 and A4 are reversed. By fixing the swap of elements at indices 2 and 3, the correct joint angle values ​​are ensured for subsequent forward / inverse kinematics calculations.

[0081] S33: MoveIt collision detection, listens to the / planning_scene topic and joint state through PlanningSceneMonitor, creates CollisionRequest and CollisionResult objects, calls checkCollision(req,res, robot_state) to perform collision detection, the detection configuration is req.contacts=true to get contact point information, req.distance=false to not calculate distance, the triggered action is to output collision log and prevent joint command issuance;

[0082] S34: Safe area boundary detection. The safe area boundary parameters are X[-1.5,0.44]m, Y[-1.5,1.5]m, Z[0.1,1.5]m, and the collision warning threshold is 0.05m. Detection is performed using the checkSafetyBounds function. A warning log is triggered when the robotic arm approaches the safe boundary. Figure 4 As shown, the system constructs a safety obstacle avoidance and emergency stop protection mechanism. This process is triggered by a safety timer, and after acquiring the current pose, it enters the checkSafetyBounds safety detection stage. First, it enters the first judgment node "Is the safe distance exceeded?". If "Yes", the robotic arm movement is terminated and a DANGER status signal is issued. If "No", it enters the second judgment node "Is the safe boundary approached?". If "Yes", a WARNING status signal is issued to indicate potential risks. If "No", a SAFE status signal is issued, indicating that the current movement is in a safe state. In addition, the system also includes state machine speed monitoring. When the global maximum joint angular velocity exceeds the limit, it will actively degrade and automatically recover after the speed drops back to the stationary threshold.

[0083] Step S4: Virtual-Real Collaboration and Time-Inverse Distance Smoothing Delay Compensation

[0084] In the `interpolateFuturePosition` function of `SafeSimToRealController`, the target position 0.05 seconds in the future is predicted by maintaining a 0.3-second historical window and applying time-inverse distance weighting (weight = 1 / (|Δt|+0.01)) to each joint. This effectively compensates for communication delays and sampling asynchrony between the simulation and the real machine. This algorithm is purely data-driven, with calculations involving only weighted averaging. The single prediction delay is negligible (far less than 1ms), and no system model is required. The implementation code is approximately 30 lines, significantly reducing deployment complexity. Compared to Smith predictors requiring online model identification or traditional extrapolation methods, this method has significant advantages in computational efficiency and implementation simplicity. Actual tests show that within a communication delay range of 50~150ms, the joint position prediction error can be controlled at the millimeter level, meeting the smoothness requirements of virtual-real collaborative control.

[0085] In the virtual-real joint control synchronization system, the simulation environment is based on the Gazebo Harmonic loading of the robotic arm model, and the ROS2 control interface is implemented through the libgazebo_ros2_control.so plugin; the real robotic arm communicates with the client at high frequency through the FRI protocol.

[0086] To address the communication delay and sampling asynchrony issues between the simulation and the actual machine, the system subscribes to the ` / joint_states` topic and employs a time-inverse distance weighted historical state smoothing estimation algorithm to generate the target position for the next control cycle. Let the i-th joint be in the prediction step size... The estimated position after that is :

[0087]

[0088] Wherein, the estimated position of the i-th joint at the time after compensation delay; Current system time; Predict step size or delay compensation time; : The weighted sum of all historical sample locations; Sum all data points (index j) within the historical window; Weighting coefficients; The actual measured position of the i-th joint in the j-th historical sample; : The timestamp of the j-th historical sample; Weighted summation;

[0089] Weighting coefficient Strictly determined by the reciprocal of the time distance:

[0090]

[0091] in, : represents time distance. To prevent regularization smoothing terms with a denominator of zero, data points closer to the current time have higher weights, effectively eliminating trajectory abrupt changes caused by network packet loss.

[0092] S42: Speed ​​limiting mechanism. The maximum speeds for the 7 joints are J1=1.483 rad / s, J2=1.483 rad / s, J3=1.745 rad / s, J4=1.745 rad / s, J5=2.269 rad / s, J6=2.269 rad / s, and J7=2.269 rad / s respectively. The formula for the maximum displacement limit in a single step is:

[0093]

[0094] Global conservative single-step displacement threshold, applied to instruction increment verification for all 7 joints; The minimum value of the maximum speed limit for all joints in the system; : Control command cycle, set to 0.05s (corresponding to control frequency 20Hz).

[0095] S43: Emergency stop mechanism, triggered when the joint speed exceeds 80% of the limit, and the action is to cancelAllGoals to cancel all trajectory targets and reset the safety status via the ~ / reset_safety service;

[0096] S44: To address the frequent jumps between "movement / stationary" states caused by localized jitter during high-frequency operations, this invention designs a self-recovery mechanism for a safety state machine based on the global maximum joint angular velocity. The system monitors the operating speed of the seven joints in real time and divides the safety state machine into four levels based on the speed exceeding the limit: NORMAL (normal operation), SLOWDOWN (decelerated operation), PAUSED (paused operation), and EMERGENCY (emergency stop, in which all trajectory targets are canceled). After the system enters the PAUSED or SLOWDOWN state, the control logic extracts the maximum value among all global joint angular velocities in real time. When it is determined that the global maximum joint angular velocity is continuously lower than the preset stationary judgment threshold, it is confirmed that the robotic arm has entered a completely stable stationary state. At this time, the state machine will automatically perform a safety state recovery, allowing a degrade from PAUSED to SLOWDOWN, or a recovery from SLOWDOWN to the NORMAL operating state, greatly improving the robustness of the teleoperation system.

[0097] S45: Joint command monitoring, with a monitoring cycle of 500ms, and the monitoring topic is / forward_position_controller / commands. It monitors the joint command reception status in real time and triggers a warning if no command is received within a timeout period.

[0098] like Figure 8 The practical state diagrams shown demonstrate the mirror synchronization effect of the above control method in both the real physical environment and the digital twin environment. The top row of images shows the robotic arm starting from the "initial position" and precisely responding to the operator's "left-right movement," "up-down movement," and "forward-backward movement" (3D spatial translation). The bottom row of images shows the operator controlling the posture of the Touch device's end effector, driving the robotic arm to complete "yaw control," "pitch control," and "roll control" (3D spatial rotation), as well as "gripping control" via buttons / keyboard. Throughout the process, a time-inverse distance weighted historical state smoothing estimation algorithm is used to effectively overcome motion jitter caused by network latency, achieving high smoothness and high precision heterogeneous teleoperation.

[0099] Step S5: Force feedback closed-loop mapping.

[0100] When an emergency stop is not triggered by the force threshold, six-dimensional force / torque data is obtained by subscribing to the / wrench topic via WrenchSubscriberNode. The mapForceToDAC function is then called to convert the force values ​​into DAC values ​​for the haptic device's motor. The force feedback mapping formula is as follows:

[0101]

[0102] in, Digital quantities driven by motors; Direction control item; : Amplitude mapping function; : The absolute value of the input force; Dead zone threshold.

[0103]

[0104] in, The ratio of sensor force to Touch output force; DAC full-scale value (22000 in the code); : Maximum output force of the touch device; : Independent gain of the i-th axis.

[0105] The motion automatically stops when the force sensor data exceeds the threshold. The force threshold is force_threshold_=30.0N, the detection axis is any axis, and the trigger action is to set send_pose_flag_=false to stop the motion.

[0106] like Figure 7 As shown, the system is designed with a robust force sensor callback-triggered safety control process. The process begins by acquiring real-time force feedback data, and then proceeds to the first judgment node: "Force in any direction ≥ threshold (e.g., 30.0N)?".

[0107] If the result is "no", only the force value is recorded and no emergency stop action is triggered. At the same time, the force data after removing the dead zone is combined with the mapping function (including the device DAC full scale, maximum output force and independent gain of each axis) to convert it into a driving digital quantity and feed it back to the master haptic device to provide force feedback.

[0108] If the determination is "yes", then proceed to the second judgment node: "Is the flag sent_pose_flag true?". If it is "no", it means that the system is already in a stopped state and only records the force value; if it is "yes", the system first sets send_pose_flag = false (to avoid repeated triggering), then prints a warning message and forcibly stops the movement of the robotic arm, providing hardware-level collision protection.

[0109] Example 2: Heterogeneous Teleoperation Virtual-Real Cooperative Control System

[0110] Based on the methods provided in the above embodiments, this embodiment provides a system architecture. The system is highly modular, including: a data acquisition module (responsible for reading multi-source asynchronous data and zero-bias preprocessing), a mapping control module (performing coordinate transformation, deadlock avoidance, and incremental instruction generation), a kinematics and safety verification module (continuous LMA algorithm analysis and triple anti-collision blocking mechanism), a virtual-real collaborative synchronization module (implementing DDS distributed instruction issuance, time-inverse distance weighted smoothing, and state machine anti-jitter based on global maximum speed), and a force feedback closed-loop module (performing dead zone culling and DAC mapping rendering).

[0111] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium (such as ROM, magnetic disk, optical disk, etc.).

[0112] As described above, although the invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the invention itself. Various changes in form and detail may be made without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. A heterogeneous teleoperation virtual-real collaborative control method based on ROS2, applied to a teleoperation system including a master-end operating device, a slave-end robotic arm, and a virtual simulation environment, characterized in that, Includes the following steps: The pose data of the master-end operating device and the six-dimensional force data of the end effector of the slave-end robotic arm are collected in real time. Based on the pose data, the pose change of the master end is calculated, and the pose change of the master end is converted into a Cartesian pose increment command of the slave end robot arm through an incremental mapping control strategy combined with a preset scaling factor. This includes: obtaining the spatial position and quaternion pose of the master end operating device in the current sampling period and the previous sampling period; based on the configuration difference between the master end operating device and the slave end robot arm, the coordinate axis of the spatial position change is swapped and / or the direction is reversed, and the difference between the current spatial position of the master end and the previous sampling period is scaled using a position scaling factor to obtain a position increment vector in the robot arm coordinate system; the quaternion pose change of the master end in the current sampling period and the previous sampling period is calculated using an attitude scaling factor to obtain an attitude increment quaternion in the robot arm coordinate system; the position increment vector and the attitude increment quaternion are respectively superimposed on the current position and current pose of the slave end robot arm to generate the Cartesian pose increment command. The Cartesian pose increment command is subjected to inverse kinematics calculation to generate the end joint angle command. During the calculation and command issuance process, multi-level safety status verification is performed. If the verification fails, the issuance of the end joint angle command is blocked. The joint angle command, verified by security checks, is synchronously sent to both the real slave robotic arm and the virtual robotic arm in the virtual simulation environment. During the synchronous control process, a time-inverse distance weighted historical state smoothing estimation algorithm is used to smooth the target position of the slave robotic arm caused by communication delay. This includes: acquiring a set of actual measured position samples of the slave robotic arm within a historical time window and their corresponding timestamps; calculating the time distance between the current system time plus the delay prediction step and each historical sample timestamp; using the reciprocal of the sum of the absolute value of the time distance and a preset smoothing regularization factor as the weight coefficient of each actual measured position sample; and using the weight coefficient to perform a weighted summation and normalization of all actual measured position samples within the historical time window to obtain the estimated target position of the slave robotic arm in the next control cycle after compensating for communication delay. Determine whether the six-dimensional force data exceeds a preset force threshold. If it does, trigger a stop command. If it does not exceed the threshold, convert the six-dimensional force data into a driving digital quantity through a mapping function and feed it back to the master operation device to perform force haptic rendering.

2. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 1, characterized in that, The inverse kinematics calculation of the Cartesian pose increment command includes: Listen to the joint state topic data published by the slave robot arm. When the order of the joint array published by the underlying driver is inconsistent with the expected order of the constructed kinematic chain, extract the joint state topic data and perform fixed position cross-replacement on the joint elements that have physical index inversion in order to obtain the correct current joint angle. Using the corrected current joint angle as the initial value, the LMA algorithm is used to solve for the target joint angle corresponding to the Cartesian pose increment command.

3. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 1, characterized in that, The multi-level safety state verification also includes a safety state machine self-recovery mechanism based on the global maximum joint angular velocity: The operating speed of each joint of the slave robotic arm is monitored in real time; Based on the degree to which the joint's operating speed exceeds the limit, the system's safety status is dynamically switched between four levels: normal operation, deceleration operation, paused operation, and emergency stop. When the system is in the deceleration or paused operation state, the global maximum joint angular velocity is continuously calculated. When it is determined that the global maximum joint angular velocity is continuously lower than the static determination threshold, and the robotic arm is confirmed to have entered a stable static state, the system's safe state is allowed to gradually recover to the lower limit state.

4. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 3, characterized in that, The multi-level security status verification also includes: Spatial collision avoidance verification: Path collision detection is performed using a kinematic model. If the command from the end joint angle is detected to cause the robotic arm to collide with itself or with the environment, the issuance of the command is prevented. Safety zone boundary verification: The end effector pose of the slave robot arm is acquired in real time. When it is determined that the end effector exceeds the preset safety distance, a danger status is issued and the movement is stopped; when it is determined that the end effector approaches the preset safety boundary, a warning status is issued.

5. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 1, characterized in that, The method also includes underlying communication optimization and anti-jitter mechanisms based on ROS2: Use the best_effort QoS policy to subscribe to joint state topics to reduce end-to-end communication latency; When calculating the master terminal pose change, a dead zone filtering judgment condition is set: when the position change of the master terminal pose change is less than 0.0005 rad and the attitude change is less than 0.0017 rad, it is judged as a small jitter and the cycle command is discarded.

6. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 1, characterized in that, Before performing the judgment and digital conversion, the process also includes the steps of preprocessing and zero-point offset compensation of the six-dimensional force data: when the slave robot arm is in the initial static no-load state, historical six-dimensional force data within a preset time window is collected and the average value is calculated to obtain the initial zero offset. During real-time teleoperation, the initial zero bias is subtracted from the real-time collected six-dimensional force data to obtain the compensated real contact force data; The conversion of the six-dimensional force data into a driving digital quantity through a mapping function includes: obtaining the direction information of the actual contact force data and the absolute value of its corresponding force; subtracting a preset dead zone threshold from the absolute value of the actual contact force data and inputting it into a preset amplitude mapping function for calculation to obtain a target amplitude; multiplying the direction information by the target amplitude to generate the driving digital quantity fed back to the master operating device; wherein, the amplitude mapping function is jointly determined by a preset ratio of the sensor force to the output force of the master operating device, the full-scale value of the digital-to-analog converter of the master operating device, the maximum output force threshold, and the independent gain coefficient of each axis.

7. The heterogeneous teleoperation virtual-real collaborative control method based on ROS2 according to claim 1, characterized in that, The method also supports seamless switching of control between the virtual simulation environment and the real slave robotic arm, the switching logic including: Pause the current synchronous issuance of the end joint angle command; Read the current end-effector pose of the virtual robotic arm and use it as the initial alignment pose of the real slave robotic arm; Drive the actual slave robotic arm to move smoothly to this initial alignment pose; After confirming that the actual slave robotic arm has reached the initial alignment pose, the synchronous issuance of the slave joint angle command is resumed.

8. A control system for implementing the ROS2-based heterogeneous teleoperation virtual-real collaborative control method as described in claim 1, characterized in that, include: The data acquisition module is used to acquire the pose data of the master operating device in real time at a first frequency and to acquire the six-dimensional force data of the end of the slave robotic arm at a second frequency, wherein the first frequency is higher than the second frequency. The mapping control module is used to calculate the master end pose change based on the pose data, and convert the master end pose change into the slave end robot arm's Cartesian pose increment command by combining the incremental mapping control strategy with a preset scaling factor. The kinematics and safety verification module is used to perform inverse kinematics calculation on the Cartesian pose increment command to generate the end joint angle command, and to perform multi-level safety status verification during the calculation and command issuance process. If the verification fails, the issuance of the end joint angle command is blocked. The virtual-real collaboration synchronization module is used to synchronously send the joint angle command of the slave end that has passed the security verification to the real slave end robot arm and the virtual robot arm in the virtual simulation environment. In the synchronization control process, a historical state smoothing estimation algorithm based on time inverse distance weighting is used to smoothly compensate for the target position of the slave end robot arm caused by communication delay. The force feedback closed-loop module is used to determine whether the six-dimensional force data exceeds a preset force threshold. If it does, a stop command is triggered. If it does not exceed the threshold, the six-dimensional force data after removing the dead zone is converted into a driving digital quantity through a mapping function and fed back to the master operation device to perform force haptic rendering.

Images