A control method for a somatosensory teleoperated robot system based on 5G network

Abstract

The present invention belongs to the field of robot control, and specifically discloses a control method for a somatosensory teleoperation robot system based on a 5G network, which includes: S1 obtains the operator's real-time joint angle and corresponding timestamp, and transmits them to the robot; S2 uses the real-time joint angle as the target joint angle, determines the next joint angle of the robot according to the target joint angle and the current joint angle of the robot, and stores each obtained joint angle in the robot in the form of a stack; S3 compares the airborne timestamp and the corresponding timestamp, and determines that it is a packet loss when the difference is greater than the packet loss judgment threshold; S4 switches to the airborne control mode when packet loss occurs: the average value between the data in each two stacks is taken to automatically supplement the stack, and the data in the supplemented new stack is sequentially popped out of the stack as the new joint angle of the robot. The present invention can automatically switch from the somatosensory teleoperation control mode to the airborne control mode, effectively avoiding the robot's emergency stop due to network fluctuations and causing harm to the patient.

Description

Technical Field

[0001] The present invention belongs to the field of robot control, and more specifically, relates to a control method for a somatosensory remote-operated robot system based on a 5G network. Background Art

[0002] 5G is a widely used mobile communication technology and an upgraded version of 4G mobile communication technology. 5G has the characteristics of high speed, low latency, energy saving, high bandwidth and a large number of access devices. It can effectively solve various problems caused by network limitations in the 4G era and promote industries such as industry and medical care to enter a new era of the Internet of Everything.

[0003] Traditional remote control is often based on wired connections. Although wired connections are stable, they are limited by network cables, resulting in inflexibility and cumbersome pre-preparation. The application of 5G technology to 5G can greatly improve the flexibility of equipment and reduce the workload required for pre-preparation. However, because 5G is a high-frequency short-wave signal, its characteristics are small propagation range and weak penetration. This makes it easy to lose packets when using 5G networks for remote control, making it difficult to meet the communication needs of somatosensory remote control. Summary of the invention

[0004] In response to the above defects or improvement needs of the prior art, the present invention provides a control method for a somatosensory remote-operation robot system based on a 5G network. The purpose of the method is to utilize 5G and somatosensory remote-operation technology to automatically switch from the somatosensory remote-operation control mode to the airborne control mode when packet loss occurs, thereby effectively avoiding emergency stops and injuries to the robot due to network fluctuations.

[0005] To achieve the above object, the present invention proposes a 5G network-based somatosensory teleoperation robot system control method, comprising the following steps:

[0006] S1. During the operator's activity, obtain the operator's real-time joint angles and corresponding timestamps, and transmit them to the robot;

[0007] S2, using the real-time joint angle as the target joint angle, and determining the next joint angle of the robot according to the target joint angle and the current joint angle of the robot; storing each obtained joint angle in the robot in the form of a stack, and the robot moves according to the joint angle;

[0008] S3, compare the current airborne timestamp and the corresponding timestamp of the target joint angle, and when the timestamp difference is greater than the packet loss determination threshold, determine that it is a packet loss, and enter step S4; otherwise, repeat steps S1 and S2;

[0009] S4, when packet loss occurs, switch to the onboard control mode: take the average value between the data in each two stacks and automatically add the stack, and pop the data in the new stack after addition in sequence as the new joint angle of the robot;

[0010] When the robot receives the new target joint angle transmitted before the data in the stack is completely popped out, it switches back to the somatosensory teleoperation control mode, i.e., repeats steps S1 to S3;

[0011] When all the data in the stack are popped out and the robot still has not received a new target joint angle, the robot stops at the position of the last joint angle in the stack.

[0012] As a further preference, in step S2, the next joint angle of the robot is determined, specifically by: subtracting the target joint angle from the current joint angle of the robot, and when the absolute value of the difference is less than an angle threshold, setting the target joint angle as the next joint angle of the robot; otherwise, setting the current joint angle plus or minus the angle threshold as the next joint angle of the robot.

[0013] As a further preference, each joint angle obtained is stored in the robot in the form of a stack, and when the data in the stack overflows, the data at the bottom of the stack is discarded first.

[0014] As a further preferred embodiment, the real-time joint angles and corresponding timestamps are transmitted to the robot via 5GCPE through a somatosensory teleoperation workstation.

[0015] As a further preference, in step S1, the quaternion of the operator's chest and upper arm is first measured, and then the rotation angle of each joint angle in the corresponding movement is calculated as the real-time joint angle based on the quaternion of the corresponding wearing part.

[0016] As a further preference, in step S4, when all the data in the stack are popped out and the robot still has not received the new target joint angle, the robot stops at the position of the last joint angle in the stack and pops up an error message to prompt the operator to troubleshoot after restoring communication.

[0017] As a further preference, a flexible clamping device is installed at the end of the robot's mechanical arm for throat swab sampling for nucleic acid testing.

[0018] In general, the above technical solution conceived by the present invention has the following technical advantages compared with the prior art:

[0019] 1. The present invention first determines whether packet loss occurs during the communication process, and then designs an onboard control mode to be used when packet loss occurs, so that in the event of network fluctuations, the joint angles in the onboard control mode are also controlled by the operator, which can effectively avoid misoperation and prevent the robot from stopping suddenly due to network packet loss and causing harm to the patient.

[0020] 2. The present invention uses event triggering to determine whether packet loss occurs during the communication process. Compared with existing communication methods, it adds a judgment mechanism, which can increase communication reliability. At the same time, it can enable the robot system to automatically re-access control by the operator in the event of a brief network fluctuation, thereby increasing the practicality of the control system.

[0021] 3. The present invention uses control mode switching to reduce the impact on the system when network fluctuations occur and avoid harm to patients. It can also effectively remind the operator to eliminate network failures, improve the remote control effect, and avoid the operator's repeated actions causing misoperation of the robot system.

[0022] 4. The present invention designs a method for determining the joint angle of the robot, and makes the robot's motion trajectory smoother through angle threshold judgment. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 This is a schematic diagram of the structure of a somatosensory teleoperated robot system based on a 5G network according to an embodiment of the present invention;

[0024] Figure 2 This is a flowchart of a method for controlling a somatosensory teleoperated robot system based on a 5G network according to an embodiment of the present invention. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0026] The embodiment of the present invention provides a somatosensory teleoperation robot system based on a 5G network, such as Figure 1As shown, the robot system includes a robot, a somatosensory remote operation workstation, an image workstation, an image acquisition device, and a 5G customer premise equipment (CPE); the somatosensory remote operation workstation is connected to the robot through 5G CPE communication, and the image acquisition device is used to collect the image information of the operation end in real time, and it is connected to the image workstation; the image workstation is used to display the image of the operation end in real time for the operator to judge the next operation; a flexible clamping device is installed at the end of the robot's mechanical arm, and the robot system can be used for throat swab sampling for nucleic acid testing.

[0027] The control method of the somatosensory teleoperation robot system is as follows: Figure 2 As shown, the following steps are included:

[0028] S1. Obtain the operator's position and motion posture and convert them into joint angles.

[0029] The inertial sensor is used to obtain the operator's real-time position and motion posture. Specifically, the quaternion of the chest is measured. and the quaternion of the upper arm Then the quaternion of the shoulder can be expressed as According to the quaternion of the corresponding worn part, the rotation angle of each mechanical joint angle required for the corresponding movement control output can be obtained, and the real-time joint angle and the corresponding timestamp can be transmitted to the joint motor of the mobile robot via 5G CPE.

[0030] S2, smooth motion trajectory.

[0031] The received real-time joint angle is set as the target joint angle, and then the target joint angle is subtracted from the current joint angle of the robot. When the absolute value of the difference is less than the preset angle threshold q thres , the target joint angle is set to the next joint angle of the robot; when the absolute value of the difference is greater than or equal to the angle threshold q thres When the current joint angle is added or subtracted from the angle threshold, the next joint angle of the robot is set. At the same time, each joint angle is stored in the robot in the form of a stack, and when the data in the stack overflows, the data at the bottom of the stack is discarded first.

[0032] S3. Determine whether packet loss occurs.

[0033] Constantly compare the current airborne timestamp with the target joint angle timestamp. When the absolute value of the difference between the current airborne timestamp and the target joint angle timestamp is greater than the packet loss judgment threshold t thres When the packet is lost, it is determined and the process goes to step S4; otherwise, the somatosensory teleoperation control mode is continued, that is, the robot movement is determined according to the operator's action as described above.

[0034] S4. Switch to onboard control mode when packet loss occurs.

[0035] In the onboard control mode: the data in the stack will automatically add the stack by taking the average value between every two data, and the data in the new stack after addition will be popped out in sequence as the new joint angle of the robot arm; so that the joint angle in the onboard control mode is also the joint angle controlled by the operator, which can effectively avoid misoperation.

[0036] When all the data in the stack has been popped out and the robot still has not received the new target joint angle transmitted by the somatosensory teleoperation workstation, the robotic arm will stop at the position of the last joint angle in the stack, and after communication is restored, an error message will pop up on the image workstation to prompt the operator to troubleshoot the problem, thereby avoiding the operator's repeated actions that may cause misoperation of the robot system.

[0037] When the robot receives a new target joint angle transmitted by the somatosensory teleoperation workstation before the data in the stack is completely popped out, it automatically switches back to the somatosensory teleoperation mode and continues to be controlled by the operator.

[0038] In summary, the method of the present invention stores the passed trajectory points into a stack. When packet loss occurs due to a network failure, it can automatically switch to the airborne control mode, and automatically switch back to the remote control mode when the network is restored. When the network cannot restore communication in time, the operator is actively reminded to eliminate the network failure.

[0039] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A 5G network-based somatosensory teleoperation robot system control method, characterized in that: The steps include: S1. During the operator's activity, obtain the operator's real-time joint angle and corresponding timestamp, and transmit them to the robot; S2, using the real-time joint angle as the target joint angle, and determining the next joint angle of the robot according to the target joint angle and the current joint angle of the robot; storing each obtained joint angle in the robot in the form of a stack, and the robot moves according to the joint angle; S3, compare the current airborne timestamp and the corresponding timestamp of the target joint angle, and when the timestamp difference is greater than the packet loss determination threshold, determine that it is packet loss, and enter step S4; otherwise, repeat steps S1 and S2; S4, when packet loss occurs, switch to the onboard control mode: take the average value between the data in each two stacks and automatically add the stack, and pop the data in the new stack after addition in sequence as the new joint angle of the robot; When the robot receives the new target joint angle transmitted before the data in the stack is completely popped out, it switches back to the somatosensory teleoperation control mode, i.e., repeats steps S1 to S3; When all the data in the stack are popped out and the robot still has not received a new target joint angle, the robot stops at the position of the last joint angle in the stack.

2. The method for controlling a somatosensory teleoperated robot system based on a 5G network as claimed in claim 1, characterized in that: In step S2, the next joint angle of the robot is determined, specifically: the target joint angle is subtracted from the current joint angle of the robot. When the absolute value of the difference is less than the angle threshold, the target joint angle is set as the next joint angle of the robot; otherwise, the current joint angle plus or minus the angle threshold is set as the next joint angle of the robot.

3. The method for controlling a somatosensory teleoperated robot system based on a 5G network as claimed in claim 2, characterized in that: Each joint angle obtained is stored in the robot in the form of a stack. When the data in the stack overflows, the data at the bottom of the stack is discarded first.

4. The method for controlling a somatosensory teleoperated robot system based on a 5G network as claimed in claim 1, characterized in that: The real-time joint angles and corresponding timestamps are transmitted to the robot via 5GCPE through the somatosensory teleoperation workstation.

5. The method for controlling a somatosensory teleoperated robot system based on a 5G network as claimed in claim 1, characterized in that: In step S1, the quaternion of the chest and upper arm of the operator is first measured, and then the rotation angle of each joint angle in the corresponding movement is obtained as the real-time joint angle according to the quaternion of the corresponding wearing part.

6. The method for controlling a somatosensory teleoperated robot system based on a 5G network as claimed in claim 1, characterized in that: In step S4, when all the data in the stack are popped out and the robot still does not receive the new target joint angle, the robot stops at the position of the last joint angle in the stack and pops up an error message to prompt the operator to troubleshoot after communication is restored.

7. The method for controlling a somatosensory teleoperated robot system based on a 5G network according to any one of claims 1 to 6, characterized in that: A flexible clamping device is installed at the end of the robot's mechanical arm for throat swab sampling for nucleic acid testing.

Images