Remote ultrasonic robot force feedback teleoperation device and control method

Abstract

The application relates to the technical field of medical ultrasonic examination, and provides a force feedback teleoperation device and a control method for remote ultrasonic robot examination, which comprises a series posture mechanism assembly, a parallel position mechanism assembly, a base support assembly and a control driving assembly; the movement of the series posture mechanism assembly corresponds to the deflection axis X, the pitch axis Y and the roll axis Z axis posture of a robot ultrasonic probe, and the series posture mechanism assembly feeds back the contact torque of the X axis direction of the robot ultrasonic probe; the movement of the parallel position mechanism assembly corresponds to the X, Y and Z axis positions of the robot ultrasonic probe, and the parallel position mechanism assembly feeds back the contact force of the X, Y and Z axis directions of the robot ultrasonic probe; the position and posture master-slave teleoperation control method and the contact force slave master feedback control method are adopted, and the transparency and the sense of presence of a doctor operator are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of medical ultrasonic examination, and in particular to a force feedback teleoperation device and control method for remote ultrasonic robot examination. BACKGROUND

[0002] Ultrasound examination is a medical imaging diagnostic technique that uses ultrasonic medium to examine the size, shape and internal echoes of internal organs of the human body, and is widely used for examination and diagnosis of various diseases. Ultrasound examination has very high requirements for the skills of the operator, and high-quality ultrasound images can be obtained with superb skills and rich experience. Through continuous sliding scanning, fixed-point side scanning, cross scanning and other moving operations of the ultrasonic probe, i.e. the combination of “sliding”, “turning”, “shaking” and “tilting” movements of the ultrasonic probe, the transverse, longitudinal and oblique sections of the examination site can be obtained. Remote ultrasonic robots are medical examination equipment that uses robot technology, communication technology and ultrasonic examination technology, and can realize ultrasonic examination of remote personnel. Doctors can remotely operate the ultrasonic robot to perform examination and diagnosis, realize sharing of high-quality medical resources, and alleviate the problem of uneven distribution of medical resources in remote areas.

[0003] The existing remote ultrasonic robot adopts the following technical solution: the slave end uses a mechanical arm with a force sensor to drive the ultrasonic probe to move flexibly for examination, the master end uses a teleoperation device to control the movement of the mechanical arm, and the communication technology is used to realize the transmission of control data and audio and video data between the master end and the slave end.

[0004] The existing master end teleoperation adopts the following technical solution: for example, application No. CN201821392527.3, the teleoperation device (hereinafter referred to as the information acquisition module) is the integrated application of a mature commercial force feedback device Touch; for example, application No. CN201710770294.X, the teleoperation device (hereinafter referred to as the controller) is the integrated application of a mature commercial force feedback device Omega.3; for example, application No. CN201810615912.8, the teleoperation device (hereinafter referred to as the operation hand device) uses a posture sensor and a position sensor to acquire the posture and position of the master end, and uses a force sensor to control the contact force of the ultrasonic probe of the slave end mechanical arm; for example, application No. CN202111199054.1, the teleoperation device (hereinafter referred to as the handheld device) uses a posture angular velocity sensor, an optical speed sensor and a pressure sensor to acquire the speed, angular velocity and pressing force, the actual contact force of the slave end is fed back through the display screen of the master end, and different intensity vibrations are fed back according to the difference between the contact force and the pressing force.

[0005] In summary, the existing remote ultrasound robot teleoperation device mainly has the following problems: the commercial mature force feedback device is not developed for the remote ultrasound application scenario and does not conform to the operation and use habits of doctors; the contact type movement of the position sensor can only provide plane position and speed information and cannot obtain height direction movement information; the actual contact force feedback of the slave end is displayed through a display screen or is fed back through vibration intensity, and doctors cannot perceive the real contact force of the slave end probe. SUMMARY

[0006] In order to solve the problems in the prior art, realize accurate feedback of contact force and torque, and improve the transparency and sense of presence of doctors using robots for teleoperation inspection, the application provides a remote ultrasound robot force feedback teleoperation device and a control method. The specific technical scheme is as follows:

[0007] A remote ultrasound robot force feedback teleoperation device is composed of a series attitude mechanism assembly, a parallel position mechanism assembly, a base support assembly and a control driving assembly; the series attitude mechanism assembly is connected with the parallel position mechanism assembly through a pitch pin shaft; the parallel position mechanism assembly is fixedly installed on a plane of the base support assembly; the control driving assembly is installed inside the base support assembly; the movement of the series attitude mechanism assembly corresponds to the deflection shaft X, the pitch shaft Y and the roll shaft Z attitude of a robot ultrasound probe and feeds back the contact torque of the robot ultrasound probe in the X axis direction; the movement of the parallel position mechanism assembly corresponds to the X, Y and Z axis positions of the robot ultrasound probe and feeds back the contact force in the X, Y and Z axis directions of the robot ultrasound probe.

[0008] The series attitude mechanism assembly is composed of a profiled handle, a deflection motor, a roll ring and a pitch ring; the profiled handle is internally integrated with an angular velocity sensor and a gravity sensor; the deflection motor is coaxially fixedly installed with the roll ring, the motor shaft is connected with the profiled handle, the deflection motor is provided with an encoder, and the encoder value corresponds to the deflection angle of the profiled handle; the roll ring is inside the pitch ring and is connected through a roll pin shaft; the pitch ring is inside a translation fixed disc and is connected through a pitch pin shaft; the motor shaft, the pitch pin shaft and the roll pin shaft are perpendicular to each other in any attitude in space and intersect at a point.

[0009] The functions and features of the series posture mechanism assembly are as follows: the motor shaft, the pitch pin shaft and the roll pin shaft correspond to the deflection axis X, the pitch axis Y and the roll axis Z of the space posture of the profiling handle respectively, the space posture is synchronous with the mapping of the posture of the ultrasonic probe of the remote ultrasonic robot mechanical arm, and the posture control of the ultrasonic probe by the profiling handle is realized; the angular velocity sensor integrated in the profiling handle can detect the angular velocity of the profiling handle around the deflection axis X, the pitch axis Y and the roll axis Z, and the relative deflection angle, the relative pitch angle and the relative roll angle are obtained through the angular velocity integration algorithm of the control driving assembly respectively; the gravity sensor integrated in the profiling handle can detect the absolute pitch angle and the absolute roll angle of the profiling handle, and is used for the vertical initial zero position calibration of the profiling handle; the deflection motor encoder can detect the absolute deflection angle of the profiling handle, and is used for aligning the roll angle of the ultrasonic probe; the deflection motor provides the torque in the direction of the deflection axis X of the main end profiling handle, and realizes the torque feedback of the ultrasonic probe in the direction of the deflection axis X. The series posture mechanism assembly can realize the detection of the space posture deflection angle, the pitch angle and the roll angle by the teleoperation device, realizes the "turn", "shake" and "tilt" actions of the remote ultrasonic probe, and feeds back the contact torque of the "turn" action of the remote ultrasonic probe. Since the contact torque of the "shake" and "tilt" actions is small, in order to reduce the system complexity, the torque feedback is not set in the direction of the pitch axis Y and the roll axis Z.

[0010] The parallel position mechanism assembly is composed of linear motion assemblies, parallel link assemblies and translational fixed disc assemblies; the number of linear motion assemblies is 3, which are arranged horizontally on the mounting surface of the base support assembly, and the mutual position included angle is 120° horizontally; the number of parallel link assemblies is 3, which are hingedly connected with the linear motion assemblies respectively; the translational fixed disc assemblies are hingedly connected with the three parallel link assemblies, and the spatial position degrees of freedom are the front and back Z axis direction, the left and right Y axis direction and the up and down X axis direction.

[0011] Further, the linear motion assembly is composed of a linear guide rail, a slider assembly, a flexible transmission body, a speed reduction motor, a tensioning assembly, a transmission wheel and a guide rail seat; the flexible transmission body is a synchronous belt or a steel wire rope, and the transmission wheel is a synchronous pulley or a rope wheel; the linear guide rail is fixedly installed on the guide rail seat, and the slider assembly is freely movable on the linear guide rail; the output shaft of the speed reduction motor is fixedly connected with the transmission wheel, drives the flexible transmission body, drives the slider assembly to move, and records the moving position through the encoder; the flexible transmission body passes around the transmission wheel and the tensioning assembly at both ends, and is fixed on the slider assembly; the tensioning assembly adjusts the tightness of the flexible transmission body through a tensioning screw; the slider assembly is provided with a slider bearing seat, and two bearing end covers are respectively positioned on the outer rings of two bearings, and the axial gap of the bearing is adjusted through end cover gaskets.

[0012] Further, the parallel link assembly is composed of 2 links, 4 link pin shafts, 4 link hinge heads and 2 link rotation shafts, and has a parallelogram structure; the link hinge heads are installed at the two ends of the link, and are hingedly connected with the link rotation shafts through the link pin shafts; the link rotation shafts are hingedly connected with the slider bearing seats on the slider assembly, and the other link rotation shafts are hingedly connected with the translation bearing seats on the translation fixed disc assembly.

[0013] Further, the translation fixed disc assembly is composed of a translation fixed disc and translation bearing seats; the translation fixed disc has a hollow annular structure in the middle, and is provided with a pitch shaft hole in the diameter direction; the three translation bearing seats are uniformly fixed and installed on the circumference of the translation fixed disc, and have a horizontal angle of 120° with each other, and the bearing seats are provided with end covers on the two sides for positioning the two bearing outer rings, and the axial clearance of the bearing is adjusted through the end cover gaskets.

[0014] The parallel position mechanism assembly has the following functions and characteristics: the three-degree-of-freedom parallel mechanism composed of the linear motion assembly, the parallel link assembly and the translation fixed disc assembly can realize the position movement of the profiling handle in the horizontal space Z and Y directions, corresponding to the forward and backward and left and right sliding actions of the ultrasonic probe on the human body surface. The position movement of the profiling handle in the vertical space X direction corresponds to the depth of the ultrasonic probe pressing the human body surface. The end adopts a mechanical arm with a force sensor to sense the interaction force between the ultrasonic probe and the human body, and feeds back the contact force information in the Cartesian space X, Y and Z directions to the master remote control device, and the contact force is calculated to the joint space of the parallel position mechanism through the control driving assembly, so that the interaction force feedback can be generated by controlling the driving current of the reduction motor. Through the compensation algorithm of the driving control assembly, the gravity, friction and inertia force of the remote control device can be compensated, and the influence of other external forces on the interaction force feedback can be eliminated. Since the ultrasonic examination speed is small, the inertia force can be ignored.

[0015] The base support assembly is composed of a mounting surface and base legs, and the mounting surface is provided with a positioning structure of the linear motion assembly, and the height of the base legs is greater than the height of the reduction motor of the linear motion assembly.

[0016] The control driving assembly comprises a motor driver and a remote control controller; the motor driver controls the driving current to realize real-time force and torque feedback, and acquires the motor code position data; the remote control controller realizes the function of force feedback remote control through control logic and control algorithm according to the input of the encoder information, force feedback information and other parameters.

[0017] The remote ultrasonic robot force feedback remote control method of the application comprises a position and attitude master-slave remote control method and a contact force slave-master feedback control method.

[0018] The position and attitude master-slave remote control method is as follows:

[0019] S1, the end mechanical arm moves to the position directly above the to-be-inspected part, the ultrasound probe is vertical and maintains an initial safe height from the human body surface, taking the mechanical arm base coordinate system as the reference, the initial position and attitude coordinates of the ultrasound probe are

[0020] S2, the teleoperation device is started, and the parallel position mechanism assembly drives the profiling handle back to the initial position, taking the base support assembly coordinate system as the reference, the initial position coordinate matrix of the profiling handle is C P M ;

[0021] S3, according to the initial attitude of the ultrasound probe, the teleoperation device deflection motor drives the profiling handle deflection shaft X to coincide with the ultrasound probe deflection shaft X;

[0022] S4, the doctor vertically rightens the profiling handle under the auxiliary indication of the profiling handle gravity sensor, and sets the initial attitude angle of the pitch shaft Y and the roll shaft Z, taking the base support assembly coordinate system as the reference, the initial position and attitude coordinates of the profiling handle are

[0023] S5, the ultrasound probe coordinate system S x,y,z is aligned with the profiling handle coordinate system M x,y,z in the direction of the coordinate axis, so that the attitude matrix of the profiling handle and the ultrasound probe coordinate systems is equal B R S = C R M ;

[0024] S6, the doctor operates the profiling handle to start teleoperation, based on the set interaction frequency, at the next cycle time, the controller drives the assembly to obtain the parallel position mechanism assembly encoder data, through the kinematics forward solution algorithm, the new position matrix of the profiling handle is obtained C P M ′M; the serial attitude mechanism assembly angular velocity sensor data is obtained, through angular velocity integration and kinematics forward solution algorithm, the new attitude matrix of the profiling handle is obtained C R′ M ;

[0025] S7, according to the position mapping ratio k, the position increment matrix k( C P M ′M- C P M ) and the attitude increment matrix C R′ M - C R M of the ultrasound probe are calculated;

[0026] S8, the teleoperation device sends control data to the slave robot arm, drives the ultrasound probe to the target position and pose, and the target pose coordinates are

[0027] S9, based on the set interaction frequency, continuously obtain the master teleoperation device data, calculate the pose coordinates of the profiling handle, and continuously send the target pose coordinates to the slave robot arm, so as to realize the master-slave teleoperation follow-up control of the position and pose of the ultrasound probe.

[0028] The contact force from the master feedback control method is as follows:

[0029] S1, the doctor operates the profiling handle, obtains the motor encoder and angular velocity sensor data, and controls the slave robot arm through integral algorithm, forward kinematics algorithm and proportional mapping master-slave control strategy, so as to realize the master-slave teleoperation control of the position and pose of the ultrasound probe; the ultrasound probe contacts the surface of the body of the person to be detected, and starts the combination of "sliding", "turning", "shaking" and "tilting" actions for ultrasound examination;

[0030] S2, the slave robot arm obtains the contact force and torque by measuring the force sensor data with the robot arm base coordinate system as the reference, and the contact force and torque are

[0031] B F S = e , e F T = ex , ey F ez = ex , ey T ez , T ;

[0032] S3, through the slave master force feedback control strategy such as coordinate system conversion and proportional mapping, the target force and torque of the master profiling handle are obtained with the base support component coordinate system as the reference, and the target force and torque of the master profiling handle are

[0033] C F M = ek , ek F T = ex , ey F ez = ex , T 0, 0 ;

[0034] S4, through the master end force feedback teleoperation device Jacobian matrix J transpose operation, the master end joint torque is obtained

[0035] τ ek = JTC F M = [τ1, τ2, τ3, τ4, 0, 0] T ;

[0036] S5, calculate the gravity compensation torque τ g and the friction compensation torque τ f superimposed on each joint torque, through the torque constant calculation of the motor driver, get each motor drive current I e ;

[0037] S6, each motor generates torque τ e , through the series pose mechanism assembly and parallel position mechanism assembly, the contact force and torque are fed back to the physician operator through the master end profiling handle.

[0038] The beneficial effects of the present application are:

[0039] The force feedback teleoperation device has four degrees of freedom force feedback, which meets the force feedback requirements of the "slide", "turn", "shake" and "tilt" actions of the ultrasonic probe;

[0040] The force feedback teleoperation device uses a combination of parallel mechanism and series mechanism, which increases the rigidity of the mechanism, reduces the overall motion inertia, decouples the position control and attitude control, and reduces the control calculation complexity;

[0041] The force feedback teleoperation device uses gravity compensation and friction compensation method, which reduces the interference of external force, reduces the fatigue feeling of operation, improves the transparency and immediacy of the physician operator. BRIEF DESCRIPTION OF DRAWINGS

[0042] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiment or prior art description will be briefly introduced below, and obviously, other drawings can also be obtained by those skilled in the art without creative labor on the basis of these drawings;

[0043] Figure 1 : Axonometric view of force feedback teleoperation device;

[0044] Figure 2 : Top view of force feedback teleoperation device;

[0045] Figure 3 : Structure diagram of series pose mechanism assembly;

[0046] Figure 4 : Structure diagram of parallel position mechanism assembly;

[0047] Figure 5 : Structure diagram of linear motion assembly;

[0048] Figure 6 : slider assembly cross-sectional view

[0049] Figure 7 : parallel link assembly structure diagram

[0050] Figure 8 : contact force from master feedback control flow chart

[0051] Figure 9 : position, attitude master-slave teleoperation servo control flow chart

[0052] Figure label description: 1. Series attitude mechanism assembly; 10. Profiling handle; 11. Deflection motor; 12. Rolling ring; 121. Rolling pin; 13. Pitch ring; 131. Pitch pin; 2. Parallel position mechanism assembly; 21. Linear motion assembly; 211. Linear guide rail; 212. Slider assembly; 2121. Slider bearing seat; 2122. Bearing end cover; 2123. Bearing; 213. Flexible transmission body; 214. Reducing motor; 215. Tensioning assembly; 2151. Tensioning screw; 216. Transmission wheel; 217. Guide rail seat; 22. Parallel link assembly; 221. Link; 222. Link pin; 223. Link hinge head; 224. Link rotation shaft; 23. Translational fixed disc assembly; 231. Translational fixed disc; 232. Translational bearing seat; 3. Base support assembly; 4. Control driving assembly. DETAILED DESCRIPTION

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

[0054] In the description of the present application, it should be understood that the terms "front and back", "up and down", "left and right", "height", "middle", "inner" and the like indicate the orientation or positional relationship, only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the indicated components or elements must have a specific orientation, be constructed and operated in a specific orientation, therefore cannot be understood as a limitation on the present application.

[0055] As Figure 1 , Figure 2As shown, the remote ultrasonic robot force feedback teleoperation device is composed of a serial posture mechanism assembly 1, a parallel position mechanism assembly 2, a base support assembly 3 and a control driving assembly 4; the serial posture mechanism assembly 1 is connected with the parallel position mechanism assembly 2 through a pitch pin shaft 131; the parallel position mechanism assembly 2 is fixedly installed on a plane of the base support assembly 3; the control driving assembly 4 is installed inside the base support assembly 3; the movement of the serial posture mechanism assembly 1 corresponds to the posture of the deflection axis X, the pitch axis Y and the roll axis Z of the robot ultrasonic probe, and feedbacks the contact torque of the robot ultrasonic probe in the X axis direction; the movement of the parallel position mechanism assembly 2 corresponds to the position of the X, Y and Z axes of the robot ultrasonic probe, and feedbacks the contact force in the X, Y and Z axis directions of the robot ultrasonic probe.

[0056] As shown, the base support assembly 3 is composed of a mounting surface and a base support leg, and the mounting surface is provided with a positioning structure of the linear motion assembly 21, and the height of the base support leg is greater than the height of the speed reduction motor 214 of the linear motion assembly.

[0057] As shown, the control driving assembly 4 includes a motor driver and a teleoperation controller; the motor driver functions to control the driving current to realize real-time force and torque feedback, and to obtain the motor encoding position data; the teleoperation controller realizes the function of force feedback teleoperation through control logic and control algorithm according to the input of encoder information, force feedback information and other parameters.

[0058] As shown, Figure 3 As shown, X, Y and Z respectively represent mutually orthogonal spatial coordinate axes, and the coordinate axis direction conforms to the right-hand rule. The serial posture mechanism assembly 1 is composed of a profiling handle 10, a deflection motor 11, a roll ring 12 and a pitch ring 13; the profiling handle 10 is internally integrated with an angular velocity sensor and a gravity sensor; the deflection motor 11 is coaxially fixedly installed with the roll ring 12, the motor shaft is connected with the profiling handle 10, the deflection motor 11 is provided with an encoder, and the encoder value corresponds to the deflection angle of the profiling handle 10; the roll ring 12 is inside the pitch ring 13 and connected through a roll pin shaft 121; the pitch ring 13 is inside the translational fixed disc 231 and connected through a pitch pin shaft 131; for the convenience of understanding, the pin shaft is disassembled in the figure, the motor shaft, the pitch pin shaft 131 and the roll pin shaft 121 are perpendicular to each other in any posture in space, and the three axes intersect at a point.

[0059] The functions and features of the series posture mechanism assembly 1 are as follows: the motor shaft, the pitch pin shaft 131 and the roll pin shaft 121 correspond to the deflection axis X, the pitch axis Y and the roll axis Z of the space posture of the profiling handle 10 respectively, the space posture is synchronous with the mapping of the posture of the ultrasonic probe of the remote ultrasonic robot mechanical arm, and the posture control of the ultrasonic probe by the profiling handle 10 is realized; the angular velocity sensor integrated in the profiling handle 10 can detect the angular velocity of the profiling handle 10 around the deflection axis X, around the pitch axis Y and around the roll axis Z, and the relative deflection angle, the relative pitch angle and the relative roll angle are obtained through the angular velocity integral algorithm of the control driving assembly respectively; the gravity sensor integrated in the profiling handle 10 can detect the absolute pitch angle and the absolute roll angle of the profiling handle 10, which is used for the vertical initial zero position calibration of the profiling handle 10; the encoder of the deflection motor 11 can detect the absolute deflection angle of the profiling handle 10, which is used for aligning the roll angle of the ultrasonic probe; the deflection motor 11 provides the torque in the direction of the deflection axis X of the main end profiling handle 10, so as to realize the torque feedback of the ultrasonic probe in the direction of the deflection axis X. The series posture mechanism assembly 1 can realize the detection of the space posture deflection angle, the pitch angle and the roll angle by the teleoperation device, realize the "turn", "shake" and "tilt" actions of the remote ultrasonic probe, and feedback the contact torque of the "turn" action of the remote ultrasonic probe. Since the contact torque of the "shake" and "tilt" actions is small, in order to reduce the complexity of the system, the torque feedback is not set in the direction of the pitch axis Y and the roll axis Z.

[0060] As shown in Figure 4 , X, Y and Z represent mutually orthogonal space coordinate axes respectively, and the coordinate axis direction conforms to the right-hand rule. The parallel position mechanism assembly 2 is composed of linear motion assemblies 21, parallel link assemblies 22 and translational fixed disc assemblies 23; the number of the linear motion assemblies 21 is three, which are horizontally arranged on the mounting surface of the base support assembly 3, and the mutual position included angle is 120° horizontally; the number of the parallel link assemblies 22 is three, which are respectively hinged with the linear motion assemblies 21; the translational fixed disc assembly 23 is hinged with the three parallel link assemblies 22, and the spatial position degree of freedom thereof is the front and back Z axis direction, the left and right Y axis direction and the up and down X axis direction.

[0061] The translational fixed disc assembly 23 is composed of a translational fixed disc 231 and translational bearing seats 232; the translational fixed disc 231 is a hollow ring structure in the middle, and is provided with a pitch axis hole in the diameter direction; the three translational bearing seats 232 are fixedly installed on the circumference of the translational fixed disc 231, and the mutual position included angle is 120° horizontally, and the bearing seats 232 have end covers on both sides for positioning two bearing outer rings respectively, and the bearing axial gap is adjusted through the end cover gasket.

[0062] As shown in Figure 5 , Figure 6As shown, the linear motion assembly 21 consists of a linear guide rail 211, a slider assembly 212, a flexible transmission body 213, a geared motor 214, a tensioning assembly 215, a transmission wheel 216, and a guide rail base 217. The flexible transmission body 213 is a synchronous belt or a steel wire rope; in this embodiment, a synchronous belt is used. The transmission wheel 216 is a synchronous belt pulley or a rope pulley; in this embodiment, a synchronous belt pulley is used. The linear guide rail 211 is fixedly mounted on the guide rail base 217, and the slider assembly 212 moves freely on the linear guide rail 211. The output shaft of the geared motor 214 is connected to the transmission wheel 214. 6. Fixed connection, driving flexible transmission body 213 to move slider assembly 212, and recording the movement position through encoder; both ends of flexible transmission body 213 pass around transmission wheel 216 and tensioning assembly 215 and are fixed on slider assembly 212; tensioning assembly 215 adjusts the tightness of flexible transmission body 213 through tensioning screw 2151; slider assembly 212 is equipped with slider bearing seat 2121, and the two bearing end caps 2122 on both sides respectively position the outer ring of two bearings 2123, and the axial clearance of bearing 2123 is adjusted by end cap shims.

[0063] like Figure 6 , Figure 7 The diagram shows the structure of the parallel link assembly of the present invention. The parallel link assembly 22 consists of two links 221, four link pins 222, four link hinge heads 223, and two link rotating shafts 224, and its structure is a parallelogram configuration. The link hinge heads 223 are installed at both ends of the link and are hinged to the link rotating shafts 224 through the link pins 222. The link rotating shafts 224 are hinged to the slider bearing 2121 seat on the slider assembly 212, and the other link rotating shaft 224 is hinged to the translation bearing seat 232 on the translational fixed plate assembly 23.

[0064] The functions and characteristics of the parallel position mechanism component 2 shown are as follows: This three-degree-of-freedom parallel mechanism, composed of a linear motion component 21, a parallel linkage component 22, and a translational fixed plate component 23, enables the contoured handle 10 to move in the horizontal Z and Y directions, corresponding to the ultrasound probe's "sliding" motion on the human body surface. The contoured handle 10 moves in the vertical X direction, corresponding to the depth at which the ultrasound probe presses against the human body surface. A force-sensor-equipped robotic arm at the slave end senses the interaction force between the ultrasound probe and the human body, feeding back the contact force information in the X, Y, and Z directions of Cartesian space to the master end teleoperation device. The contact force is calculated into the joint space of the parallel position mechanism by the control drive component 4, and the interaction force feedback is generated by controlling the drive current of the reduction motor 214. Through the compensation algorithm of the drive control component 4, the gravity, friction, and inertial forces of the teleoperation device can be compensated, eliminating the influence of other external forces on the interaction force feedback. Since the ultrasound examination speed is relatively low, the inertial force can be ignored.

[0065] like Figure 8The contact force feedback control flowchart of the application is as follows:

[0066] S1, the doctor operates the master handle, controls the slave mechanical arm through the integral algorithm, the forward kinematics algorithm and the proportional mapping master-slave control strategy by acquiring the motor encoder and the angular velocity sensor data, realizes the master-slave remote operation control of the position and the posture of the ultrasonic probe, and the ultrasonic probe contacts the surface of the body of the detected person, and the ultrasonic examination of the combination of the sliding, the rotating, the shaking and the tilting is started;

[0067] S2, the slave mechanical arm obtains the contact force and the torque by measuring the force sensor data with the base coordinate system of the mechanical arm as the reference,

[0068] B F S =[F e ,T e ] T =[F ex ,F ey ,F ez ,T ex ,T ey ,T ez ] T ;

[0069] S3, the target force and the torque of the master handle are obtained by the coordinate system conversion, the proportional mapping and the slave-master force feedback control strategy with the base support assembly coordinate system as the reference,

[0070] C F M =[F ek ,T ek ] T =[F ex ,F ey ,F ez ,T ex ,0,0] T ;

[0071] S4, the joint torques of the master are obtained by the transpose operation of the Jacobian matrix J of the master force feedback remote operation device,

[0072] τ ek =J TC F M =[τ1,τ2,τ3,τ4,0,0] T ;

[0073] S5, the gravity compensation torque τ g and the friction compensation torque τ f are calculated and superimposed on the joint torques, and the motor drive currents I are obtained through the torque constant calculation of the motor drivere ;

[0074] S6, Torque τ generated by each motor e By connecting the posture mechanism components in series and the position mechanism components in parallel, the contact force and torque are fed back to the doctor operator through the main end contour handle.

[0075] like Figure 9 The flowchart of the position and attitude master-slave teleoperation follow-up control of the present invention is as follows:

[0076] S1. The robotic arm moves to a position directly above the area to be inspected, with the ultrasonic probe vertical and maintaining an initial safe height relative to the human body surface. Using the robotic arm base coordinate system as a reference, the initial position and orientation coordinates of the ultrasonic probe are:

[0077] S2. The remote control device is activated, and the parallel positioning mechanism component drives the contouring handle back to its initial position. Using the base support component coordinate system as a reference, the initial position coordinate matrix of the contouring handle is: C P M ;

[0078] S3. Based on the initial posture of the ultrasonic probe, the deflection motor of the remote control device drives the deflection axis X of the contour handle to coincide and align with the deflection axis X of the ultrasonic probe.

[0079] S4. With the assistance of the gravity sensor on the contour handle, the doctor vertically aligns the contour handle and sets the initial attitude angles for the pitch axis (Y) and roll axis (Z). Using the coordinate system of the base support assembly as a reference, the initial position and attitude coordinates of the contour handle are:

[0080] S5, Ultrasonic probe coordinate system S x,y,z With the contour handle coordinate system M x,y,z Align and coincide the coordinate axes to make the attitude matrices of the contour handle and the ultrasonic probe coordinate system equal. B R S = C R M ;

[0081] S6. The doctor initiates remote operation using the orthogonal handpiece. Based on the set interaction frequency, at the next cycle, the controller drive component acquires encoder data from the parallel position mechanism component and obtains the new position matrix of the orthogonal handpiece using a forward kinematics algorithm. C P′ M ; Obtain angular velocity sensor data for the serial attitude mechanism components, and obtain the new attitude matrix of the contour handle through angular velocity integration and forward kinematics algorithm. C R′ M ;

[0082] S7, according to the position mapping scale k, the position increment matrix k of the ultrasonic probe is calculated C P′ M - C P M ), the posture increment matrix C R′ M - C R M ;

[0083] S8, the slave manipulator sends control data to the master remote control device, drives the ultrasonic probe to the target position and posture, and the target pose coordinates are

[0084] S9, based on the set interaction frequency, continuously obtain the master remote control device data, calculate the positive solution of the profiling handle pose coordinates, and continuously send the target pose coordinates to the slave manipulator, so as to realize the master-slave remote operation follow-up control of the ultrasonic probe position and posture.

[0085] In the description of the present application, the description of the terms "one embodiment", "example", "specific example" and the like means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are contained in at least one embodiment or example of the present application. In the present application, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0086] The basic principles, main features and advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above embodiments, and the above embodiments and descriptions in the specification are only to illustrate the principles of the present application. Without departing from the spirit and scope of the present application, various changes and improvements can be made to the present application, and these changes and improvements all fall within the scope of the present application.

Claims

1. A remote ultrasonic robot force feedback teleoperation device, characterized in that... The robot ultrasonic probe is composed of a series attitude mechanism assembly, a parallel position mechanism assembly, a base support assembly, and a control drive assembly. The series attitude mechanism assembly and the parallel position mechanism assembly are connected by a pitch pin. The parallel position mechanism assembly is fixedly mounted on the upper plane of the base support assembly. The control drive assembly is installed inside the base support assembly. The movement of the series attitude mechanism assembly corresponds to the X-axis, Y-axis, and Z-axis attitude of the robot ultrasonic probe, and provides feedback on the contact torque in the X-axis direction of the robot ultrasonic probe. The movement of the parallel position mechanism assembly corresponds to the X, Y, and Z-axis positions of the robot ultrasonic probe, and provides feedback on the contact force in the X, Y, and Z-axis directions of the robot ultrasonic probe. The series attitude mechanism assembly consists of a contour handle, a deflection motor, a rolling ring, and a pitch ring. The contour handle integrates an angular velocity sensor and a gravity sensor. The deflection motor and the rolling ring... The moving ring component is coaxially fixedly installed, the motor shaft is connected to the contour handle, and the deflection motor is equipped with an encoder whose value corresponds to the deflection angle of the contour handle. The rolling ring component is inside the pitch ring component and connected by a rolling pin. The pitch ring component is inside the translational fixed plate and connected by a pitch pin. The motor shaft, pitch pin, and rolling pin are perpendicular to each other in any spatial orientation, and the three axes intersect at a point. The parallel position mechanism assembly consists of a linear motion assembly, a parallel linkage assembly, and a translational fixed plate assembly. There are three sets of linear motion assemblies, horizontally arranged on the mounting surface of the base support assembly, with a horizontal angle of 120° between them. There are three sets of parallel linkage assemblies, each hinged to the linear motion assembly. The translational fixed plate assembly is hinged to the three sets of parallel linkage assemblies, and its spatial positional freedom is in the forward / backward Z-axis direction, the left / right Y-axis direction, and the up / down X-axis direction.

2. The remote ultrasonic robot force feedback teleoperation device according to claim 1, characterized in that... The linear motion assembly consists of a linear guide rail, a slider assembly, a flexible transmission body, a geared motor, a tensioning assembly, a transmission wheel, and a guide rail base. The flexible transmission body is a synchronous belt or wire rope, and the transmission wheel is a synchronous belt pulley or rope pulley. The linear guide rail is fixedly mounted on the guide rail base, and the slider assembly moves freely on the linear guide rail. The output shaft of the geared motor is fixedly connected to the transmission wheel, driving the flexible transmission body and moving the slider assembly, and the movement position is recorded by an encoder. The two ends of the flexible transmission body pass around the transmission wheel and the tensioning assembly and are fixed to the slider assembly. The tensioning assembly adjusts the tightness of the flexible transmission body through tensioning screws. A slider bearing seat is installed on the slider assembly, and the two bearing end caps on both sides respectively position the outer rings of the two bearings, and the axial clearance of the bearings is adjusted by the end cap shims.

3. The remote ultrasonic robot force feedback teleoperation device according to claim 1, characterized in that... The parallel link assembly consists of 2 links, 4 link pins, 4 link hinge heads, and 2 link shafts, and its structure is a parallelogram configuration. The link hinge heads are installed at both ends of the link and are hinged to the link shafts through the link pins. The link shafts are hinged to the slider bearing seats on the slider assembly, and the other link shaft is hinged to the translation bearing seats on the translational fixed plate assembly.

4. The remote ultrasonic robot force feedback teleoperation device according to claim 1, characterized in that, The master-slave teleoperation follow-up control method for position and attitude is as follows: S1. The robotic arm moves to a position directly above the area to be inspected, with the ultrasonic probe vertical and maintaining an initial safe height relative to the human body surface. Using the robotic arm base coordinate system as a reference, the initial position and orientation coordinates of the ultrasonic probe are: S2. The remote control device is activated, and the parallel positioning mechanism component drives the contouring handle back to its initial position. Using the base support component coordinate system as a reference, the initial position coordinate matrix of the contouring handle is: C P M ; S3. Based on the initial posture of the ultrasonic probe, the deflection motor of the remote control device drives the deflection axis X of the contour handle to coincide and align with the deflection axis X of the ultrasonic probe. S4. With the assistance of the gravity sensor on the contour handle, the doctor vertically aligns the contour handle and sets the initial attitude angles for the pitch axis (Y) and roll axis (Z). Using the coordinate system of the base support assembly as a reference, the initial position and attitude coordinates of the contour handle are: S5, Ultrasonic probe coordinate system S x，y，z With the contour handle coordinate system M x，y，z Align and coincide the coordinate axes to make the attitude matrices of the contour handle and the ultrasonic probe coordinate system equal. B R S = C R M ; S6. The doctor initiates remote operation using the orthogonal handpiece. Based on the set interaction frequency, at the next cycle, the controller drive component acquires encoder data from the parallel position mechanism component and obtains the new position matrix of the orthogonal handpiece using a forward kinematics algorithm. C P M ′;Acquire angular velocity sensor data of the serial attitude mechanism components, and obtain the new attitude matrix of the contour handle through angular velocity integration and forward kinematics algorithm. C R′ M ; S7. Based on the position mapping ratio k, calculate the position increment matrix k of the ultrasound probe. C P M ′- C P M Attitude increment matrix C R′ M - C R M ; S8. The remote control device sends control data to the slave robotic arm, driving the ultrasonic probe to reach the target's position and orientation. The target's position and orientation coordinates are... S9. Based on the set interaction frequency, continuously acquire data from the master teleoperation device, perform forward calculation of the contour handle's pose coordinates, and continuously send the target pose coordinates to the slave robotic arm, thereby realizing master-slave teleoperation follow-up control of the ultrasonic probe's position and attitude.

5. The remote ultrasonic robot force feedback teleoperation device according to claim 1, characterized in that, The contact force is controlled by the main feedback method as follows: S1. The doctor operates the contour handle, acquires data from the motor encoder and angular velocity sensor, and controls the slave robotic arm through master-slave control strategies such as integral algorithm, forward kinematics algorithm and proportional mapping to achieve master-slave remote operation control of the position and attitude of the ultrasound probe; the ultrasound probe contacts the surface of the body being examined and begins the ultrasound examination with a combination of "sliding", "turning", "shaking" and "tilting" movements. S2. The end-effector robotic arm obtains the contact force and torque by measuring data from the force sensor and referencing the coordinate system of the robotic arm base. B F S =[F e ,T e ] T =[F ex ,F ey ,F ez ,T ex ,T ey ,T ez ] T ; S3. Through coordinate system transformation and proportional mapping, the target force and torque of the main end contour handle are obtained from the main feedback control strategy, with the base support component coordinate system as a reference. C F M =[F ek ,T ek ] T =[F ex ,F ey ,F ez ,T ex ,0,0] T ; S4. By transposing the Jacobian matrix J of the main-end force feedback teleoperation device, the torques of each joint at the main end are obtained as follows: t ek =J TC F M =[τ1,τ2,τ3,τ4,0,0] T ; S5. Calculate the gravity compensation torque τ of the main end force feedback remote control device. g and frictional compensation torque τ f The torques of each joint are superimposed on the torques of the motors, and the torque constant of the motor driver is calculated to obtain the drive current I of each motor. e ; S6, Torque τ generated by each motor e By connecting the posture mechanism components in series and the position mechanism components in parallel, the contact force and torque are fed back to the doctor operator through the main end contour handle.

Images