One-key switchable multi-mode teleoperation robotic system and method

Abstract

The application discloses a one-key switchable multi-mode teleoperation robot system and method, which comprises a three-level closed loop architecture of pose collection, signal processing and execution feedback; when a mode switching button event is triggered, a VR handle analyzes the event as a mode control instruction and encapsulates the instruction as a ROS2 message, and publishes the message to a topic / Teleop / gripper_mode; a pose_converter node subscribes to a handle pose topic, completes cross-zero jump correction, synchronization window verification and double-arm cooperative calculation, and outputs a target pose; a mode manager node subscribes to the topic / Teleop / gripper_mode, updates a state machine after obtaining a mode_id, and loads control parameters of a corresponding mode through a parameter server; the execution feedback comprises motion control and state feedback of a robot end; the expected trajectory of a mirror arm is ensured to be continuous and loop-free; and the window length is dynamically reconfigured during mode switching; the application ensures the safety and stability during mode switching, the extensible ROS parameter package of the application supports hot reloading of the same node among multiple modes without recompilation.

Description

Technical Field

[0001] This invention belongs to the field of robot VR teleoperation technology, and relates to a multi-mode teleoperation robot system and method that can be switched with one click. Background Technology

[0002] Traditional VR teleoperated robot systems mainly rely on the master operator to control the slave robot's two arms through VR controllers. However, mode switching is usually achieved through software interfaces or complex operation procedures, resulting in low operating efficiency and poor user experience.

[0003] In existing robotic dual-arm telecontrol systems, during collaborative tasks (such as moving boxes), the host computer typically plans the trajectories of the left and right arms separately, failing to fully utilize symmetry. This leads to accumulated synchronization errors, box tilting, and even box falling. Furthermore, existing mode-switching mechanisms lack adaptability to different scenarios; for example, in a box-moving scenario, they cannot quickly switch to a posture control strategy that emphasizes arm symmetry and end-effector alignment.

[0004] Chinese patent CN110587569A discloses a collaborative teleoperation control method for a dual-arm robot. In this method, a single operator at the master end manipulates two hand controllers to control the dual-arm robot at the slave end. The position, speed, and force information of the master operator's hand controllers are transmitted to the slave robot, which then coordinates its control based on the information sent from the master end. However, it does not address the dynamic switching of symmetrical collaboration between the two arms in different scenarios.

[0005] Existing technologies also have shortcomings in handling VR controller posture ambiguity and single-arm data loss tolerance, and cannot meet the real-time and security requirements of multi-mode teleoperation. Summary of the Invention

[0006] The problem solved by this invention is to provide a system and method for a multi-mode teleoperated robot that can be switched with one click. It utilizes VR controller button events to trigger dynamic reconfiguration of ROS nodes on the host computer, thereby realizing the control of joint limit and dual-arm collaborative strategy under multi-mode switching.

[0007] This invention is achieved through the following technical solution: A multi-mode teleoperated robot system with one-click switching includes a three-level closed-loop architecture of pose acquisition, signal processing, and execution feedback. The pose acquisition refers to the VR controller acquiring the operator's hand pose in real time, encapsulating it into a ROS2 message, and publishing it over the network to the controller pose topics / vr_left_pose and / vr_right_pose; When the mode switch button event is triggered, the VR controller parses it into a mode control command, encapsulates it into a ROS2 message, publishes it to the topic / Teleop / gripper_mode, and transmits it synchronously to the host computer; The signal processing includes the following ROS nodes on the host computer: The pose_converter node subscribes to the handle pose topic, completes zero-jump correction, synchronization window verification, dual-arm collaborative calculation, and outputs the target pose. The mode manager node subscribes to the topic / Teleop / gripper_mode, obtains the mode_id, updates the state machine, and loads the control parameters for the corresponding mode through the parameter server. The execution feedback includes motion control and status feedback from the robot end; The motion control involves the motion control layer node receiving the target pose information, converting the target pose into joint angle commands through inverse kinematics, and distributing them to the joint drivers on the robot via the EtherCAT bus. The joint drivers then drive the robot's two arms to track the target pose. The status feedback is the real-time status of each joint actuator fed back to the host computer via EtherCAT; Information including the robot's working scene, end-effector pose, and mode status is fed back to the VR headset via EtherCAT or wireless transmission devices.

[0008] Furthermore, the signal processing also includes a fault-tolerant processing node: real-time monitoring of the VR controller data update status; if the data delay of any controller exceeds a threshold, a single-arm missing fault tolerance strategy is triggered.

[0009] Furthermore, the configuration stored in the parameter server includes: the mapping relationship between VR controller button combinations and mode_id, and the control strategy parameters corresponding to each mode_id; When a combination of VR controller buttons triggers a mode switching event, it is mapped to the mode manager node and the pose_converter node; the hot reload architecture is used to dynamically load the corresponding strategy parameters according to the mode_id to perform mode switching.

[0010] The present invention also provides a control method for the multi-mode teleoperated robot system that can be switched with one key, characterized in that, regardless of whether the mode switching button event is triggered, the pose_converter node continuously outputs the target pose using an incremental-symmetric-window three-in-one algorithm throughout the entire teleoperation process; When switching modes, the target pose output is not interrupted. The strategy parameters of the new mode are dynamically loaded through the parameter_callback mechanism, and the robot end effector deceleration motion is constrained by velocity planning. The incremental-symmetric-window three-in-one algorithm includes the following operations: a. Receive the PoseStamped of the VR handle in the nodelet zero-copy memory area, convert it to a Quaternion, and cache it in a sliding window; b. Use cross-zero increment parsing for the Y value of the left handle: If the signs of the current frame and the previous frame are opposite and |Δy| < M, calculate delta along the continuous path; otherwise, maintain the sign direction; ; Where, is the increment or displacement change of the Y axis of the left handle, is the absolute coordinate value of the Y axis of the left handle at the current time t, is the absolute coordinate value of the Y axis of the left handle at the previous time t-1, () is the sign function, and M is the set displacement threshold; c. Mirror the Y value of the right handle in real time and output ; d. Inject ΔyL and ΔyR into the PID position loop that controls the robot's arms only when displacement events occur in both the left and right handles within the synchronization window; otherwise, maintain the output of the previous cycle; e. When the mode is switched, use S-curve speed planning.

[0011] Furthermore, cross-zero jump hardware-level correction is also performed in the ROS nodelet zero-copy memory area: Insert a sign retainer before converting PoseStamped to Quaternion: Record the sign of the z component of the previous frame's quaternion: For the quaternion of the given time q(t)=[w,x,y,z], track the sign of the z component: ; If a sign flip occurs signz(t)≠signz(t 1), and the angle difference is less than the threshold, force the reuse of the old sign: Then ; Where, is the angle between two quaternions, q (t) is the quaternion at the current time, and q (t-1) is the quaternion at the previous time; is the z component of the quaternion at the current time.

[0012] Furthermore, the pose_converter node uses a parameter_callback mechanism to dynamically capture and respond to changes in parameters in the parameter server: Within the same nodelet, the mode_id is dynamically captured using parameter_callback; First, execute the S-curve deceleration to zero force, then load the strategy parameters for the new mode.

[0013] Furthermore, the pose_converter node also employs the following control strategy: Synchronization window: The Y-symmetry value is updated only when both VR controllers have new data in the sliding window and the displacement is >0.001m; otherwise, the output of the previous cycle is maintained to prevent accidental operation with one hand. Safety Output: If the VR controller data delay exceeds 200 ms, immediately issue a zero-position flag and send the information to the motion control layer to switch to zero-force mode.

[0014] Compared with the prior art, the present invention has the following beneficial technical effects: This invention solves the VR controller posture ambiguity problem and the 180° posture jump problem that occurs when the Y-axis crosses ±π by using continuous angle difference and sign preservation, ensuring that the expected trajectory of the mirror arm is continuous and without loops; and dynamically reconfigures the window length when switching modes; ensuring the safety and smoothness of the mode switching process. For example, in the box moving mode, it implements a dual-layer safety mechanism of single-arm data missing fault tolerance + dual-arm Y-axis symmetrical real-time incremental analysis, so that the box posture transitions smoothly.

[0015] This invention establishes a 50 ms sliding window to prevent cabinet distortion caused by accidental single-handed touches. By constraining the temporal continuity and spatial consistency between touch events, it distinguishes between user-intended input events and non-intended trigger events. After synchronizing the window with dual handsets, the rate of accidental single-handed touch events decreased from 12% to 0.3%. This invention provides an extensible ROS parameter package that supports hot reloading of the same node across multiple modes based on mode_id, without the need for recompilation. New scenarios (such as other collaborative tasks) are added by injecting new limits and collaborative strategies through a yaml file, requiring zero code changes and only updating the parameter file.

[0016] The present invention has high switching efficiency: in 100 experiments with 10 subjects, the average switching time between different modes was 0.42s, which is 94% faster than the menu switching method; and it has high coordination accuracy: the box tilt angle of the box-moving task is 0.8°±0.2°, and the synchronization error is 0.9 mm±0.3 mm, which is 5 times better than the existing technology. Attached Figure Description

[0017] Figure 1This is a schematic diagram of the overall system of the present invention; it shows the VR controller, VR headset, host computer, dual-arm robot and communication link, and the button event triggering path is marked; Figure 2 This is a schematic diagram of the ROS node graph; it includes the pose_converter node, the pattern manager, and the motion control layer (motion_control_layer). Arrows indicate topic subscription / publishing. Figure 3 The pose conversion process for pose_converter is as follows: start → read handle pose → mode judgment (if mode=0: inverse motion solution; if mode=1: symmetric calculation; if mode=2: Z-unified) → cross-zero point processing → synchronization window verification → output target_pose → end; Figure 4 This is a timing diagram for a dual-hand controller synchronization window; the horizontal axis represents time, and the vertical axis represents event flags; the diagram indicates a 50ms window and that Y_sym is only updated when the counter is ≥2; the example shows that accidental single-handed touches are filtered out; Figure 5 The figure shows the velocity planning for the S-curve with asymptotic limits; the horizontal axis represents time and the vertical axis represents velocity; the figure shows the deceleration segment 200 ms before switching, and the jerk limit and angular acceleration α limit are marked. Detailed Implementation

[0018] The present invention will be further described in detail below with reference to embodiments. These descriptions are for illustrative purposes only and are not intended to limit the scope of the invention.

[0019] See Figure 1 A multi-mode teleoperated robot system that can be switched with one click is characterized by a three-level closed-loop architecture including pose acquisition, signal processing, and execution feedback. The pose acquisition refers to the VR controller acquiring the operator's hand pose in real time, encapsulating it into a ROS2 message, and publishing it over the network to the controller pose topics / vr_left_pose and / vr_right_pose; When the mode switch button event is triggered, the VR controller parses it into a mode control command, encapsulates it into a ROS2 message, publishes it to the topic / Teleop / gripper_mode, and transmits it synchronously to the host computer; The signal processing includes the following ROS nodes on the host computer: The pose_converter node subscribes to the handle pose topic, completes zero-jump correction, synchronization window verification, dual-arm collaborative calculation, and outputs the target pose. The mode manager node subscribes to the topic / Teleop / gripper_mode, updates the state machine after obtaining the mode_id, and loads the control parameters corresponding to the mode through the parameter server; The execution feedback includes the motion control and status feedback of the robot side; The motion control is that the motion control layer node receives the target pose information, converts the target pose into joint angle commands through inverse kinematics, and distributes them to the joint drivers on the robot side via the EtherCAT bus. The joint drivers drive the robot's two arms to track the target pose; The status feedback is that the real-time status of each joint driver is fed back to the host computer via EtherCAT; Information including the robot's working scenario, end pose, and mode status is fed back to the VR headset via EtherCAT or a wireless transmission device.

[0020] The configurations stored in the parameter server include: the mapping relationship between the VR handle button combinations and the mode_id, and the control strategy parameters corresponding to each mode_id; When a mode switching event is triggered by a VR handle button combination, it is mapped to the mode manager node and the pose_converter node; the corresponding policy parameters are dynamically loaded according to the mode_id using a hot reload architecture for mode switching.

[0021] See Figures 2-5 , regardless of whether the mode switching button event is triggered, the pose_converter node continuously outputs the target pose using an incremental-symmetric-window triple algorithm throughout the teleoperation process; During mode switching, the target pose output is not interrupted. The policy parameters of the new mode are dynamically loaded through the parameter_callback mechanism, and the robot end is decelerated by speed planning; The incremental-symmetric-window triple algorithm includes the following operations: a. Receive the PoseStamped of the VR handle in the nodelet zero-copy memory area, convert it to Quaternion, and cache it in a sliding window; b. Perform cross-zero increment parsing on the Y value of the left handle: If the signs of the current frame and the previous frame are opposite and |Δy| < M, calculate delta according to the continuous path; otherwise, keep the sign direction; ; Among them, is the increment or displacement change of the Y axis of the left handle, is the absolute coordinate value of the Y axis of the left handle at the current time t, This represents the absolute Y-axis coordinate value of the left handle at the previous time t-1. ) is the sign function, and M is the set displacement threshold; c. Mirror the Y-value of the right handle in real time and output. ; d. ΔyL and ΔyR are injected into the PID position loop controlling the robot's two arms only if displacement events occur in both the left and right handles within the synchronization window; otherwise, the output of the previous cycle is maintained. e. When switching modes, use S-curve speed planning.

[0022] The teleoperation of this invention is achieved through a three-level closed-loop architecture of pose acquisition (hardware acquisition), signal processing (software processing), and execution feedback: (1) Pose acquisition: VR controller data acquisition and transmission The VR controllers (Valve Index Controllers) capture the operator's hand pose (XYZ position and pose quaternions) in real time and encapsulate it into ROS2 standard messages (geometry_msgs) at a frequency of 100Hz. / PoseStamped), published over the network to the topics / vr_left_pose (left handle) and / vr_right_pose (right handle); The mode switching button event triggered by the operator (such as a long press of a combination key for 1 second) is parsed by the controller into a mode control command (Int32 type), published to the topic / Teleop / gripper_mode, and synchronously transmitted to the host computer.

[0023] (2) Signal processing: Data processing of ROS nodes on the host computer Pattern parsing: The pattern manager node subscribes to the topic / Teleop / gripper_mode, obtains the mode_id, updates the state machine, and loads the control parameters (such as symmetry tolerance and speed limit) of the corresponding pattern through the parameter server. Pose optimization: The pose_converter node subscribes to the handle pose topic, executes the incremental-symmetric-window three-in-one algorithm, completes the zero jump correction, synchronous window verification, and dual-arm collaborative calculation (such as Y-axis symmetry and Z-axis unification), and outputs the target pose; Fault-tolerant processing node: Real-time monitoring of VR controller data update status. If the data delay of any controller exceeds the threshold (e.g., timeout of 100ms), the single-arm missing fault tolerance strategy is automatically triggered to ensure operation continuity.

[0024] (3) Execution feedback: Robot end motion control and state feedback The motion control layer (ROS node, robot joint control unit deployed on the host computer) subscribes to relevant topics, converts the target pose into joint angle commands through inverse kinematics, and distributes them to each joint driver via the EtherCAT bus; The joint actuators execute closed-loop control at a 1kHz cycle to drive the robot's two arms to track the target pose; at the same time, the real-time joint status (angle, velocity, torque) is fed back to the host computer via EtherCAT to form a control closed loop. Visual feedback: Information such as the robot's working scene, end effector pose, and mode status are displayed in real time via the head-mounted display, helping the operator adjust their actions and achieve precise remote operation.

[0025] The following is a detailed explanation of each part.

[0026] VR controller and VR headset; This invention uses a VR controller and a VR headset as devices for remotely controlling a robot. The operator holds the VR controller to operate the robot and wears the VR headset to receive the feedback results. Specifically, the operator can remotely control the robot by moving the VR controller. In case of an accident, the robot can be forcibly reset by pressing the button on the VR controller.

[0027] The robot's camera transmits images to a VR headset, allowing the operator to see what the robot is seeing. Simultaneously, the VR headset controls the operator's head posture. Using built-in sensors such as an IMU, the VR headset captures real-time head posture data (represented as quaternions) and transmits this data to a host computer (industrial control computer) via network communication protocols. Upon receiving the posture data, the host computer forwards it synchronously to the robot's head motion control unit, thus enabling the robot's head to follow and synchronize with the operator's head posture in real time.

[0028] The core of this synchronization mechanism is that the operator's head posture data and the robot's head motion data are kept consistent in real time, ensuring that the operator's perspective in the VR environment is completely aligned with the robot's visual perception, thereby improving the immersion and control precision of remote operation.

[0029] Host computer: i7-12700H + Ubuntu 22.04 + ROS2 Humble; On the robot side: a dual-arm collaborative robot, only a parameterized configuration mechanism is used (only the YAML parameter file is updated), without modifying the core code, which can achieve the adaptation of the dual-arm collaborative robot, fully demonstrating the versatility advantage of the present invention; EtherCAT is a real-time industrial fieldbus technology based on Ethernet. Using EtherCAT for high-speed communication of joint actuators in dual-arm robots, it supports a communication cycle of 1 kHz to ensure the real-time performance and synchronization of motion control.

[0030] Furthermore, the high-speed communication of the joint actuators in the dual-arm robot includes two communication layers, and the communication objects of the EtherCAT bus are as follows: Core communication object: internal synchronous communication between the joint actuators of the robot; for example, EtherCAT, as a real-time industrial fieldbus, enables high-speed synchronous transmission of position, speed and torque data of 12 joint actuators (6 joints in each arm + end effector actuator), ensuring the synchronization of the joint movements of the two arms (1kHz control cycle). Indirect communication: Communication is established between the EtherCAT master station and the host computer; real-time status data of the joint actuators (such as joint angles and load torques) are aggregated by the EtherCAT master station and transmitted to the host computer via the ROS2 DDS protocol; at the same time, the target pose commands issued by the host computer are distributed to each joint actuator via the EtherCAT master station, realizing the control closed loop between the host computer and the robot.

[0031] Communication between the VR controller and the host computer: 5GHz Wi-Fi 6+ROS2 DDS FastRTPS, QoS configured as RELIABLE, DEADLINE 100 ms.

[0032] See Figure 2 The node architecture shown is a pose conversion node (pose_converter), which is a functional node set up in the ROS2 Humble framework and deployed on the host computer (i7-12700H + Ubuntu 22.04).

[0033] This node is implemented based on the ROS2 node development specification. Through ROS2's topic subscription / publishing mechanism, parameter server, and parameter_callback mechanism, it completes operations such as receiving handle pose data, loading mode strategy, dual-arm collaborative calculation, and publishing target pose. Its operation depends on the ROS2 ecosystem.

[0034] Subscriptions include: / vr_left_pose, / vr_right_pose (geometry_msgs / PoseStamped, 100Hz); / Teleop / gripper_mode (remote operation / gripping mode, Int32, real-time switching mode); Publishing: including / robot_left_target_pose, / robot_right_target_pose; the target pose for topic publishing, transmitted to the motion control layer node; The motion control layer receives the target pose command and distributes it to each joint actuator via the EtherCAT master station to drive the joints to execute the command.

[0035] Its core function is: calculate_symmetric_positions(l_pose, r_pose); Input: left and right handle poses / vr_left_pose and / vr_right_pose; Output: Target pose at the end of both arms.

[0036] The following explains the mode switching process: By triggering a mode switching event through a specific button combination on the VR controller (press and hold for 1 second), the event is directly mapped to the dynamic reconfiguration of the ROS node on the host computer, enabling seamless switching from free mode to specific scene modes (such as box-moving mode).

[0037] The host computer (ROS node) stores the configuration through the parameter server and uses a hot reload architecture to dynamically load the corresponding strategy based on mode_id.

[0038] The configuration stored on the host computer parameter server contains two core parts: the mapping relationship between VR controller button combinations and mode_id, and the control strategy parameters corresponding to each mode_id.

[0039] `parameter_callback` is a mechanism in ROS2 used to dynamically capture and respond to changes in parameters in a parameter server. In this invention, the `pose_converter` node uses it to dynamically capture changes in `mode_id` (mode ID), thereby triggering hot reloading of the mode and switching of the policy object.

[0040] Within the same nodelet: use parameter_callback to dynamically capture mode_id; The strategy is switched based on mode_id; when switching strategies, the S-curve is first executed to decelerate to zero force, and then the new strategy is injected. The process is completed in 200 ms, and the peak impact is <0.2 N·m.

[0041] The following is an example of the correspondence between the parameter server's stored configuration and scene modes: Free mode (mode_id=0): Directly reproduces the handle pose; the robot's dual-arm end effectors are independently controlled in X / Y / Z, without a cooperative strategy.

[0042] Box-moving mode (mode_id=1): Dynamically write parameters, such as maximum joint speed max_joint_vel=0.3rad / s, symmetry tolerance symm_tolerance=1 mm; In box-moving mode, enable dual-arm symmetrical control: the robot's dual-arm end effectors are mirrored symmetrically along the Y-axis (ΔyR = -ΔyL), and the X / Z axes are collinear and averaged to ensure end-effector alignment and prevent box tilting.

[0043] Horizontal interpolation mode (mode_id=2): Dynamically write parameters, such as Z-axis tolerance z_tolerance=0.5 mm, speed limit vel_limit=0.1 m / s; Axis control: Unify Z-axis averaging, X / Y keep original values ​​(independent control). Adding a new pattern store: Simply add an enumeration (e.g., mode=4:PrecisionAssembly) to the pattern manager state machine and add a YAML configuration file to the parameter server to define the strategy parameters of the new pattern (e.g., force_threshold: 0.5 N·m); VR controller control recognition: Mode switching is triggered by specific button events on the VR controller, and the events (such as pressing and holding the right controller combination button for 1 second) are directly mapped to the ROS topic / Teleop / gripper_mode (Int32 type). The pose_converter node subscribes to the mode_id of this topic to complete the hot mode reload.

[0044] Throughout the teleoperation process, the pose_converter node continuously performs the incremental-symmetric-window three-in-one algorithm calculation; the core function of the algorithm is to process the handle pose data and output the target pose of the two arm ends. Regardless of whether the mode switching state is in progress, as long as the teleoperation system is running (handle data is continuously input), the algorithm will perform cyclic calculations at a frequency of 100Hz. When switching modes, the algorithm's calculation process is not interrupted. Only the cooperative strategy parameters of the new mode are dynamically loaded through the parameter_callback mechanism (such as enabling Y-axis symmetry when mode=1 and enabling Z-axis unification when mode=2). The calculation logic (incremental parsing, synchronous window verification, and zero-crossing correction) remains consistent. During the 200ms S-curve deceleration process of mode switching, the algorithm continues to calculate the target pose, but constrains the end motion through velocity planning to ensure a smooth transition and avoid sudden attitude changes; mode switching only changes its cooperative strategy parameters and does not affect the continuity of calculation.

[0045] The incremental-symmetric-window triple algorithm receives the pose data of the left and right handles. After incremental parsing, symmetric transformation, and synchronous window verification, it calculates the target poses of the ends of the two arms, / robot_left_target_pose and / robot_right_target_pose, and then publishes them to the motion control layer (ROS node); as Figure 3 shown, it specifically includes the following operations: a. Receive the PoseStamped of the two handles in the nodelet zero-copy memory area and cache it in a 50 ms sliding window; b. To solve the pose jump problem when the Y-axis crosses 0, continuous angular difference + sign retention is proposed: Perform cross-zero incremental parsing on the Y value of the left handle: If the signs of the current frame and the previous frame are opposite and |Δy| < M, calculate delta along the continuous path; otherwise, maintain the sign direction; Among them, is the increment (displacement change) of the Y-axis of the left handle, refers to the absolute coordinate value of the Y-axis of the left handle at the current moment t, refers to the absolute coordinate value of the Y-axis of the left handle at the previous moment t - 1, ) refers to the sign function, and M is the set displacement threshold (for example, 0.05 m); c. Mirror the Y value of the right handle in real time and output ; d. As Figure 4 shown, only when displacement events occur for both the left and right handles within the window, inject ΔyL and ΔyR into the PID position loop (set in the upper computer) that controls the two arms of the robot; otherwise, maintain the output of the previous cycle; e. When switching modes, decelerate with a 200 ms S-curve. Jerk (jounce) is the third derivative of acceleration with respect to time.

[0046] Specifically, when switching modes, use S-curve velocity planning, limit Jerk to a constant ±2000 rad / s³ to ensure a smooth and impact-free switching process, with angular acceleration |α| ≤ 0.25 rad / s², and spherical interpolation of quaternions Δ ≤ arcsin(0.01 rad).

[0047] As Figure 5 shown, the transitional S-curve velocity curve that changes with time t ∈ [0, T] (where T = 0.2 s) is: Among them, v(t) is the instantaneous velocity, For initial velocity, Let T be the final velocity and T be the transition time. Jerk is restricted to: ; Angular acceleration is limited to: ; Quaternion interpolation uses spherical linear interpolation (SLERP): .

[0048] For the initial quaternion, It is the final quaternion; Furthermore, the pose_converter node also employs the following strategies: Synchronous window strategy: The Y-symmetry value is updated only when both VR controllers have new data in the sliding window and the displacement is >0.001m; otherwise, the output of the previous cycle is maintained to prevent accidental operation with one hand. Safety output strategy: If the VR controller data delay exceeds 200 ms, immediately issue a zero-position flag and send the information to the motion control layer to switch to zero-force mode.

[0049] Single-arm missing fault tolerance strategy: If any handle Topic is not updated for 100 ms, it automatically degenerates to the "single-arm drive" sub-mode: the last valid Y absolute value is used as the symmetry reference; X / Z is still averaged by both handles to ensure that the center of gravity of the box does not drift; once the missing side is recovered, it immediately and seamlessly switches back to the dual-arm incremental mode, with a change in attitude of <0.3° during the switching process.

[0050] Implement a hardware-level correction strategy across zero jumps in the ROS nodelet zero-copy memory area (latency < 0.1 ms): Insert a sign holder before converting PoseStamped to Quaternion (PoseStamped→Quaternion): PoseStamped is a standard message type in ROS used to represent timestamped poses, which includes position (XYZ coordinates) and orientation. The sign holder records the z-component sign of the quaternion from the previous frame; if the z-sign of the current frame is flipped and |Δ |<15°, forces the reuse of old symbols to prevent 180° loops in RViz; The specific operation is as follows: given a quaternion q(t) = [w, x, y, z], then track the sign of the z component: ; If a sign flip occurs (signz(t) ≠ signz(t)) 1)) and angle difference Less than the threshold: but ; in, Let q(t) be the angle between the two quaternions, q(t) be the quaternion at the current time, and q(t-1) be the quaternion at the previous time. The z-component of the quaternion at the current time; The threshold for Δθ is set to 15° (i.e., when |Δθ|<15°, it is determined to be "very small angle difference"), which is used to filter out quaternion sign flipping caused by sensor noise and prevent 180° attitude jump; Specific implementation examples are given below.

[0051] Example 1 Switching between Free Mode and Box-Moving Mode: ① Align the input quaternion with the reference frame to prevent base drift; ② Based on the current mode (switch settings with one click using the VR controller): mode=0 (Free Mode): Directly reproduces the controller pose; mode=1 (box-moving mode): Y-axis symmetry (left_y = +|y_m|, right_y = –|y_m|); X / Z collinearity (left_x = right_x = (l_x+r_x) / 2; left_z = right_z = (l_z+r_z) / 2); fixed quaternion orientation alignment; mode=2 (horizontal interpolation mode): Unify the Z-axis average, and keep the original X / Y values; ③ Zero-point processing: The Y-axis displacement of the left handle is calculated in real time, and two cases are handled: If the current frame Y value (y_L(t)) has the same sign as the previous frame Y value (y_L(t-1)), or the signs are opposite but the displacement difference |y_L(t)-y_L(t-1)| < the set threshold M (e.g., 0.05m), then the increment is calculated according to the continuous path: Δy_L = y_L(t) - y_L(t-1); If the signs are opposite and the displacement difference |y_L(t)-y_L(t-1)|≥M, then the sign direction of the previous frame is maintained, and the increment is calculated as: Δy_L = sign(y_L(t-1)). (|y_L (t)| - |y_L (t-1)|), to avoid zero-crossing transitions.

[0052] Record the sign of the z component of the quaternion in the previous frame (sign_z(t-1)). If the sign of the z component in the current frame (sign_z(t)) is flipped and the angle Δθ between the two frames of quaternions is less than 15°, then the sign of the z component in the current frame is forced to be reused as sign_z(t-1) to ensure the continuity of the attitude trajectory and prevent 180° loops from appearing during RViz visualization.

[0053] By employing a dual logic of continuous displacement increment calculation and attitude sign preservation, the problem of zero-crossing transitions is completely solved. ④ Synchronization window: The Y-symmetry value is updated only when both handles have new data within 50ms and the displacement is >0.001m; otherwise, the output of the previous cycle is maintained to prevent accidental operation by one hand.

[0054] d. Dynamic mode switching: By subscribing to / Teleop / gripper_mode via VR controller button events, hot mode reloading can be completed within 10 ms.

[0055] e. Safety Output: If the controller data timeout exceeds 200 ms, immediately issue the "zero position" flag and the motion control layer switches to zero force mode.

[0056] Progressive limit and S-curve velocity planning: After receiving a new target, the motion control layer performs "spatial constraint-collinearity" verification; in the box moving mode, if |left_z–right_z|>2 mm, the Z error is evenly distributed through the S-curve within 200 ms until the collinearity error is <0.5 mm before the clamping force is allowed to be raised.

[0057] Example 2 Switching between box-moving modes Step 1: The operator presses and holds the X, Y, A, and B buttons on the left and right handles for 1 second to trigger a one-button switch; Step 2: The mode manager state machine changes from Free=0 to BoxCoLift=1, and publishes mode_id=1; Step 3: Dynamically write control parameters (such as max_joint_vel=0.3 rad / s, symm_tolerance=1 mm, grip_force=20 N) to the parameter reconfiguration service. Step 4: Enable symmetric algorithm in motion control layer: Y-axis symmetry, X / Z-axis collinearity; inverse joint limit check; feedforward torque compensation; Step 5: If an abnormality is detected, immediately switch to Safe=99 and the joint enters zero-force mode.

[0058] Example 3 Switching between horizontal interpolation modes Step 1: The operator presses and holds the combination button on the left handle for 1 second to trigger one-key switching; Step 2: The state machine changes from Free=0 to AlignInsert=2, and publishes mode_id=2; Step 3: Dynamically write parameters (e.g., z_tolerance=0.5 mm, vel_limit=0.1 m / s); Step 4: Enable uniform averaging along the Z-axis and independent control for X / Y axes in the motion control layer to ensure end alignment; Step 5: Exception handling is the same as above.

[0059] Example 4 Exception handling details a. Communication interruption > 200 ms → Safe mode (issues reset commands to all motor joints sequentially to avoid collisions); b. Node disconnection > 50 ms → Safe mode; c. Handlebar step loss > 0.5 m → Safe mode; Exit Safe Mode: Clear fault + press and hold Menu 2 s on both controllers → return to Free Mode.

[0060] Example 5 New scenarios (such as precision assembly tasks) a. Add an enumeration to the mode manager state machine (e.g., mode=3: PrecisionAssembly); b. Add a new YAML file to the parameter server to define new policy parameters (e.g., force_threshold: 0.5 N·m); c. Register new mode callback functions in the motion control layer without modifying the underlying real-time loop; d. Supports plug-in extensions, and can be integrated with an autonomous path planning module in the future.

[0061] The mode manager state machine is the core module in the host ROS node used to manage the state of multiple mode switching. It is based on a finite state machine (FSM) design and has the following specific characteristics: Mode enumeration definition: All supported modes are predefined by enumerating them in code, including basic modes (free mode mode=0, box moving mode mode=1, horizontal insertion mode mode=2) and exception handling mode (safe mode mode=99), and support for adding new mode extensions (such as precision assembly mode=3). State switching control: Receives mode switching commands triggered by VR controller button events in real time, or automatic switching commands triggered by system abnormalities (such as network latency or node disconnection), and controls the state machine to switch between different modes in an orderly manner; mode_id publication: When the state changes, the corresponding mode_id is published to the parameter server, triggering the hot reload of the policy (re-instantiation of the policy object) of the pose_converter node. Abnormal status fallback: When abnormalities such as communication interruption >200ms, node disconnection >50ms, or handle step loss >0.5m are detected, the system automatically switches to Safe mode, driving the robot joints into zero-force mode to avoid collisions.

[0062] Example 6 Switching performance test 1) Operational scenario: Box moving mode: Box size 300 mm × 200 mm × 150 mm, mass 2 kg; Initial position: relative to the robot base coordinate system (0.6, 0, 0.1) m.

[0063] Horizontal interlocking mode: Target slot size 200 mm × 100 mm, position (0.8, 0, 0.2) m.

[0064] 2) Parameter configuration: config / pose_converter.yaml: current_orientation_mode:1; sync_window_ms:50; cross_zero_threshold:3.0rad; z_tolerance:0.5 #mm (for mode=2).

[0065] 3) Experimental procedure: a. The operator simultaneously approaches the handles of the device with both controllers in the head-mounted display. The VR system collects the poses of the left and right controllers in real time and sends them to the pose_converter node via the / Teleop / left_pose and / Teleop / right_pose topics.

[0066] b. Press and hold the combination button on the right handle for 1 second. The mode manager will automatically publish mode=1 to / Teleop / gripper_mode. The pose_converter node will receive the information and switch to "gripper mode".

[0067] c.pose_converter node pose control: Over 10 consecutive cycles, the left handle's Y-axis smoothly moved from 0.102 m to 0.100 m, and the right handle's Y-axis smoothly moved from -0.098 m to -0.100 m, with the error always less than 1 mm. By averaging the Z-axis values ​​at the ends of the two arms, the height difference is ensured to be only 0.3 mm, achieving spatial Z-axis collinearity, limiting Y-axis symmetry and X / Z-axis averaging, preventing single-handed misoperation, and ensuring synchronization and stability when the two arms clamp the box.

[0068] d. The motion control layer receives / target_left_pose and / target_right_pose published by pose_converter and initiates an S-curve to raise the box; the box tilt angle is displayed on the VR headset through real-time feedback, with a maximum tilt angle of only 0.8°, ensuring safe handling.

[0069] e. After the box is placed in the target position, the operator double-clicks the Menu button, the mode manager issues mode=0, pose_converter automatically switches back to dynamic tracking mode, and both arms resume free operation.

[0070] f. Position both handles near the slot (example of horizontal insertion mode), press and hold the combination button on the left handle for 1 second, and switch to mode=2 with one click.

[0071] g. Unified Z-axis strategy under horizontal interpolation mode (mode=2) The core of this strategy is: Unify the Z-axis averaging to ensure strict alignment of the end heights of both arms; maintain the original X / Y values ​​(independently adjustable), allowing the operator to independently control the X and Y axes using handles for fine-tuning of the plane. The goal is to achieve an alignment accuracy of <0.5 mm. Unify the Z-axis averaging, and adjust X / Y independently; ensure an alignment error of <0.5 mm by increasing the displacement increment to <0.1 m / s.

[0072] h. After completing the insertion, double-click the Menu button to switch back to free mode.

[0073] iData Acquisition.

[0074] A total of 30 handling tests were conducted, and the box did not fall off. The average tilt angle of the box was 0.6°, and the average synchronization error was 0.7 mm, which is 5 times better than the existing technology. After the dual-handle synchronization window was set up, the single-handed accidental touch event rate dropped from 12% to 0.3%.

[0075] The horizontal interpolation experiment was conducted 20 times, with an alignment success rate of 98% and an average error of 0.4 mm.

[0076] The one-click switching mechanism adapts to multiple scenarios (such as box moving and insertion). Throughout the entire process of box lifting and placement, the posture and position are constrained by the algorithm to ensure operational stability and accuracy.

[0077] This invention achieves Y-axis symmetry and X / Z-axis collinearity between the two arms, adapting to handling tasks and preventing misoperation. It supports synchronous window detection for both handles; Y-axis updates are only allowed when both handles change simultaneously, improving collaborative safety.

[0078] In 100 experiments with 10 subjects, the average switching time between different modes was 0.42 seconds, which is 94% faster than the menu switching method.

[0079] The embodiments given above are preferred examples of implementing the present invention, which fully demonstrate the practical application value and beneficial effects of the present invention in remote dual-arm collaborative handling scenarios.

[0080] This invention is not limited to the above embodiments. Any non-essential additions or substitutions made by those skilled in the art based on the technical features of the present invention shall fall within the protection scope of this invention.

Claims

1. A multi-mode teleoperated robot system with one-button switching capability, characterized in that, It includes a three-level closed-loop architecture encompassing pose acquisition, signal processing, and execution feedback; The pose acquisition refers to the VR controller acquiring the operator's hand pose in real time, encapsulating it into a ROS2 message, and publishing it over the network to the controller pose topics / vr_left_pose and / vr_right_pose; When the mode switch button event is triggered, the VR controller parses it into a mode control command, encapsulates it into a ROS2 message, publishes it to the topic / Teleop / gripper_mode, and transmits it synchronously to the host computer; The signal processing includes the following ROS nodes on the host computer: The pose_converter node subscribes to the handle pose topic, completes zero-jump correction, synchronization window verification, dual-arm collaborative calculation, and outputs the target pose. The mode manager node subscribes to the topic / Teleop / gripper_mode, obtains the mode_id, updates the state machine, and loads the control parameters for the corresponding mode through the parameter server. The execution feedback includes motion control and status feedback from the robot end; The motion control involves the motion control layer node receiving the target pose information, converting the target pose into joint angle commands through inverse kinematics, and distributing them to the joint drivers on the robot via the EtherCAT bus. The joint drivers then drive the robot's two arms to track the target pose. The status feedback is the real-time status of each joint actuator fed back to the host computer via EtherCAT; Information including the robot's working scene, end-effector pose, and mode status is fed back to the VR headset via EtherCAT or wireless transmission devices.

2. The multi-mode teleoperated robot system with one-click switching as described in claim 1, characterized in that, The signal processing also includes a fault-tolerant processing node: real-time monitoring of VR controller data update status; if the data delay of any controller exceeds a threshold, a single-arm missing fault tolerance strategy is triggered.

3. The multi-mode teleoperated robot system with one-click switching as described in claim 1, characterized in that, The configuration stored on the parameter server includes: the mapping relationship between VR controller button combinations and mode_id, and the control strategy parameters corresponding to each mode_id; When a combination of VR controller buttons triggers a mode switching event, it is mapped to the mode manager node and the pose_converter node; the hot reload architecture is used to dynamically load the corresponding strategy parameters according to the mode_id to perform mode switching.

4. The multi-mode teleoperated robot system with one-click switching as described in claim 1 or 3, characterized in that, The mode_id stored on the parameter server and the corresponding control policy parameters for the scenario include: Free mode, mode_id=0, reproduces the VR controller pose; the robot's dual-arm end effectors are independently controlled in X / Y / Z, without a dual-arm collaborative strategy; In the box-moving mode, mode_id=1, parameters including maximum joint speed and symmetry tolerance are dynamically written. The robot's dual-arm end effectors are mirrored symmetrically on the Y-axis and collinearly averaged on the X and Z axes. In the horizontal interpolation mode, mode_id=2, parameters including Z-axis tolerance and speed limit are dynamically written. The robot's dual-arm end effectors are uniformly averaged along the Z-axis, and X / Y are independently controlled. To add a new pattern, add an enumeration to the pattern manager node and add a YAML configuration file to the parameter server to define the strategy parameters of the new pattern.

5. The control method for the multi-mode teleoperated robot system with one-key switching as described in claim 1, characterized in that, Regardless of whether the mode switch button event is triggered, the pose_converter node continuously outputs the target pose during the entire teleoperation process using an incremental-symmetric-window triple algorithm; When the mode is switched, the output of the target pose is not interrupted. The policy parameters of the new mode are dynamically loaded through the parameter_callback mechanism, and the end of the robot is decelerated through velocity planning; The incremental-symmetric-window triple algorithm includes the following operations: a. Receive the PoseStamped of the VR handle in the nodelet zero-copy memory area, convert it to Quaternion, and cache it in a sliding window; b. Use cross-zero increment parsing for the Y value of the left handle: If the current frame and the previous frame have opposite signs and |Δy| < M, calculate delta according to the continuous path; otherwise, keep the sign direction; ； in, This represents the increment or displacement change of the left handle along the Y-axis. This represents the absolute Y-axis coordinate of the left handle at the current time t. This represents the absolute Y-axis coordinate value of the left handle at the previous time t-1. ) is the sign function, and M is the set displacement threshold; c. Mirror the Y-value of the right handle in real time and output. ; d. Only when displacement events occur in both the left and right handles within the synchronization window, inject ΔyL and ΔyR into the PID position loop that controls the robot's two arms; otherwise, keep the output of the previous cycle; e. When the mode is switched, use the S-curve velocity planning.

6. The control method for a multi-mode teleoperated robot system with one-key switching as described in claim 5, characterized in that, The S-curve velocity planning is as follows: The transitional S-curve velocity curve that changes with time t ∈ [0, T] is: Where v(t) is the instantaneous velocity, For initial velocity, Let T be the final velocity and T be the transition time. Jerk is restricted to: ; Angular acceleration is limited to: ; Quaternion interpolation uses spherical linear interpolation: in, For the initial quaternion, It is the final quaternion; Quaternion spherical interpolation is restricted to: .

7. The control method for a multi-mode teleoperated robot system with one-key switching as described in claim 5, characterized in that, When the pose_converter node detects that any VR handle has not been updated within the threshold time, it degrades to the single-arm drive mode: Use the absolute value of the last valid Y as the symmetry reference, and the X / Z is still averaged by the two handles; Once the missing side is restored, immediately switch back to the original mode.

8. The control method for a multi-mode teleoperated robot system with one-key switching as described in claim 5, characterized in that, Also perform cross-zero jump hardware-level correction in the ROS nodelet zero-copy memory area: Insert a sign keeper before converting PoseStamped to Quaternion: Record the sign of the z component of the previous frame's Quaternion: Given a quaternion q(t) = [w, x, y, z], track the sign of the z-component: ; If a sign flip occurs, signz(t) ≠ signz(t) 1), and the angle difference If the value is less than the threshold, force the reuse of the old symbol: but ; in, Let q(t) be the angle between the two quaternions, q(t) be the quaternion at the current time, and q(t-1) be the quaternion at the previous time. Let z be the z-component of the quaternion at the current time.

9. The control method for a multi-mode teleoperated robot system with one-key switching as described in claim 5, characterized in that, The pose_converter node uses the parameter_callback mechanism to dynamically capture and respond to changes in parameters in the parameter server: Dynamically capture mode_id within the same nodelet using parameter_callback; First execute S-curve deceleration to zero force, and then load the policy parameters of the new mode.

10. The control method for a multi-mode teleoperated robot system with one-key switching as described in claim 5, characterized in that, The pose_converter node also adopts the following control strategies: Synchronization window: Only update the Y symmetry amount when there is new data in both VR handles within the sliding window and the displacement > 0.001m; otherwise, keep the output of the previous cycle to prevent single-hand misoperation; Safe output: If the VR handle data delay timeout > 200 ms, immediately publish the zero position flag bit and send information about entering the zero-force mode to the motion control layer.

Images