CN114442490A - Main end control method of blood vessel intervention robot system based on self-adaptive force feedback - Google Patents

Main end control method of blood vessel intervention robot system based on self-adaptive force feedback Download PDF

Info

Publication number
CN114442490A
CN114442490A CN202210361274.8A CN202210361274A CN114442490A CN 114442490 A CN114442490 A CN 114442490A CN 202210361274 A CN202210361274 A CN 202210361274A CN 114442490 A CN114442490 A CN 114442490A
Authority
CN
China
Prior art keywords
motor
model
slave
force
operating rod
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210361274.8A
Other languages
Chinese (zh)
Other versions
CN114442490B (en
Inventor
柏健
黄志俊
刘金勇
钱坤
郑泽阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Langbokang Medical Technology Co ltd
Original Assignee
Lancet Robotics Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Lancet Robotics Co Ltd filed Critical Lancet Robotics Co Ltd
Priority to CN202210361274.8A priority Critical patent/CN114442490B/en
Publication of CN114442490A publication Critical patent/CN114442490A/en
Application granted granted Critical
Publication of CN114442490B publication Critical patent/CN114442490B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B34/37—Master-slave robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/301—Surgical robots for introducing or steering flexible instruments inserted into the body, e.g. catheters or endoscopes
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/303—Surgical robots specifically adapted for manipulations within body lumens, e.g. within lumen of gut, spine, or blood vessels

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Robotics (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • Veterinary Medicine (AREA)
  • Biomedical Technology (AREA)
  • Molecular Biology (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The embodiment of the invention discloses a main end control method of a vascular intervention robot system based on adaptive force feedback, wherein the method comprises the following steps: constructing a motor model; constructing a slide rail model; acquiring a slave end sensor signal, and filtering the slave end sensor signal; constructing a reference model of the motor, and setting a controller to correct parameters of the motor model in real time based on the reference model; estimating friction force based on the corrected motor model and the slide rail model, and compensating the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback. The method can realize accurate force feedback, so that the vascular intervention robot system has strong immunity and robustness, and can avoid output fluctuation to a greater extent.

Description

Main end control method of blood vessel intervention robot system based on self-adaptive force feedback
Technical Field
The invention relates to the field of control, in particular to a main-end control method of a vascular intervention robot system based on adaptive force feedback.
Background
In recent years, with the progress of robotics and the increased emphasis on health problems, robotic-assisted interventional therapy has become a focus of attention. The current vascular intervention operation equipment is low in utilization degree, and is mostly operated by experienced doctors. The training time required for the interventionalist is also long and the operation of the interventional procedure is more dependent on empirical procedures. In addition, because human blood vessels are bent and narrow, have more branches and have fragile blood vessel walls, great requirements are put on the precision of catheter operation by doctors in the experimental process, and very fine operation methods are required by the doctors. The long-time refined operation will inevitably cause operation fatigue of the doctor, thereby possibly affecting the operation effect. In addition, the operating doctor needs to be exposed to the X-ray for a long time regularly to perform the operation, even if wearing the radiation-proof lead clothes, the doctor cannot be guaranteed not to be irradiated by the X-ray, and the heavy lead clothes not only can affect the operating precision of the doctor, but also can cause a great burden to the body of the doctor after being worn for a long time, and can cause a chronic impact on the body health of the doctor.
Therefore, a blood vessel intervention robot system appears, which generally adopts a master-slave operation mode, is exposed to X-rays by the blood vessel intervention robot system to perform an operation, and is controlled by a doctor to work outside a ward in a remote operation mode, so that the doctor is prevented from being irradiated by the X-rays in the operating room. Compared with the operation of a doctor exposed to X-rays, the blood vessel interventional robot system has the characteristics of high precision and high accuracy, and can eliminate the influence of shaking hands on interventional operation. In addition, the blood vessel intervention robot system can help doctors to complete more complex intervention operations in clinical application, and the dependence degree of the complex operations on the experience of the doctors is effectively reduced. Taking a stent operation as an example, the blood vessel intervention robot system needs to judge the positions of a guide wire, a catheter and a balloon according to real-time CT images in the operation process, and a doctor operates the blood vessel intervention robot system to realize remote control of the blood vessel intervention robot. However, the existing robot system for vascular intervention cannot determine the current position state of the current interventional material by sensing the current resistance and other information on the catheter, the balloon and the guide wire during the operation process like the conventional operation process, so that the doctor cannot determine the current operation state by 'hand feeling' in real time, but can determine the current operation state by injecting more doses of contrast medium through the CT during the operation process. This not only prolongs the operation time, but also causes secondary damage to the patient.
Disclosure of Invention
According to an aspect of the invention, a vessel intervention robot system main-end control method based on adaptive force feedback is provided, which includes:
constructing a motor model, wherein the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance of the slave end equipment in the horizontal direction;
constructing a sliding rail model, wherein the sliding rail model is used for estimating the friction force of the main-end equipment in the using process;
acquiring a slave-end sensor signal, and filtering the slave-end sensor signal to obtain a processed sensor signal;
constructing a reference model of the motor, wherein the reference model of the motor is a model for representing error-free operation of the motor; setting a controller to correct parameters of the motor model in real time based on the reference model;
estimating friction force based on the corrected motor model and the slide rail model, and compensating the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
Optionally, in each of the above method embodiments of the present invention, the motor model is:
Figure DEST_PATH_IMAGE001
，
Figure DEST_PATH_IMAGE002
，
Figure DEST_PATH_IMAGE003
，
wherein u, i, R,
Figure DEST_PATH_IMAGE004
L respectively represents the terminal voltage, line current, winding internal resistance, internal flux linkage and winding inductance of the motor model, Ke、KtRespectively a back-emf coefficient and a torque coefficient, TeRepresenting the output torque of the motor, omega is the current rotating speed of the motor, t is the current time, J is the moment of inertia constant of the motor, BzIs the damping coefficient of the motor.
Optionally, in each of the above method embodiments of the present invention, the slide rail model is:
Figure DEST_PATH_IMAGE005
，
Figure DEST_PATH_IMAGE006
，
Figure DEST_PATH_IMAGE007
，
Figure DEST_PATH_IMAGE008
，
and initial position condition
Figure DEST_PATH_IMAGE009
，
Wherein M istotalIs the sum of the mass of the first sliding block, the second sliding block, the first motor, the operating rod and the coupling, dwThe diameter of the synchronous wheel is equal to the diameter of the synchronous wheel, the synchronous wheel comprises a first synchronous wheel and a second synchronous wheel, the second synchronous wheel is driven by a second motor to rotate, the second synchronous wheel, the first synchronous wheel and the synchronous belt jointly form a belt transmission device, the synchronous belt is used for driving a sliding block fixed on the synchronous belt to move, the sliding block comprises a first sliding block and a second sliding block, the second sliding block is used for fixing the first motor on the synchronous belt, and the first motor and the first sliding block fixed on the synchronous belt keep relatively static in the horizontal direction; the operating rod and a motor shaft of the first motor are fixed through a coupler, and the coupler drives the operating rod to rotate;
Figure DEST_PATH_IMAGE010
is the total sliding friction coefficient in the horizontal direction, FhApplying horizontal force to the operating rod for a user, wherein p, v and a are the position, the speed and the acceleration of the sliding block in the fixed horizontal direction respectively; is provided with
Figure DEST_PATH_IMAGE011
，FrefA force signal needing to be tracked is transmitted from the end sensor and is filtered, sign is a sign function and represents 1 when the sign is greater than 0, 1 when the sign is less than 0, and 0 when the sign is equal to 0; g is a gravity constant, f (t) is disturbance generated by a mechanical structure when the main-end equipment operates, and p0Is the initial position of the slide rail.
Optionally, in each of the above method embodiments of the present invention, the filtering the slave-end sensor signal includes:
constructing a nonlinear tracking differentiator, and filtering by using the nonlinear tracking differentiator as a low-pass filter, wherein the nonlinear tracking differentiator is as follows:
Figure DEST_PATH_IMAGE012
，
therein is obtainedThe slave-end sensor signal is taken as forg，fsun(z1,z2R, h) satisfy
Figure DEST_PATH_IMAGE013
，
Figure DEST_PATH_IMAGE014
，
Figure DEST_PATH_IMAGE015
，
Figure DEST_PATH_IMAGE016
，
Wherein, Fref(k +1) is the original signal obtained by the slave-end sensor through the filtering algorithm at the moment of k +1, Fref(k) The original signal obtained by the slave end sensor through the filtering algorithm at the moment k, h is the actual sampling interval time of the slave end sensor, and Dref(k) For the first derivative, D, of the original signal obtained by the slave-end sensor through the filtering algorithm at time kref(k +1) is the first derivative of the original signal obtained by the slave end sensor through the filtering algorithm at the moment of k +1, v (k) is an intermediate variable in the operation process, forg(k) The original signal with noise actually measured by the slave-end sensor at the moment k, r is a filter coefficient, k 'is a discretized time sequence at the current moment, fix (k') is an upward rounding function, and sat (a, b) is a saturation function and represents that when | a |, < y > is zero<When b is detected, sat (a, b) = a, and in the rest cases sat (a, b) = sign (a) × b.
Optionally, in each of the above method embodiments of the present invention, the reference model is:
Figure DEST_PATH_IMAGE017
，
Figure DEST_PATH_IMAGE018
，
wherein, the matrix AmAnd B is a representation in the form of a parameter matrix,
Figure DEST_PATH_IMAGE019
are state variables of the reference model.
Optionally, in each of the above method embodiments of the present invention, the setting a controller to perform real-time correction on the parameter of the motor model based on the reference model includes:
setting a first controller, wherein the first controller is as follows:
Figure DEST_PATH_IMAGE020
，
wherein C is a matrix
Figure DEST_PATH_IMAGE021
，
Figure DEST_PATH_IMAGE022
Refers to the input reference signal, x (t) is the state variable of the motor model, e (t) represents the output error,
Figure DEST_PATH_IMAGE023
is a parameter to be adjusted;
and correcting the flux linkage and the winding resistance of the motor model in real time based on the first controller.
Optionally, in each of the above method embodiments of the present invention, the friction force is estimated based on the corrected motor model and the slide rail model, where the estimated friction force is
Figure DEST_PATH_IMAGE024
Wherein
Figure 736050DEST_PATH_IMAGE010
the friction coefficient of the relative movement between the first sliding block and the sliding rail, the second sliding block and the sliding rail, or the friction coefficient existing when the operating rod and the first sliding block rotate.
Optionally, in the above method embodiments of the present invention, a second controller is configured, where the second controller is configured to apply, to the operating lever, the torsion force received by the recovered slave device and the horizontal resistance received by the recovered slave device, and the second controller is:
Figure DEST_PATH_IMAGE025
，
where k is a time series discretized at the current time, r (k) is the processed sensor signal, y (t) = ft(t) is the output of the slide, i.e. the force felt by the user at the end of the operating lever, kiJ is a variable symbol in standard summation operation, N is an upper limit value of a set integral summation sequence, r (k-j) is a sensor signal processed at the moment k-j, y (k-j) is the output of a slide rail at the moment k-j, and k is a parameter to be adjusteddFor the parameter to be adjusted, DrefAnd (k-1) is the first derivative of the original signal obtained by the filtering algorithm at the time k-1.
According to another aspect of the present invention, there is provided a main-end control device of a vascular intervention robot system based on adaptive force feedback, including:
the motor model building module is configured to build a motor model, the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance of the slave end equipment in the horizontal direction;
the sliding rail module building module is configured to build a sliding rail model, and the sliding rail model is used for estimating the friction force of the main-end equipment in the using process;
the filtering module is configured to acquire a slave-end sensor signal, and filter the slave-end sensor signal to obtain a processed sensor signal;
a correction module configured to construct a reference model of the electric machine, the reference model of the electric machine being a model characterizing error-free operation of the electric machine; setting a controller to correct parameters of the motor model in real time based on the reference model;
a force feedback module configured to estimate a friction force based on the corrected motor model and the slide rail model, and compensate the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
According to yet another aspect of the present invention, there is provided a computer readable storage medium having stored therein a plurality of instructions for being loaded by a processor and for performing the method as described above.
According to still another aspect of the present invention, there is provided an electronic apparatus, including:
a processor for executing a plurality of instructions;
a memory to store a plurality of instructions;
wherein the plurality of instructions are for storage by the memory and for loading and execution by the processor of the method as previously described.
The invention has the following technical effects: (1) can realize accurate force feedback, provide convenience for the doctor operation, avoid the doctor when carrying out the vascular intervention operation, wear the plumbous clothing and lead to the problem that the operation time is long, the quality is poor, avoided the doctor to expose under the CT ray in the art simultaneously, can protect doctor's health. (2) The method has the advantages of small phase lag, quick response and strong real-time performance compared with the traditional low-pass filter through the estimation and processing of the force signal by the nonlinear tracking differentiator, and has the advantages of no need of a reference model and less calculation amount compared with a Kalman filter. The derivative signal given by the non-linear tracking differentiator, including the non-derivative function, also gives an approximate derivative, which facilitates the rest of the controllers to utilize the derivative signal to improve control performance. (3) The self-adaptive control method is adopted for the motor torque, so that the influence of heat generated when the motor operates on the system parameters of the motor can be avoided, the structural perturbation generated when the motor operates abnormally can also be avoided to a certain extent, and compared with the traditional PID control, the self-adaptive control method has the advantage of avoiding complicated parameter adjustment. In addition, the invention also designs a stable controller by utilizing the Lyapunov method, and the stable controller is a large-range asymptotic stable controller, so that the motor system has strong interference resistance and robustness. The stability of the present invention is more suitable for the medical industry, such as high risk operation, than optimal control, such as LQR control. (4) In the aspect of force feedback control, the magnitude of the reaction force is estimated by utilizing the idea of expanding the state observer in the active disturbance rejection technology, so that the method has strong robustness, is smooth and can avoid output fluctuation to a large extent.
The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.
Drawings
The above and other objects, features and advantages of the present invention will become more apparent by describing in more detail embodiments of the present invention with reference to the attached drawings. The accompanying drawings are included to provide a further understanding of the embodiments of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings, like reference numbers generally represent like parts or steps.
Fig. 1 is a flow chart illustrating a method provided by an exemplary embodiment of the present invention.
Fig. 2A is a schematic mechanical structure diagram of a main-end control part of a vascular interventional robot system according to an exemplary embodiment of the present invention.
Fig. 2B is a left side view of a mechanical structure of a main-end control portion of a vascular interventional robot system according to an exemplary embodiment of the invention.
Fig. 2C is a top view of a main-end control part mechanism of a vascular interventional robot system according to an exemplary embodiment of the invention.
Fig. 2D is a schematic three-dimensional structural diagram of a main-end control part mechanism of a vascular interventional robot system according to an exemplary embodiment of the invention.
Fig. 3 is a schematic diagram of a filtering effect provided by an exemplary embodiment of the present invention.
Fig. 4 is a schematic diagram of a motor control principle provided by an exemplary embodiment of the present invention.
Fig. 5 is a flowchart illustrating a method according to another exemplary embodiment of the present invention.
Fig. 6 is a schematic structural diagram of an apparatus according to an exemplary embodiment of the present invention.
Fig. 7 is a structure of an electronic device according to an exemplary embodiment of the present invention.
Reference numerals:
1, touching a key; 2, operating a lever; 3, a first slide block; 4, a coupler; 5, a second slide block; 6, a first motor and an encoder; 7, a slide rail; 8, a limit screw and a contact switch; 9, a first synchronizing wheel; 10, synchronous belt; 11, a second synchronizing wheel; 12, a second motor and an encoder.
Detailed Description
Hereinafter, example embodiments according to the present invention will be described in detail with reference to the accompanying drawings. It is to be understood that the described embodiments are merely a subset of embodiments of the invention and not all embodiments of the invention, with the understanding that the invention is not limited to the example embodiments described herein.
It should be noted that: the relative arrangement of the components and steps, the numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present invention unless specifically stated otherwise.
It will be understood by those skilled in the art that the terms "first", "second", "S1" to "S5" and the like in the embodiments of the present invention are used only for distinguishing different steps, devices, modules and the like, and do not represent any particular technical meaning nor necessarily indicate a logical order therebetween. For example, the steps S1 and S2 are not limited to the order and may be processed in parallel.
It should also be understood that in embodiments of the present invention, "a plurality" may refer to two or more and "at least one" may refer to one, two or more.
It is also to be understood that any reference to any component, data, or structure in the embodiments of the invention may be generally understood as one or more, unless explicitly defined otherwise or stated to the contrary hereinafter.
In addition, the term "and/or" in the present invention is only one kind of association relationship describing the associated object, and means that there may be three kinds of relationships, for example, a and/or B, which may mean: a exists alone, A and B exist simultaneously, and B exists alone. In the present invention, the character "/" generally indicates that the preceding and following related objects are in an "or" relationship.
It should also be understood that the description of the embodiments of the present invention emphasizes the differences between the embodiments, and the same or similar parts may be referred to each other, so that the descriptions thereof are omitted for brevity.
Meanwhile, it should be understood that the sizes of the respective portions shown in the drawings are not drawn in an actual proportional relationship for the convenience of description.
The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application, or uses.
Techniques, methods, and apparatus known to those of ordinary skill in the relevant art may not be discussed in detail, but are intended to be part of the specification where appropriate.
It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, further discussion thereof is not required in subsequent figures.
Embodiments of the invention are operational with numerous other general purpose or special purpose computing system environments or configurations, and with numerous other electronic devices, such as terminal devices, computer systems, servers, etc. Examples of well known terminal devices, computing systems, environments, and/or configurations that may be suitable for use with electronic devices, such as terminal devices, computer systems, servers, and the like, include, but are not limited to: personal computer systems, server computer systems, thin clients, thick clients, hand-held or laptop devices, microprocessor-based systems, set-top boxes, programmable consumer electronics, networked personal computers, minicomputer systems, mainframe computer systems, distributed cloud computing environments that include any of the above, and the like.
Electronic devices such as terminal devices, computer systems, servers, etc. may be described in the general context of computer system-executable instructions, such as program modules, being executed by a computer system. Generally, program modules may include routines, programs, objects, components, logic, data structures, etc. that perform particular tasks or implement particular abstract data types. The computer system/server may be practiced in distributed cloud computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed cloud computing environment, program modules may be located in both local and remote computer system storage media including memory storage devices.
Exemplary method
Fig. 1 is a flowchart illustrating a control method of a main terminal of a vascular intervention robot system based on adaptive force feedback according to an exemplary embodiment of the present invention. The mechanical structure of the main end control part of the vascular interventional robot system is shown in figures 2A-2D.
The blood vessel intervention robot system based on self-adaptive force feedback is divided into a main end device and a slave end device, wherein the main end device is operated by a user at a far end so as to control the slave end device to perform an operation, the main end device comprises a touch key 1, an operating rod 2, a first sliding block 3, a coupler 4, a second sliding block 5, a first motor and an encoder 6, a sliding rail 7, a limit screw and a contact switch 8, a first synchronous wheel 9, a synchronous belt 10, a second synchronous wheel 11, a second motor and an encoder 12, a main control chip and a force sensor (not shown), the first synchronous wheel 9 is a driven wheel, the second synchronous wheel 11 and the synchronous belt 10 jointly form a belt transmission device, and the second synchronous wheel 11 is a driving wheel and is driven to rotate by the second motor; the synchronous belt 10 is used for driving a sliding block fixed on the synchronous belt to move, the sliding block comprises a first sliding block 3 and a second sliding block 5, the first sliding block 3 is used for transmitting a horizontal movement component of the operating rod 2 to the synchronous belt 10, the second sliding block is used for fixing the first motor on the synchronous belt, and the first motor and the first sliding block fixed on the synchronous belt keep relatively static in the horizontal direction; the slide rail 7 is connected with the synchronous belt 10 and provides a horizontal moving track for the slide block; the limiting screw and the contact switch are fixed at the boundary position of the movement of the sliding block and used for preventing the sliding block from separating from the sliding rail and correcting the encoder so as to obtain the absolute position of the sliding block; the operation rod 2 is operated by a user and can transmit actions (including push-pull actions, rotation actions and the like) of the user, the operation rod 2 and a motor shaft of the first motor are fixed through a coupler 4, the coupler 4 drives the operation rod 2 to rotate, and the movement of the rotation component of the operation rod 2 by the user can be transmitted to the first motor; second motor and encoder 12 are used for driving the rotation of second synchronizing wheel 11, and then drive hold-in range 10 carries out horizontal motion, drives again and is fixed in first slider 3 and the 5 horizontal motion of second slider on the hold-in range 10 to drive action bars 2 and carry out the motion of horizontal direction. The master end equipment also comprises a touch key used for judging whether a user contacts the operating rod 2 or not, and if the user contacts the operating rod 2, force feedback is started, namely the resistance on the slave end equipment is restored; otherwise, the first sliding block 3 and the second sliding block 5 are returned to the middle position of the sliding rail 7. The slave end device (not shown) is an executing mechanism of a vascular interventional operation on a patient, and comprises a vascular interventional robot, a propelling device of a catheter, a guide wire and a balloon, and a slave end sensor which is arranged on the propelling device, wherein the slave end sensor can be a force sensor.
The blood vessel intervention robot system based on the adaptive force feedback has the working mode that: when the slave end equipment performs vessel intervention, the vessel intervention robot operates the catheter, the guide wire or the balloon, and under the current state, certain resistance is felt at the tail end due to the influence of factors such as vessel wall obstruction, blood flow viscosity and the like, so that the certain resistance is sensed by the force sensor arranged at the slave end. The slave end sensor senses the signal and converts the signal into a corresponding signal, the corresponding signal is transmitted to the master end equipment for processing through the communication cable, and the master end restores and reproduces the resistance through executing a force feedback program, so that a user can feel the resistance borne by the slave end guide wire, the catheter or the balloon at the end of the operation rod. Therefore, a resistance signal sensed by a slave end sensor of the slave end equipment is obtained, and the resistance signal is transmitted to a main control chip of the master end equipment through a communication cable. The main control chip adjusts output torque of the first motor and the second motor by controlling current values of the first motor and the second motor, the output torque drives a sliding rail-sliding block system through a synchronous wheel-synchronous belt system, and the output torque is reflected to a user, so that guide wires and resistance force applied to the slave end equipment in the advancing process of a catheter or a saccule are restored in the master end equipment, therefore, hand feeling in a real operation can be simulated in real time by the master end equipment, the user applies action to an operating rod, and a force sensor of the master end equipment senses the action to generate a sensing signal; and sending the sensing signal as a motion instruction to a slave end to control the slave end.
The embodiment can be applied to an electronic device, as shown in fig. 1, and includes the following steps:
step S1, constructing a motor model, wherein the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction, and the resistance in the horizontal direction on the slave end equipment is restored.
In this embodiment, the first motor is configured to drive the operating rod to rotate, and to restore the torque applied to the slave device, that is, the first motor is configured to restore the torque applied to the current catheter, guide wire, or balloon sensed by the force sensor of the slave device. The second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance in the horizontal direction on the slave end device, namely the second motor is used for reducing the resistance in the horizontal direction on the current catheter, guide wire or balloon when the current catheter, guide wire or balloon is pushed or pulled back, which is sensed by the force sensor of the slave end device.
The first motor and the second motor are motors of the same type, and therefore, the motor model is suitable for the first motor and the second motor. The input signals of the first motor and the second motor are partial signals of the slave end sensor. The two motors are different in that the first motor is responsible for controlling the force feedback of the rotating part of the operating rod, and the second motor is responsible for controlling the force feedback of the advancing and retreating of the operating rod. Therefore, the first motor and the second motor are in a consistent model. In order to simulate the hand feeling in a real operation in real time, a motor torque controller needs to be constructed, and the motor torques of the first motor and the second motor are controlled through the motor torque controller. The motor model constructed in the embodiment provides a dynamic system model for a subsequently constructed motor torque controller.
In this embodiment, the motor model is:
Figure 528556DEST_PATH_IMAGE001
，
Figure 481862DEST_PATH_IMAGE002
，
Figure 814755DEST_PATH_IMAGE003
，
wherein u, i, R,
Figure 832389DEST_PATH_IMAGE004
L respectively represents the terminal voltage, line current, winding internal resistance, internal flux linkage and winding inductance of the motor model, Ke、KtRespectively a back-emf coefficient and a torque coefficient, TeRepresenting the output torque of the motor, omega is the current rotating speed of the motor, t is the current time, J is the moment of inertia constant of the motor, BzIs the damping coefficient of the motor.
The voltage is an output signal of a controller driving the motor through a driver, and is also an input signal of a motor model, and is used for controlling variables inside the motor. The resistance, flux linkage and inductance can be obtained by the specification document of the motor. The current can be obtained by a current sensor built in the driver, the back electromotive force coefficient and the torque coefficient can be obtained by the specification of the motor, and the values of omega, t, J and B can be obtained from the specification document of the motor.
Let x1=i，x2= ω, the motor model is converted into a state space equation:
Figure DEST_PATH_IMAGE026
，
the output torque of the motor is
Figure DEST_PATH_IMAGE027
。
And step S2, constructing a slide rail model, wherein the slide rail model is used for estimating the friction force of the main terminal equipment in the using process.
The friction force to which the main-end device is subjected during use includes, for example: the first sliding block and the second sliding block are respectively arranged on the guide rail and fixed on the synchronous belt and are used for driving the operating rod, the first motor and the coupler to move; friction generated when the operation lever is rotated, and the like. The slide rail model is used for offsetting the friction force, and in the process of offsetting the friction force, the restored current guide wire received by the slave end equipment, the resistance and the torsion in the horizontal direction of the catheter and the balloon act on the operating rod, and then the restored current guide wire received by the slave end equipment, the resistance and the torsion in the horizontal direction of the catheter and the balloon are transmitted to the hand of a user through the operating rod, so that the user has a relatively real operation hand feeling.
In this embodiment, the slide rail model is:
Figure 700857DEST_PATH_IMAGE005
，
Figure 905573DEST_PATH_IMAGE006
，
Figure 776577DEST_PATH_IMAGE007
，
Figure 878745DEST_PATH_IMAGE008
，
and initial position condition
Figure 809792DEST_PATH_IMAGE009
，
Wherein, MtotalIs the sum of the mass of the first sliding block, the second sliding block, the first motor, the operating rod and the coupling, dwThe diameters of the first synchronizing wheel and the second synchronizing wheel,
Figure 236225DEST_PATH_IMAGE010
is the total sliding friction coefficient in the horizontal direction, FhApplying horizontal force to the operating rod for a user, wherein p, v and a are the position, the speed and the acceleration of the sliding block in the fixed horizontal direction respectively, and the sliding block comprises a first sliding block and a second sliding block; order output
Figure 143876DEST_PATH_IMAGE011
，FrefThe method comprises the steps of (1) obtaining a force signal to be tracked, wherein the force signal to be tracked is a filtered force signal transmitted by a slave-end sensor, (v) sign of speed v, (g) a gravity constant, and (f), (t) disturbance generated when a mechanical structure of a master-end device operates. In this embodiment, it is assumed that the motor shaft and the synchronizing wheel shaft do not slide relatively, and elastic deformation of the synchronizing belt is ignored. The clockwise rotation of the synchronizing wheel and the direction of the belt driven by the synchronizing wheel are set as the positive direction. The disturbance generated during the operation of the mechanical structure comprises relative motion between a synchronous belt and a synchronous wheel, and the second motor receives electromagnetic interference or jitter generated by high-order harmonics in a power supply during the operation, and mechanical vibration and other disturbance which are difficult to model and cannot be estimated.
The object of the invention is to regulate TeLet out
Figure 604944DEST_PATH_IMAGE011
Meanwhile, by mapping the force of the slave end to the master end device, a user can sense the push-pull force and the torsion sensed by the slave end sensor, and by acquiring the difference value between the current position and the midpoint position of the slide rail and by using position type PID control, the first slide block, the second slide block and all devices fixed on the first slide block drive the synchronous wheel-conveyor belt-slide rail system to move the positions of the first slide block, the second slide block and all devices to the midpoint position of the slide rail through the second motor. The centering control uses a centering control method commonly used in the art, and thus the centering control is not described in detail.
And step S3, acquiring a slave sensor signal, and filtering the slave sensor signal to obtain a processed sensor signal.
The sensor at the slave end can be a force sensor, and the obtained signal is a force sensing signal which is used for sensing the horizontal push-pull force and the torsion of the current guide wire, the catheter and the saccule at the slave end.
The filtering the slave sensor signal comprises:
constructing a nonlinear tracking differentiator, and filtering by using the nonlinear tracking differentiator as a low-pass filter, wherein the nonlinear tracking differentiator is as follows:
Figure 972472DEST_PATH_IMAGE012
，
wherein the acquired slave-end sensor signal is forg，fsun(z1、z2R, h) satisfy
Figure 886201DEST_PATH_IMAGE013
，
Figure 334893DEST_PATH_IMAGE014
，
Figure 916047DEST_PATH_IMAGE015
，
Figure 188897DEST_PATH_IMAGE016
，
Wherein, Fref(k +1) is the original signal obtained by the slave-end sensor through the filtering algorithm at the moment of k +1, Fref(k) The original signal obtained by the slave end sensor through the filtering algorithm at the moment k, h is the actual sampling interval time of the slave end sensor, and Dref(k) For the first derivative, D, of the original signal obtained by the slave-end sensor through the filtering algorithm at time kref(k +1) is the first derivative of the original signal obtained by the slave end sensor through the filtering algorithm at the moment of k +1, v (k) is an intermediate variable in the operation process, forg(k) The original signal with noise actually measured by the slave-end sensor at the moment k, r is a filter coefficient, k 'is a discretized time sequence at the current moment, fix (k') is an upward rounding function, and sat (a, b) is a saturation function and represents that when | a |, < y > is zero<When b is detected, sat (a, b) = a, and in the rest cases sat (a, b) = sign (a) × b.
Considering that the signal of the force sensor received by the main end of the blood vessel intervention robot system is caused by mechanical vibration, blood flow resistance and the like, a disturbance signal cannot be avoided. The invention adopts the active disturbance rejection control theory and applies the active disturbance rejection control theory to the vascular intervention robot system. By establishing the nonlinear tracking differentiator as a low-pass filter, the high-frequency noise and derivative signals thereof in disturbance can be filtered in real time for users to use, such as doctors to observe, operators to debug and design and use the controller.
Fig. 3 shows the filtering effect of filtering with the nonlinear tracking differentiator.
Step S4, constructing a reference model of the motor, wherein the reference model of the motor is a model for representing the error-free operation of the motor; and setting a controller to carry out real-time correction on the parameters of the motor model on the basis of the reference model.
As shown in fig. 4, the reference model of the motor is a permanent magnet dc motor model, and relevant parameters can be obtained through a product description document, so as to construct the reference model of the motor. Through the reference model, rapid parameter correction is facilitated for the changing condition of the parameters of the motor.
The reference model is:
Figure 589922DEST_PATH_IMAGE017
，
Figure 104955DEST_PATH_IMAGE018
，
wherein, the matrix AmAnd B is a representation in the form of a parameter matrix,
Figure 540615DEST_PATH_IMAGE019
the state variables observed by the state observer.
Since the first motor and the second motor are the same motor, the first motor and the second motor can be controlled by the same controller. However, during the use of the first motor and the second motor, some parameters may have differences, such as different friction forces, so that the first motor and the second motor can be controlled by adjusting the parameters of the controller. For example, two controllers are instantiated, which are each used to control the first and the second electric machine, and which differ in the value of the parameter.
The setting controller for real-time correction of the parameters of the motor model based on the reference model comprises:
setting a first controller, wherein the first controller is as follows:
Figure 249946DEST_PATH_IMAGE020
，
wherein, the matrix
Figure DEST_PATH_IMAGE028
，
Figure DEST_PATH_IMAGE029
And C is a matrix
Figure 63835DEST_PATH_IMAGE021
，
Figure DEST_PATH_IMAGE030
Refers to the input reference signal, x (t) is the state variable of the motor model, e (t) represents the output error, and e = r-y,
Figure 523504DEST_PATH_IMAGE023
is the parameter to be adjusted.
Based on the first controller, the parameters of the motor model are corrected in real time in a manner that: according to
Figure 79250DEST_PATH_IMAGE028
，
Figure 693902DEST_PATH_IMAGE029
And correcting the flux linkage and the winding resistance in real time.
In this embodiment, strict stability and robustness are required due to the operation of the medical instrument. The controller was therefore designed based on the Lyapunov second method.
Because the line resistance and flux linkage parameters are susceptible to the influence of temperature change during the operation of the motor, the embodiment adopts a control idea based on model self-adaptation, and can estimate and compensate and control on line in real time.
Writing the motor model as a standard matrix development equation description form:
Figure DEST_PATH_IMAGE031
，
wherein from the differential equation:
Figure DEST_PATH_IMAGE032
，
u (t) is the input, which is the terminal voltage of the machine model.
Wherein, because the magnetic resistance and the wire resistance can change along with the change of heat, the matrix A (t) is subjected to drift and perturbation, and the final output can deviate from the reference model. Here, a reference model-based state observer is established for the original model and the matrix parameters are corrected in real time.
To track the reference signal without difference, let the lyapunov function be as follows:
Figure DEST_PATH_IMAGE033
，
where r (t) refers to the input reference signal,
Figure DEST_PATH_IMAGE034
the torque output by the motor is shown, and e = r-y shows the output error.
Then, the lyapunov function is derived to obtain:
Figure DEST_PATH_IMAGE035
，
make the control rate satisfy
Figure DEST_PATH_IMAGE036
，
Can be substituted to obtain
Figure DEST_PATH_IMAGE037
。
The method can be obtained according to Lyapunov criterion, and only simple parameter adjustment is needed for the reference model
Figure 738694DEST_PATH_IMAGE023
I.e. the output signal can be tracked at an exponential rate. And this controller is globally stable to the system.
And then carrying out self-adaptive correction on the design parameters of the motor.
Since there is no abrupt change in temperature, the resistance and flux linkage changes are almost negligible compared to the sampling control period, so its uncertain value can be approximately treated as a constant disturbance at every moment.
The approximate measurement value and the derivative signal can be extracted through the states collected by the encoder on the motor and the current sensor on the driver for controlling the motor and filtering the collected signals.
The state variable x (t) is regarded as a parameter in the same sampling period moment, and the current approximate parameter value is obtained through a motor model as follows:
Figure DEST_PATH_IMAGE038
，
Figure DEST_PATH_IMAGE039
，
will be provided with
Figure DEST_PATH_IMAGE040
R (t) instead of matrix A in u (t)mElement (1) of
Figure 877420DEST_PATH_IMAGE004
And R, solving a new control rate u (t) which is the self-adaptive control rate.
Step S5, estimating friction force based on the corrected motor model and the slide rail model, and compensating the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
Since the friction force exists when the user pushes or rotates the operating rod to move, the control of the vascular intervention robot system, especially the force feedback, is seriously influenced because the friction force has strong nonlinear action. Therefore, the friction force is compensated for, and the friction force is estimated.
Estimating the friction force based on the corrected motor model and the slide rail model, wherein the estimated friction force is
Figure 22094DEST_PATH_IMAGE024
。
And constructing a second controller, wherein the second controller is used for applying the torsion force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to an operating rod. The second controller is:
Figure 807647DEST_PATH_IMAGE025
，
where k is a time series discretized at the current time, r (k) is the processed sensor signal, y (t) = ft(t) is the output of the slide, i.e. the force experienced by the user at the end of the final operating lever, kiFor the parameter to be adjusted, the subscript i represents integral (integral), j is variable sign in standard summation operation, N is upper limit value of set integral summation sequence, r (k-j) is sensor signal processed at k-j time, y (k-j) is output of slide rail at k-j time, k (k-j) is output of slide rail at k-j timedDref (k-1) is the first derivative of the original signal obtained by the slave sensor through the filtering algorithm at time k-1, which is the parameter to be adjusted.
In this embodiment, based on the slide rail model, the state variable is set to
Figure DEST_PATH_IMAGE041
The state space is established as follows:
Figure DEST_PATH_IMAGE042
，
wherein,
Figure DEST_PATH_IMAGE043
，uh(t) represents the motor output torque, uh(t) is also a slide railInput signal of the model. F (t) represents the force applied by an operator, such as a doctor, to the joystick, and the desired output torque of the motor is used as an input signal to control the rank criterion of the system to make the system accurately controllable. The friction force is piecewise nonlinear according to the motion direction, and then the current second controller is determined according to the states at different moments, and piecewise linear control is carried out, namely force feedback control is not required to be considered when the vehicle is static.
When sliding is considered, the direction of the current running speed can be judged through signals of the encoder, and the motion states of the hands of the user and the operating rod are consistent because the hands and the operating rod are always relatively static. The force of the hand, i.e. the output y (t), is satisfied
Figure DEST_PATH_IMAGE044
Then substituting the state equation to obtain the state output:
Figure DEST_PATH_IMAGE045
。
based on the processed sensor signal, applying the restored torque force received by the slave device and the restored horizontal resistance received by the slave device to the operation rod, wherein the restored force applied to the operation rod is force feedback, and the force feedback includes:
the processed sensor signal is recorded as r (t) and related to the original system
Figure DEST_PATH_IMAGE046
Performing state expansion:
Figure DEST_PATH_IMAGE047
，
and constructing an extended state observer:
Figure DEST_PATH_IMAGE048
，
the second controller is:
Figure DEST_PATH_IMAGE049
，
wherein k isp、ki、kdRepresenting a parameter of the second controller to be adjusted. y (t) = ft(t) is the sled system output, i.e. the force experienced by the user at the end of the final joystick, z (t) is the state observer state variable, e1The error of the estimated state and the actual state is observed.
In this embodiment, the sliding friction is identified and counteracted by the parameters obtained by the encoders integrated on the first motor and the second motor and the current sensors provided on the driving circuit. The method adopts the idea of active disturbance rejection technology, treats the acceleration component generated by shaking and the acceleration component caused by friction, which are input into a system by holding an operating rod by a user, as an integral disturbance, expands the dimension of the integral disturbance and establishes a state observer. And the disturbance estimated by the state observer is used for counteracting, and the observed state is used for carrying out the pole allocation of state feedback.
And the extended state observer is used for pole allocation and discretization, and the second controller can be designed as
Figure 44462DEST_PATH_IMAGE025
，
Wherein DrefIs the derivative of the filter to acquire the reference signal.
Fig. 5 is a flowchart illustrating a method for controlling a main terminal of a vascular interventional robot system based on adaptive force feedback according to another exemplary embodiment of the invention. As shown in fig. 5, the method includes:
step S51, determining whether the touch key of the operating lever is contacted, if yes, entering step S52; if not, go to step S53;
step S52: judging whether a first sliding block and a second sliding block fixed on a synchronous belt reach limit positions or not; if yes, go to step S53; if not, determining force feedback, and entering step S54;
step S53: centering the first sliding block and the second sliding block to the middle position of the sliding rail;
step S54: based on the force feedback, applying action to the main-end equipment, and sensing the action by a force sensor of the main-end equipment to generate a sensing signal; and sending the sensing signal as a motion instruction to a slave end to control the slave end.
Further, before the step S51, the method includes the step S50 of initializing the vascular intervention robot system, wherein the second motor drives the second synchronous wheel to rotate, the second synchronous wheel drives the synchronous belt to move, so as to drive the first slider and the second slider fixed on the synchronous belt to move horizontally until the travel switch is touched, and after the travel switch is touched, the incremental encoder is calibrated, and then a position loop feedback PID control method is used to start a return program, so that the first slider and the second slider reach the middle position of the slide rail.
Further, the step S53: centering the first slider and the second slider, comprising: and starting a middle returning program by using a position loop feedback PID control method to enable the first sliding block and the second sliding block to reach the middle position of the sliding rail.
Further, after the step S54, information such as the value output by the second controller, the value measured by the sensor of the master device, and an error code generated during the operation process is recorded in the storage device through the log system.
Exemplary devices
Fig. 6 is a schematic structural diagram of a device for controlling a main end of a vascular interventional robot system based on adaptive force feedback according to an exemplary embodiment of the invention. As shown in fig. 6, the present embodiment includes:
the motor model building module is configured to build a motor model, the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance of the slave end equipment in the horizontal direction;
the sliding rail module building module is configured to build a sliding rail model, and the sliding rail model is used for estimating the friction force of the main-end equipment in the using process;
the filtering module is configured to acquire a slave-end sensor signal, and filter the slave-end sensor signal to obtain a processed sensor signal;
a correction module configured to construct a reference model of the electric machine, the reference model of the electric machine being a model characterizing error-free operation of the electric machine; setting a controller to correct parameters of the motor model in real time based on the reference model;
a force feedback module configured to estimate a friction force based on the corrected motor model and the slide rail model, and compensate the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
Exemplary electronic device
Fig. 7 is a structure of an electronic device provided by an exemplary embodiment of the present invention. The electronic device may be either or both of the first device and the second device, or a stand-alone device separate from them, which stand-alone device may communicate with the first device and the second device to receive the acquired input signals therefrom. FIG. 7 illustrates a block diagram of an electronic device in accordance with an embodiment of the disclosure. As shown in fig. 7, the electronic device includes one or more processors 71 and a memory 72.
The processor 71 may be a Central Processing Unit (CPU) or other form of processing unit having data processing capabilities and/or instruction execution capabilities, and may control other components in the electronic device to perform desired functions.
Memory 72 may include one or more computer program products that may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory. The volatile memory may include, for example, Random Access Memory (RAM), cache memory (cache), and/or the like. The non-volatile memory may include, for example, Read Only Memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer readable storage medium and executed by the processor 71 to implement the method for adaptive force feedback-based vessel intervention robot system master-end control of the software program of the various embodiments of the present disclosure described above and/or other desired functions. In one example, the electronic device may further include: an input device 73 and an output device 74, which are interconnected by a bus system and/or other form of connection mechanism (not shown).
The input device 73 may also include, for example, a keyboard, a mouse, and the like.
The output device 74 may output various information to the outside. The output devices 74 may include, for example, a display, speakers, a printer, and a communication network and remote output devices connected thereto, among others.
Of course, for simplicity, only some of the components of the electronic device relevant to the present disclosure are shown in fig. 7, omitting components such as buses, input/output interfaces, and the like. In addition, the electronic device may include any other suitable components, depending on the particular application.
Exemplary computer program product and computer-readable storage Medium
In addition to the above-described methods and apparatus, embodiments of the present disclosure may also be a computer program product comprising computer program instructions that, when executed by a processor, cause the processor to perform the steps in the method for adaptive force feedback based vessel intervention robot system master-end control according to various embodiments of the present disclosure described in the "exemplary methods" section above in this specification.
The computer program product may write program code for carrying out operations for embodiments of the present disclosure in any combination of one or more programming languages, including an object oriented programming language such as Java, C + + or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server.
Furthermore, embodiments of the present disclosure may also be a computer-readable storage medium having stored thereon computer program instructions, which, when executed by a processor, cause the processor to perform the steps in the method for adaptive force feedback based vessel intervention robot system master-end control according to various embodiments of the present disclosure described in the "exemplary methods" section above in this specification.
The computer readable storage medium may take any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may include, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
The foregoing describes the general principles of the present disclosure in conjunction with specific embodiments, however, it is noted that the advantages, effects, etc. mentioned in the present disclosure are merely examples and are not limiting, and they should not be considered essential to the various embodiments of the present disclosure. Furthermore, the foregoing disclosure of specific details is for the purpose of illustration and description and is not intended to be limiting, since the disclosure is not intended to be limited to the specific details so described.
In the present specification, the embodiments are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same or similar parts in the embodiments are referred to each other. For the system embodiment, since it basically corresponds to the method embodiment, the description is relatively simple, and for the relevant points, reference may be made to the partial description of the method embodiment.
The block diagrams of devices, apparatuses, systems referred to in this disclosure are only given as illustrative examples and are not intended to require or imply that the connections, arrangements, configurations, etc. must be made in the manner shown in the block diagrams. These devices, apparatuses, devices, systems may be connected, arranged, configured in any manner, as will be appreciated by those skilled in the art. Words such as "including," "comprising," "having," and the like are open-ended words that mean "including, but not limited to," and are used interchangeably therewith. The words "or" and "as used herein mean, and are used interchangeably with, the word" and/or, "unless the context clearly dictates otherwise. The word "such as" is used herein to mean, and is used interchangeably with, the phrase "such as but not limited to".
The methods and apparatus of the present disclosure may be implemented in a number of ways. For example, the methods and apparatus of the present disclosure may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above-described order for the steps of the method is for illustration only, and the steps of the method of the present disclosure are not limited to the order specifically described above unless specifically stated otherwise. Further, in some embodiments, the present disclosure may also be embodied as programs recorded in a recording medium, the programs including machine-readable instructions for implementing the methods according to the present disclosure. Thus, the present disclosure also covers a recording medium storing a program for executing the method according to the present disclosure.
It is also noted that in the devices, apparatuses, and methods of the present disclosure, each component or step can be decomposed and/or recombined. These decompositions and/or recombinations are to be considered equivalents of the present disclosure. The previous description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present disclosure. Various modifications to these aspects will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other aspects without departing from the scope of the disclosure. Thus, the present disclosure is not intended to be limited to the aspects shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
The foregoing description has been presented for purposes of illustration and description. Furthermore, this description is not intended to limit embodiments of the disclosure to the form disclosed herein. While a number of example aspects and embodiments have been discussed above, those of skill in the art will recognize certain variations, modifications, alterations, additions and sub-combinations thereof.

Claims (11)

1. A main end control method of a vascular intervention robot system based on adaptive force feedback is characterized by comprising the following steps:
constructing a motor model, wherein the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance of the slave end equipment in the horizontal direction;
constructing a sliding rail model, wherein the sliding rail model is used for estimating the friction force of the main-end equipment in the using process;
acquiring a slave-end sensor signal, and filtering the slave-end sensor signal to obtain a processed sensor signal;
constructing a reference model of the motor, wherein the reference model of the motor is a model for representing error-free operation of the motor; setting a controller to correct parameters of the motor model in real time based on the reference model;
estimating friction force based on the corrected motor model and the slide rail model, and compensating the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
2. The method of claim 1, wherein the motor model is:
Figure 749032DEST_PATH_IMAGE001
，
Figure 62333DEST_PATH_IMAGE002
，
Figure 827158DEST_PATH_IMAGE003
，
wherein u, i, R,
Figure 172164DEST_PATH_IMAGE004
L respectively represents the terminal voltage, line current, winding internal resistance, internal flux linkage and winding inductance of the motor model, Ke、KtRespectively a back-emf coefficient and a torque coefficient, TeRepresenting the output torque of the motor, omega is the current rotating speed of the motor, t is the current time, J is the moment of inertia constant of the motor, BzIs the damping coefficient of the motor.
3. The method of claim 2, wherein the sled model is:
Figure 160980DEST_PATH_IMAGE005
，
Figure 961577DEST_PATH_IMAGE006
，
Figure 61251DEST_PATH_IMAGE007
，
Figure 18622DEST_PATH_IMAGE008
，
and initial position condition
Figure 178339DEST_PATH_IMAGE009
，
Wherein M istotalIs the sum of the mass of the first sliding block, the second sliding block, the first motor, the operating rod and the coupling, dwThe diameter of the synchronous wheel is equal to the diameter of the synchronous wheel, the synchronous wheel comprises a first synchronous wheel and a second synchronous wheel, the second synchronous wheel is driven by a second motor to rotate, the second synchronous wheel, the first synchronous wheel and the synchronous belt jointly form a belt transmission device, the synchronous belt is used for driving a sliding block fixed on the synchronous belt to move, the sliding block comprises a first sliding block and a second sliding block, the second sliding block is used for fixing the first motor on the synchronous belt, and the first motor and the first sliding block fixed on the synchronous belt keep relatively static in the horizontal direction; the operating rod and a motor shaft of the first motor are fixed through a coupler, and the coupler drives the operating rod to rotate;
Figure 466232DEST_PATH_IMAGE010
is the total sliding friction coefficient in the horizontal direction, FhApplying horizontal force to the operating rod for a user, wherein p, v and a are the position, the speed and the acceleration of the sliding block in the fixed horizontal direction respectively; is provided with
Figure 369597DEST_PATH_IMAGE011
，FrefA force signal needing to be tracked is transmitted from the end sensor and is filtered, sign is a sign function and represents 1 when the sign is greater than 0, 1 when the sign is less than 0, and 0 when the sign is equal to 0; g is a gravity constant, f (t) is disturbance generated by a mechanical structure when the main-end equipment operates, and p0Is the initial position of the slide rail.
4. The method of claim 3, wherein said filtering said slave sensor signal comprises:
constructing a nonlinear tracking differentiator, and filtering by using the nonlinear tracking differentiator as a low-pass filter, wherein the nonlinear tracking differentiator is as follows:
Figure 689195DEST_PATH_IMAGE012
，
wherein the acquired slave-end sensor signal is forg，fsun(z1,z2R, h) satisfy
Figure 754234DEST_PATH_IMAGE013
，
Figure 732686DEST_PATH_IMAGE014
，
Figure 705321DEST_PATH_IMAGE015
，
Figure 679093DEST_PATH_IMAGE016
，
Wherein, Fref(k +1) is the original signal obtained by the slave-end sensor through the filtering algorithm at the moment of k +1, Fref(k) The original signal obtained by the slave end sensor through the filtering algorithm at the moment k, h is the actual sampling interval time of the slave end sensor, and Dref(k) For the first derivative, D, of the original signal obtained by the slave-end sensor through the filtering algorithm at time kref(k +1) is the first derivative of the original signal obtained by the slave end sensor through the filtering algorithm at the moment of k +1, and v (k) is an intermediate variable in the operation process，forg(k) The original signal with noise actually measured by the slave-end sensor at the moment k, r is a filter coefficient, k 'is a discretized time sequence at the current moment, fix (k') is an upward rounding function, and sat (a, b) is a saturation function and represents that when | a |, < y > is zero<When b is detected, sat (a, b) = a, and in the rest cases sat (a, b) = sign (a) × b.
5. The method of claim 4, wherein the reference model is:
Figure 646525DEST_PATH_IMAGE017
，
Figure 909010DEST_PATH_IMAGE018
，
wherein, the matrix AmAnd B is a representation in the form of a parameter matrix,
Figure 482074DEST_PATH_IMAGE019
are state variables of the reference model.
6. The method of claim 5, wherein the setting a controller to make real-time corrections to parameters of the motor model based on the reference model comprises:
setting a first controller, wherein the first controller is as follows:
Figure 982456DEST_PATH_IMAGE020
，
wherein C is a matrix
Figure 917527DEST_PATH_IMAGE021
，
Figure 542674DEST_PATH_IMAGE022
Reference signal input, x (t) is the shape of motor modelA state variable, e (t), representing an output error,
Figure 919429DEST_PATH_IMAGE023
is a parameter to be adjusted;
and correcting the flux linkage and the winding resistance of the motor model in real time based on the first controller.
7. The method of claim 6, wherein the friction force is estimated based on the corrected motor model and the rail model, wherein the estimated friction force is
Figure 336635DEST_PATH_IMAGE024
。
8. The method according to claim 7, wherein a second controller for applying the torsion force received by the recovered slave end apparatus and the resistance force in the horizontal direction received by the recovered slave end apparatus to the operation lever is constructed as follows:
Figure 708186DEST_PATH_IMAGE025
，
where k is a time series discretized at the current time, r (k) is the processed sensor signal, y (t) = ft(t) is the output of the slide, i.e. the force felt by the user at the end of the operating lever, kiJ is a variable symbol in standard summation operation, N is an upper limit value of a set integral summation sequence, r (k-j) is a sensor signal processed at the moment k-j, y (k-j) is the output of a slide rail at the moment k-j, and k is a parameter to be adjusteddIs the parameter to be adjusted.
9. A main end control device of a vascular intervention robot system based on adaptive force feedback is characterized by comprising:
the motor model building module is configured to build a motor model, the motor model is used for simulating a motor in main-end equipment, and the motor comprises a first motor and a second motor; the first motor is used for driving the operating rod to rotate and restoring the torsion force applied to the slave end equipment; the second motor is used for driving the operating rod to move along the horizontal direction and reducing the resistance of the slave end equipment in the horizontal direction;
the sliding rail module building module is configured to build a sliding rail model, and the sliding rail model is used for estimating the friction force of the main-end equipment in the using process;
the filtering module is configured to acquire a slave-end sensor signal, and filter the slave-end sensor signal to obtain a processed sensor signal;
a correction module configured to construct a reference model of the electric machine, the reference model of the electric machine being a model characterizing error-free operation of the electric machine; setting a controller to correct parameters of the motor model in real time based on the reference model;
a force feedback module configured to estimate a friction force based on the corrected motor model and the slide rail model, and compensate the friction force; and based on the processed sensor signals, applying the torque force received by the recovered slave end equipment and the horizontal resistance received by the recovered slave end equipment to the operating rod, wherein the force applied to the operating rod after recovery is force feedback.
10. A computer-readable storage medium having stored therein a plurality of instructions for loading by a processor and executing the method of any of claims 1-8.
11. An electronic device, characterized in that the electronic device comprises:
a processor for executing a plurality of instructions;
a memory to store a plurality of instructions;
wherein the plurality of instructions are for storage by the memory and for loading and execution by the processor of the method of any of claims 1-8.
CN202210361274.8A 2022-04-07 2022-04-07 Main end control method of vascular intervention robot system based on adaptive force feedback Active CN114442490B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210361274.8A CN114442490B (en) 2022-04-07 2022-04-07 Main end control method of vascular intervention robot system based on adaptive force feedback

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210361274.8A CN114442490B (en) 2022-04-07 2022-04-07 Main end control method of vascular intervention robot system based on adaptive force feedback

Publications (2)

Publication Number Publication Date
CN114442490A true CN114442490A (en) 2022-05-06
CN114442490B CN114442490B (en) 2022-06-24

Family

ID=81360473

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210361274.8A Active CN114442490B (en) 2022-04-07 2022-04-07 Main end control method of vascular intervention robot system based on adaptive force feedback

Country Status (1)

Country Link
CN (1) CN114442490B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115363768A (en) * 2022-10-27 2022-11-22 北京唯迈医疗设备有限公司 Interventional operation robot system and control method and medium thereof
CN116277034A (en) * 2023-05-15 2023-06-23 北京壹点灵动科技有限公司 Robot control method and device for coping with load change and electronic equipment
CN116999178A (en) * 2023-10-07 2023-11-07 北京科鹏医疗器械有限公司 Dual-frequency filtering visual master-slave mapping method operated by natural channel endoscope

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1802239A (en) * 2003-08-21 2006-07-12 提姆查克股份有限公司 Robot apparatus and robot remote operation system
CN102085663A (en) * 2010-12-22 2011-06-08 中国科学院深圳先进技术研究院 Remote operating system and method
CN102825603A (en) * 2012-09-10 2012-12-19 江苏科技大学 Network teleoperation robot system and time delay overcoming method
KR20150027618A (en) * 2013-09-04 2015-03-12 삼성전자주식회사 Surgical robot and control method thereof
CN107184274A (en) * 2017-06-05 2017-09-22 上海速介机器人科技有限公司 Blood vessel intervention operation robot manipulation handle and its control method with feel
CN107307909A (en) * 2017-07-06 2017-11-03 北京理工大学 One kind intervention robot remote operating system and its control method
CN107921625A (en) * 2015-08-25 2018-04-17 川崎重工业株式会社 Tele-manipulator system
CN108665759A (en) * 2018-05-07 2018-10-16 上海斐旭电子设备有限公司 A kind of blood vessel intervention operation simulated training system realization process
CN109968361A (en) * 2019-05-17 2019-07-05 北京理工大学 A kind of impedance remote operating control device and method based on Real-time force feedback
CN111110353A (en) * 2020-01-20 2020-05-08 北京理工大学 Main end controller of interventional operation robot
CN111973279A (en) * 2019-05-21 2020-11-24 复旦大学 Master-slave position self-adaptive tracking control method of vascular interventional surgical robot
CN112017516A (en) * 2020-08-26 2020-12-01 北京理工大学 Remote vascular intervention operation training system

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1802239A (en) * 2003-08-21 2006-07-12 提姆查克股份有限公司 Robot apparatus and robot remote operation system
CN102085663A (en) * 2010-12-22 2011-06-08 中国科学院深圳先进技术研究院 Remote operating system and method
CN102825603A (en) * 2012-09-10 2012-12-19 江苏科技大学 Network teleoperation robot system and time delay overcoming method
KR20150027618A (en) * 2013-09-04 2015-03-12 삼성전자주식회사 Surgical robot and control method thereof
CN107921625A (en) * 2015-08-25 2018-04-17 川崎重工业株式会社 Tele-manipulator system
CN107184274A (en) * 2017-06-05 2017-09-22 上海速介机器人科技有限公司 Blood vessel intervention operation robot manipulation handle and its control method with feel
CN107307909A (en) * 2017-07-06 2017-11-03 北京理工大学 One kind intervention robot remote operating system and its control method
CN108665759A (en) * 2018-05-07 2018-10-16 上海斐旭电子设备有限公司 A kind of blood vessel intervention operation simulated training system realization process
CN109968361A (en) * 2019-05-17 2019-07-05 北京理工大学 A kind of impedance remote operating control device and method based on Real-time force feedback
CN111973279A (en) * 2019-05-21 2020-11-24 复旦大学 Master-slave position self-adaptive tracking control method of vascular interventional surgical robot
CN111110353A (en) * 2020-01-20 2020-05-08 北京理工大学 Main end controller of interventional operation robot
CN112017516A (en) * 2020-08-26 2020-12-01 北京理工大学 Remote vascular intervention operation training system

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115363768A (en) * 2022-10-27 2022-11-22 北京唯迈医疗设备有限公司 Interventional operation robot system and control method and medium thereof
CN115363768B (en) * 2022-10-27 2023-04-14 北京唯迈医疗设备有限公司 Interventional operation robot system and control method and medium thereof
CN116277034A (en) * 2023-05-15 2023-06-23 北京壹点灵动科技有限公司 Robot control method and device for coping with load change and electronic equipment
CN116277034B (en) * 2023-05-15 2023-09-12 北京壹点灵动科技有限公司 Robot control method and device for coping with load change and electronic equipment
CN116999178A (en) * 2023-10-07 2023-11-07 北京科鹏医疗器械有限公司 Dual-frequency filtering visual master-slave mapping method operated by natural channel endoscope
CN116999178B (en) * 2023-10-07 2024-01-12 北京科鹏医疗器械有限公司 Dual-frequency filtering visual master-slave mapping method operated by natural channel endoscope

Also Published As

Publication number Publication date
CN114442490B (en) 2022-06-24

Similar Documents

Publication Publication Date Title
CN114442490B (en) Main end control method of vascular intervention robot system based on adaptive force feedback
US11148282B2 (en) Control device for motor drive device, control device for multi-axial motor, and control method for motor drive device
Katsura et al. A realization of haptic training system by multilateral control
CN113189865B (en) Rehabilitation robot control system based on dynamic parameter identification
JP2005316937A (en) Control device and its control method
CN110653821A (en) Control method, system, medium and equipment for mechanical arm
Wang et al. Adaptive PID-fractional-order nonsingular terminal sliding mode control for cable-driven manipulators using time-delay estimation
US10556336B1 (en) Determining robot inertial properties
Crevecoeur et al. Sensorimotor mapping for anticipatory grip force modulation
CN113664869A (en) Mechanical arm, mechanical arm control method and mechanical arm control system
WO2021060104A1 (en) Information processing device, information processing system, and information processing method
JP6237039B2 (en) Robot control apparatus and robot control method
CN116313036A (en) Hand motion prediction algorithm based on motion measurement and machine learning
US20220276711A1 (en) Position/force controller, and position/force control method and storage medium
CN114310874A (en) Cooperative robot joint torque control method and system and computer equipment
JP6502634B2 (en) Control device
WO2022145213A1 (en) Control device, control system, and control method
WO2022145150A1 (en) Control system and control method
Pitakwatchara Task space impedance control of the manipulator driven through the multistage nonlinear flexible transmission
CN117921684B (en) Control method and device of mechanical arm, storage medium and electronic equipment
CN117116459B (en) Active rehabilitation training control method and device for rehabilitation robot
WO2022004568A1 (en) Control system, control method, and program
JP2019133537A (en) Actuator controller and actuator control method
JP2007014126A (en) System identification device
JP2009032009A (en) Linear actuator

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20221008

Address after: 311399 Room 701, Building B, Tianmu Pharmaceutical Port Pharmaceutical Industry Incubation Park, No. 88, Jiuzhou Street, Jinnan Street, Lin'an District, Hangzhou, Zhejiang

Patentee after: Hangzhou Langbokang Medical Technology Co.,Ltd.

Address before: 311100 508, 5th floor, building 4, No. 5, Lvtai Road, Zhongtai street, Yuhang District, Hangzhou City, Zhejiang Province

Patentee before: Hangzhou lancet robot Co.,Ltd.

TR01 Transfer of patent right