Bilateral force feedback teleoperation method for humanoid robot with force compensation mechanism

Abstract

The present application relates to the technical field of humanoid robot control, and provides a bilateral force feedback teleoperation method of a humanoid robot with force compensation mechanism.The method is deployed using a 5G private network, which has the characteristics of low delay, high reliability and large bandwidth, effectively supporting real-time interaction of high-dimensional force sensation data in remote and cross-domain environments, enabling cross-domain teleoperation, effectively realizing high-fidelity feedback transmission of environmental contact force, significantly improving the precision, operation efficiency and overall safety of remote operation of the humanoid robot in complex industrial scenarios such as nuclear power maintenance, precision assembly and high-risk environment operation, bridging the gap between traditional automation and inefficient teleoperation, and providing reliable technical support for advanced industrial applications requiring human-machine collaborative fine force control.

Description

Technical Field

[0001] This invention relates to the field of humanoid robot control technology, and in particular to a method and system for bilateral force feedback teleoperation of a humanoid robot with a force compensation mechanism. Background Technology

[0002] In current industrial operations, especially in tasks involving complex, unstructured environments (such as disaster relief, space operations, and maintenance of high-risk facilities) or requiring sophisticated interaction (such as remote medical surgery), traditional solutions face significant challenges. On one hand, fully automated robotic arms, relying entirely on pre-programmed procedures, often fall short in scenarios with dynamically changing environments, uncertain target object positions and orientations, or requiring highly flexible human-machine collaboration, lacking the necessary adaptability and on-the-spot decision-making capabilities. On the other hand, while teleoperation technology allows human operators to intervene remotely, traditional teleoperation systems without force feedback or with rudimentary force feedback mechanisms have fundamental limitations. Operators cannot truly perceive the interaction forces generated by the robotic arm (actuator) in contact with the environment, essentially "picking objects from thin air." This not only leads to decreased operational accuracy and low task execution efficiency but also risks operational errors due to the lack of force perception, resulting in safety hazards and equipment damage risks.

[0003] Furthermore, even though some existing technologies attempt to incorporate force feedback, the interfering torque generated by the main operating arm's own gravity is often not effectively compensated and is mixed in with the feedback signal. This severely distorts the operator's perception of forces in the real environment, destroys the sense of force presence, and makes precise force-controlled remote operation difficult to achieve. At the same time, traditional remote operation methods that rely on limited connections or local area networks (such as WiFi) have bottlenecks in transmission distance, bandwidth stability, and low latency, limiting the realization of cross-regional, high-fidelity force feedback interaction.

[0004] Therefore, it is urgent to address the bottlenecks in the aforementioned industrial application requirements and traditional remote operation methods, and to overcome the key technical obstacles in realizing a two-sided force feedback system for humanoid robots. Summary of the Invention

[0005] This invention proposes a two-sided force feedback teleoperation method for humanoid robots with a force compensation mechanism. Deployed using a 5G private network, this method leverages its low latency, high reliability, and high bandwidth to effectively support real-time interaction of high-dimensional force sensing data in remote and cross-domain environments. It enables cross-domain teleoperation and effectively achieves high-fidelity feedback transmission of environmental contact forces. This significantly improves the accuracy, efficiency, and overall safety of remote operation of humanoid robots in complex industrial scenarios such as nuclear power plant maintenance, precision assembly, and high-risk environments. It bridges the gap between traditional automation and inefficient teleoperation, providing reliable technical support for advanced industrial applications requiring precise force control through human-machine collaboration.

[0006] This application provides a two-sided force feedback teleoperation method for a humanoid robot with a force compensation mechanism, characterized by the following steps:

[0007] S1. Construct a model of the robot end-tool's gravity in the sensor coordinate system. Using data from different postures, calculate the gravity vector G of the tool in the sensor coordinate system. tool Then, based on the real-time measurement of the six-dimensional force sensor reading F... sensor The rotation matrix R(θ) is calculated from the current attitude angle θ, and the influence of tool gravity is compensated in real time to obtain the external force F that reflects the actual contact or operation. true The information is then transmitted and sent to the host.

[0008] S2. Construct the URDF model of the master end robotic arm and establish its dynamic model. Collect the current joint angle data in real time. Based on the current pose state, calculate the gravity component in the dynamic model online and generate the full joint gravity compensation torque that completely cancels the weight of the robotic arm, so that the master end robotic arm exhibits zero gravity hovering characteristics under any configuration.

[0009] S3. Receive the external force information transmitted from the robot end, use coordinate transformation to convert the external force information based on the robot end manipulator base coordinate system into the external force information based on the operator end manipulator base coordinate system, and map the converted external force information to the joint space through Jacobian matrix transpose to generate joint feedback torque.

[0010] S4. The gravity compensation torque of the entire joint and the joint feedback torque are linearly superimposed to generate the total output torque of the robotic arm at the operating end and converted into joint drive current command. The current is then sent to the joint driver at the operating end for execution.

[0011] S5. Real-time acquisition of current joint angle data from the operating end, which is then transmitted to the robot end via a communication link to drive the robotic arm for remote operation.

[0012] Furthermore, the present invention also includes an adaptive admittance control algorithm, the steps of which include:

[0013] S1.1 The input to the adaptive admittance algorithm is the actual force error f. error When external force F true Information mutation exceeds the threshold, and the external force F before the mutation is reduced. true Information is set as expected force f target Substituting this into adaptive admittance control, the reference trajectory estimate is calculated. Environmental stiffness estimation The engineering calculation formula for adaptive admittance control is as follows:

[0014]

[0015] in To estimate the initial value, ε1 and ε2 are adaptive parameters. Refers to estimation error;

[0016] S1.2. Compliant control is applied to the reference trajectory estimate and environmental stiffness estimate, using the following formula:

[0017]

[0018] Where m is the mass coefficient, b is the damping coefficient, and k is the stiffness coefficient, all of which are constant matrices. Δb is the adaptive damping coefficient. These are the real-time velocity and acceleration values ​​transmitted by admittance control.

[0019] S1.3 The update rate of the adaptive damping coefficient Δb is as follows:

[0020]

[0021] Where δ = 10 -8 To prevent the denominator in the formula from being 0, τ is the update rate, and is the update of the admittance coefficient;

[0022] S1.4 Output the current actual motion trajectory x, and output the robot's Cartesian trajectory to each joint for execution after inverse kinematics calculation;

[0023] S1.5, automatically enter the next cycle calculation, update the admittance control output in real time, if the external force F true If the information mutation does not exceed the threshold, the admittance parameter is cleared to ensure the real-time performance of the algorithm.

[0024] Furthermore, the URDF model of the master-end robotic arm constructed in step S2 is as follows:

[0025]

[0026] Where, τ arm For the gravitational torque of the robotic arm, For the transpose of the Jacobian matrix of gravity of the robotic arm, the gravity vector is mapped to the joint space, m arm Let g be the mass of the robotic arm link, and g be the acceleration due to gravity.

[0027] Furthermore, in step S3, the nonlinear mapping function constructed based on the hyperbolic tangent function is first processed, and then the processed external force information is mapped to the joint space through the Jacobian matrix transpose to generate joint feedback torque, so that the operator can accurately perceive the dynamics of the distal interactive force through the tactile channel.

[0028] Furthermore, the formulas for mapping external forces in the same direction and generating joint feedback torques are as follows:

[0029]

[0030] Among them, F ext For the mapped external force data, F max The maximum amplitude of the external force is given by K, where K is the gain coefficient and F is the maximum amplitude of the external force. ori For external force data transmitted from the slave end, F ext The mapped external force data;

[0031]

[0032] Where τ ext F represents the external torque at each joint. ext As an external force, This is the transpose of the end Jacobian matrix based on the robot arm's base coordinate system.

[0033] Furthermore, in step S4, the calculation formula for the linear superposition of the full-joint gravity compensation torque and the joint feedback torque, generating the total output torque of the manipulator at the operating end, and converting it into joint drive current commands is as follows:

[0034]

[0035] I i =K a *τ i

[0036] Where K a τ is the current conversion coefficient; i I represents the total output torque of all joints. i This represents the total current across all joints.

[0037] Furthermore, step S1 also includes external force coordinate transformation, obtaining the true external force vector F in the sensor coordinate system after gravity compensation. true =[f x ,f y ,f z ,τ x ,τ y ,τ z ] T Then, by establishing the DH parameter model of the robotic arm, the homogeneous transformation matrix from the sensor coordinate system to the base coordinate system was calculated:

[0038]

[0039] In the above formula, Let {B} represent the rotation transformation matrix from the {B} coordinate system to the {A} coordinate system, where {B} is the base coordinate system, {S} is the sensor coordinate system, and {E} is the end coordinate system.

[0040] Furthermore, the robot terminal transmits data with the host terminal via a 5G private network.

[0041] The technical solution provided in this application has at least the following technical effects or advantages:

[0042] 1. Improve the accuracy and efficiency of complex teleoperation tasks. The method of this invention combines high-precision master-slave end-effector pose mapping with realistic force feedback, enabling operators to perform precise operations as if directly operating the remote environment. It effectively prevents excessive force, slippage, or damage caused by loss of force perception, significantly improves the success rate of a single operation, reduces trial and error and adjustment time, thereby improving overall work efficiency and task completion quality.

[0043] 2. Enhanced system safety and reduced operational risks. The real-time and accurate force feedback of this invention enables operators to promptly perceive unexpected collisions or abnormal contact forces (such as jamming or obstacles) between the arm and the environment, and to quickly react and adjust operations, effectively avoiding equipment overload damage or workpiece / environmental damage caused by a lack of force feedback. This provides crucial safety assurance for remote operation, reducing the probability of accidents and potential losses.

[0044] 3. Enhanced Realism of Force Feedback and Immediacy of Operation. This invention effectively eliminates the interference of the operator's own gravity on the feedback torque through precise main arm dynamic gravity compensation, ensuring that the environmental contact force obtained from the end-effector sensors can be mapped back to the main arm joint without distortion. The operator thus gains a highly realistic and nuanced perception of environmental forces, greatly enhancing the sense of presence and intuitive control during remote operation.

[0045] 4. Enabling wide-area, low-latency, and highly reliable cross-domain precision teleoperation. The deployment of this invention's method in a 5G private network environment is a key support for the system's breakthrough remote operation capabilities. The ultra-low latency provided by the 5G private network ensures millisecond-level synchronous transmission of commands and force feedback signals; its ultra-large bandwidth supports real-time interaction of high sampling rates, high-fidelity force sensing, and environmental data; and its ultra-high reliability and network slicing characteristics ensure the stability of data transmission and the immediate availability of critical security information in complex environments. Simultaneously, 5G's wide-area coverage completely breaks through the physical limitations of traditional wired or local area networks, enabling operators to perform precise force control operations on robots deployed in vast areas (such as the entire area of ​​a large nuclear power plant or remote disaster sites) from a safety center far from the site, truly unleashing the core value of "remote" operation. Attached Figure Description

[0046] Figure 1 This is a flowchart illustrating the bilateral force feedback teleoperation process of a humanoid robot with a force compensation mechanism according to an embodiment of the present invention.

[0047] Figure 2 This is a schematic diagram of the core framework processing flow of the method in the embodiment of the present invention.

[0048] Figure 3This is a flowchart of the adaptive admittance algorithm of the method in the embodiment of the present invention.

[0049] Figure 4 This is a flowchart illustrating the specific implementation of the robot end (slave end) of the method in this embodiment of the invention. Detailed Implementation

[0050] To better understand the above technical solutions, the following will provide a detailed explanation of the technical solutions in conjunction with the accompanying drawings and specific implementation methods.

[0051] This embodiment provides a two-sided force feedback teleoperation method for a humanoid robot with a force compensation mechanism, which is described in detail from both the robot end (slave end) and the operator end (master end). Figures 1-4 As shown:

[0052] The specific real-time process of the robot end (slave end) in this embodiment includes:

[0053] End-effector gravity compensation: An end-effector tool, such as a two-finger gripper, a five-finger dexterous hand, an electric drill, an electric screwdriver bit, or an electric saw, is attached to a six-dimensional force sensor installed at the end of the robot for operation.

[0054] During teleoperation, the gravity of the end effector itself introduces significant measurement interference. To accurately obtain the true external force acting on the end effector, gravity compensation must be applied to the sensor readings. The core of gravity compensation in this embodiment lies in establishing a model of the tool's gravity in the sensor coordinate system: the true external force F... true Equal to the sensor's original reading F sensor Subtract the projection of the tool's gravity in the current attitude, R(θ)·G tool ,Right now

[0055] F true =F sensor -R(θ)·G tool

[0056] Among them, G tool R(θ) is the gravity vector of the tool in the sensor coordinate system, and R(θ) is the rotation matrix determined by the robot's end effector attitude angle θ.

[0057] To solve for the unknown parameter G tool Before the teleoperation begins, the robot, carrying the end effector, moves sequentially to multiple preset pose points. After stabilizing at each point, the precise end effector pose and the corresponding raw reading F from the six-dimensional force sensor are recorded simultaneously. sensor Using this set of data containing different poses, the least squares estimation algorithm is used to calculate G. tool After obtaining this parameter, during subsequent teleoperation, the real-time measured F... sensorR(θ) calculated from the current attitude θ is used to compensate for the influence of tool gravity in real time, thereby obtaining F that reflects the actual contact or operational external force. true .

[0058] As a further optimization, this embodiment introduces an adaptive admittance control algorithm based on the existing teleoperation system architecture to enhance the system's operational capability in unstructured environments or environments with unknown external force interference. When the system senses excessive external contact force, it can actively adapt to the force, achieving compliant, low-impedance interactive control and significantly reducing contact impact. Simultaneously, by setting the stiffness characteristics of a virtual spring (environmental stiffness parameter K), after the external force disturbance is eliminated, the system can quickly and smoothly converge back to the target trajectory desired by the master (operating) command, restoring accurate teleoperation tracking performance, based on preset dynamic response characteristics. This avoids excessive contact stress that may be caused by sudden or continuous external forces in rigid teleoperation mode, thereby reducing the risk of potential damage to the robotic arm and the work object, and improving operational safety and adaptability. The formula for the adaptive admittance control algorithm is as follows:

[0059] S1.1 The input to the adaptive admittance algorithm is the actual force error f. error When external force F true Information mutation exceeds the threshold, and the external force F before the mutation is reduced. true Information is set as expected force f target Substituting this into adaptive admittance control, the reference trajectory estimate is calculated. Environmental stiffness estimation The engineering calculation formula for adaptive admittance control is as follows:

[0060]

[0061] in To estimate the initial value, ε1 and ε2 are adaptive parameters. Refers to estimation error;

[0062] S1.2. Compliant control is applied to the reference trajectory estimate and environmental stiffness estimate, using the following formula:

[0063]

[0064]

[0065] Where m is the mass coefficient, b is the damping coefficient, and k is the stiffness coefficient, all of which are constant matrices. Δb is the adaptive damping coefficient. These are the real-time velocity and acceleration values ​​transmitted by admittance control.

[0066] S1.3 The update rate of the adaptive damping coefficient Δb is as follows:

[0067]

[0068] Where δ = 10 -8 To prevent the denominator from being 0, τ is the update rate, and φ(t+1) is the admittance coefficient update;

[0069] S1.4 Output the current actual motion trajectory x, and output the robot's Cartesian trajectory to each joint for execution after inverse kinematics calculation;

[0070] S1.5, automatically enter the next cycle calculation, update the admittance control output in real time, when the external force F true If the information mutation does not exceed the threshold, the admittance parameter is cleared to ensure the real-time performance of the algorithm.

[0071] The true external force vector F in the sensor coordinate system is obtained after gravity compensation. true =[f x ,f y ,f z ,τ x ,τ y ,τ z ] T Subsequently, to achieve force control in the base coordinate system, coordinate system transformation needs to be implemented through the robot's kinematic model. By establishing the DH parameter model of the robotic arm, the homogeneous transformation matrix from the sensor coordinate system to the base coordinate system is calculated:

[0072]

[0073] In the above formula, Let {B} represent the rotation transformation matrix from the {B} coordinate system to the {A} coordinate system, where {B} is the base coordinate system, {S} is the sensor coordinate system, and {E} is the end coordinate system.

[0074] External force information transmission: The machine end sends the processed external force information to the operation end using a 5G private network.

[0075] The specific real-time process of the operation terminal (master terminal) in this embodiment includes:

[0076] Calculation of gravitational torque and compensation: Construct the URDF model of the master end robotic arm and establish its dynamic model. Collect the current joint angle data in real time. Based on the current pose state, the system calculates the gravity component in the dynamic model online and generates a full-joint gravity compensation torque that completely cancels out the weight of the robotic arm. This enables the master end robotic arm to exhibit zero-gravity hovering characteristics in any configuration, thereby reducing the load burden on the operator during remote operation and improving operation comfort and accuracy.

[0077]

[0078] Where τ armThis refers to the gravitational torque of the robotic arm; The transpose of the Jacobian matrix for the robot arm's gravity maps the gravity vector to joint space; m arm Let g be the mass of the robotic arm link; g is the acceleration due to gravity.

[0079] External Force Mapping: This process receives external force information transmitted from the robot and uses coordinate transformation to convert the information from the robot's end-arm base coordinate system to the operator's end-arm base coordinate system. This is processed using a nonlinear mapping function based on the hyperbolic tangent function. This function employs gain coefficients and threshold constraints to: amplify the signal in the smaller force range to improve micro-force sensing sensitivity; maintain the linear transmission characteristics of force changes in the middle range; and output saturation limiting when the external force exceeds a set threshold. This prevents the master-end robot from generating excessive driving torque that could accidentally drag the operator, ensuring the stability of the robot's operation. The processed external force is then transposed using the Jacobian matrix and mapped to the joint space to generate joint feedback torque, allowing the operator to accurately perceive the dynamics of the remote interactive force through the tactile channel. For example, the formula for external force mapping in one direction is as follows:

[0080]

[0081] Among them, F max The maximum amplitude of the external force is given by K, where K is the gain coefficient and F is the maximum amplitude of the external force. ori For external force data transmitted from the slave end, F ext This refers to the mapped external force data.

[0082]

[0083] Where τ ext F represents the external torque at each joint. ext External force; This is the transpose of the end Jacobian matrix based on the robot arm's base coordinate system.

[0084] Output torque: The gravity compensation torque and the external force feedback torque are linearly superimposed to generate the total output torque of the robotic arm at the operating end, which is then converted into joint drive current commands and sent to the joint actuators at the operating end for execution. The calculation formula is as follows:

[0085]

[0086] I i =K a *τ i

[0087] Where K a τ is the current conversion coefficient; i I represents the total output torque of all joints. i This represents the total current across all joints.

[0088] Joint angle transmission: Real-time acquisition of current joint angle data from the operating end, transmission to the robot end via communication link, driving the slave robotic arm to achieve remote operation.

[0089] In this embodiment, the master end of the teleoperation system acquires and transmits the joint angles of the robotic arm in real time via CAN communication. The slave end performs forward kinematics calculations based on the master end's DH parameter model to obtain the end-effector pose. To achieve cross-domain remote, high-precision, low-latency, and low-noise control, key data is transmitted between the master and slave ends via a 5G industrial private network. Leveraging its ultra-reliable low-latency communication and network slicing technology, geographical limitations are overcome, laying the foundation for remote real-time interaction and enabling cross-domain low-latency communication. Simultaneously, the system employs multi-level optimization to ensure performance: the master end's CAN data is filtered by Kalman filtering to suppress raw noise; the 5G private network provides an anti-interference channel to ensure reliable data delivery across domains. Due to the inconsistency in communication protocols used by the master and slave ends—a mismatch between the master end's 5ms-level low-speed data stream and the slave end's 1ms-level high-speed EtherCAT control—the system employs motion interpolation and trajectory planning co-optimization: the slave end performs high-precision spline interpolation on the received discrete target pose to generate a 1ms continuous smooth trajectory, and simultaneously plans velocity / acceleration constraint motion in the joint space based on this trajectory in real time. Meanwhile, the execution end jitter is suppressed by smoothing filtering at the slave end.

[0090] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements can be made without departing from the principle of the present invention, and these improvements should also be considered within the scope of protection of the present invention.

Claims

1. A method for bilateral force feedback teleoperation of a humanoid robot with a force compensation mechanism, characterized in that, Includes the following steps: S1. Construct a model of the robot's end-tool gravity in the sensor coordinate system, and calculate the gravity vector of the tool in the sensor coordinate system using data from different postures. Then, based on the real-time readings of the six-dimensional force sensor... and current attitude angle Calculate the rotation matrix It compensates for the influence of tool gravity in real time, obtaining external force that reflects the actual contact or operation. The information is then transmitted and sent to the host. S2. Construct the URDF model of the master end robotic arm and establish its dynamic model. Collect the current joint angle data in real time. Based on the current pose state, calculate the gravity component in the dynamic model online and generate the full joint gravity compensation torque that completely cancels the weight of the robotic arm, so that the master end robotic arm exhibits zero gravity hovering characteristics under any configuration. S3. Receive the external force information transmitted from the robot end, and use coordinate transformation to convert the external force information based on the robot end's manipulator's base coordinate system into external force information based on the operator's manipulator's base coordinate system. A nonlinear mapping function is constructed based on the hyperbolic tangent function to process the transformed external force information. The external force mapping formula in the same direction is: ； in, The mapped external force data, To maximize the amplitude of external force, This is the gain coefficient. For external force data transmitted from the slave end, The mapped external force data; The processed external force information is then mapped to the joint space via the Jacobian matrix transpose to generate the joint feedback torque, as shown in the formula: ； in These are the external torques at each joint. As an external force, This is the transpose of the end Jacobian matrix based on the robot arm's base coordinate system; S4. The gravity compensation torque of the entire joint and the joint feedback torque are linearly superimposed to generate the total output torque of the robotic arm at the operating end and converted into joint drive current command. The current is then sent to the joint driver at the operating end for execution. S5. Real-time acquisition of current joint angle data at the operating end, transmission to the robot end via communication link, driving the robot end robotic arm to achieve remote operation; It also includes the adaptive admittance control algorithm steps: S1.1 The input to the adaptive admittance algorithm is the actual force error. When external force Information mutation exceeding the threshold will trigger the external forces that preceded the mutation. Information set as expectation force Substituting this into the adaptive admittance control, the reference trajectory estimate is calculated. Environmental stiffness estimation The engineering calculation formula for adaptive admittance control is as follows: ； ； ； in , To estimate the initial value, , For adaptive parameters, Refers to estimation error; S1.

2. Compliant control is applied to the reference trajectory estimate and environmental stiffness estimate, using the following formula: ； ； ； ； ； Where m is the mass coefficient, b is the damping coefficient, and k is the stiffness coefficient; all three are constant matrices. For adaptive damping coefficient, These are the real-time velocity and acceleration values ​​transmitted by admittance control. S1.3, Adaptive Damping Coefficient The update rates are as follows: ; ； in, To prevent the denominator from being 0 in the formula, For update rate, Update the admittance coefficients; S1.4 Output the current actual motion trajectory The robot's Cartesian trajectory is then calculated using inverse kinematics and output to each joint for execution. S1.5, automatically enter the next cycle calculation, update the admittance control output in real time, if external force If the information mutation does not exceed the threshold, the admittance parameter is cleared to ensure the real-time performance of the algorithm.

2. The humanoid robot bilateral force feedback teleoperation method with force compensation mechanism according to claim 1, characterized in that: The URDF model of the master-end robotic arm constructed in step S2 is as follows: ； in, For the gravitational torque of the robotic arm, The transpose of the Jacobian matrix for the robot arm's gravity maps the gravity vector to joint space. For the mass of the robotic arm link, This is the acceleration due to gravity.

3. The humanoid robot bilateral force feedback teleoperation method with force compensation mechanism according to claim 1, characterized in that: In step S4, the calculation formula for the linear superposition of the full-joint gravity compensation torque and the joint feedback torque, generating the total output torque of the manipulator at the operating end, and converting it into joint drive current commands is as follows: in The current conversion factor; This represents the total output torque of all joints; This represents the total current across all joints.

4. The humanoid robot bilateral force feedback teleoperation method with force compensation mechanism according to claim 1, characterized in that: Step S1 further includes external force coordinate transformation, obtaining the true external force vector in the sensor coordinate system after gravity compensation. Then, by establishing the DH parameter model of the robotic arm, the homogeneous transformation matrix from the sensor coordinate system to the base coordinate system was calculated: ； In the above formula, express coordinate system to Rotation transformation matrix of the coordinate system Using the base coordinate system, For the sensor coordinate system, This is the end coordinate system.

5. The humanoid robot bilateral force feedback teleoperation method with force compensation mechanism according to claim 1, characterized in that: The robot terminal transmits data with the host terminal via a 5G private network.

Images