CN115317132A - Virtual clamp control method and system for robot-assisted joint replacement surgery - Google Patents

Virtual clamp control method and system for robot-assisted joint replacement surgery Download PDF

Info

Publication number
CN115317132A
CN115317132A CN202210920977.XA CN202210920977A CN115317132A CN 115317132 A CN115317132 A CN 115317132A CN 202210920977 A CN202210920977 A CN 202210920977A CN 115317132 A CN115317132 A CN 115317132A
Authority
CN
China
Prior art keywords
robot
virtual
mode
clamp
constraint
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.)
Pending
Application number
CN202210920977.XA
Other languages
Chinese (zh)
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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
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 Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202210920977.XA priority Critical patent/CN115317132A/en
Publication of CN115317132A publication Critical patent/CN115317132A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61F—FILTERS IMPLANTABLE INTO BLOOD VESSELS; PROSTHESES; DEVICES PROVIDING PATENCY TO, OR PREVENTING COLLAPSING OF, TUBULAR STRUCTURES OF THE BODY, e.g. STENTS; ORTHOPAEDIC, NURSING OR CONTRACEPTIVE DEVICES; FOMENTATION; TREATMENT OR PROTECTION OF EYES OR EARS; BANDAGES, DRESSINGS OR ABSORBENT PADS; FIRST-AID KITS
    • A61F2/00—Filters implantable into blood vessels; Prostheses, i.e. artificial substitutes or replacements for parts of the body; Appliances for connecting them with the body; Devices providing patency to, or preventing collapsing of, tubular structures of the body, e.g. stents
    • A61F2/02—Prostheses implantable into the body
    • A61F2/30—Joints
    • A61F2/46—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor
    • A61F2/4603—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor for insertion or extraction of endoprosthetic joints or of accessories thereof
    • A61F2/4609—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor for insertion or extraction of endoprosthetic joints or of accessories thereof of acetabular cups
    • 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
    • A61F—FILTERS IMPLANTABLE INTO BLOOD VESSELS; PROSTHESES; DEVICES PROVIDING PATENCY TO, OR PREVENTING COLLAPSING OF, TUBULAR STRUCTURES OF THE BODY, e.g. STENTS; ORTHOPAEDIC, NURSING OR CONTRACEPTIVE DEVICES; FOMENTATION; TREATMENT OR PROTECTION OF EYES OR EARS; BANDAGES, DRESSINGS OR ABSORBENT PADS; FIRST-AID KITS
    • A61F2/00—Filters implantable into blood vessels; Prostheses, i.e. artificial substitutes or replacements for parts of the body; Appliances for connecting them with the body; Devices providing patency to, or preventing collapsing of, tubular structures of the body, e.g. stents
    • A61F2/02—Prostheses implantable into the body
    • A61F2/30—Joints
    • A61F2/46—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor
    • A61F2002/4632—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor using computer-controlled surgery, e.g. robotic surgery
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61F—FILTERS IMPLANTABLE INTO BLOOD VESSELS; PROSTHESES; DEVICES PROVIDING PATENCY TO, OR PREVENTING COLLAPSING OF, TUBULAR STRUCTURES OF THE BODY, e.g. STENTS; ORTHOPAEDIC, NURSING OR CONTRACEPTIVE DEVICES; FOMENTATION; TREATMENT OR PROTECTION OF EYES OR EARS; BANDAGES, DRESSINGS OR ABSORBENT PADS; FIRST-AID KITS
    • A61F2/00—Filters implantable into blood vessels; Prostheses, i.e. artificial substitutes or replacements for parts of the body; Appliances for connecting them with the body; Devices providing patency to, or preventing collapsing of, tubular structures of the body, e.g. stents
    • A61F2/02—Prostheses implantable into the body
    • A61F2/30—Joints
    • A61F2/46—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor
    • A61F2002/4688—Special tools or methods for implanting or extracting artificial joints, accessories, bone grafts or substitutes, or particular adaptations therefor having operating or control means

Landscapes

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

Abstract

The invention belongs to the technical field of human-computer interaction control, and discloses a virtual fixture control method and a virtual fixture control system for a robot-assisted joint replacement surgery, wherein (1) the position, the speed and the force of a robot relative to each virtual fixture curved surface are calculated in real time, and a tangent plane where the virtual fixture curved surface closest to the position of the robot is located and the curvature of the robot in the tangential speed direction are calculated; (2) Dividing the area where the robot is located and an impedance control mode based on the calculation result of the step (1); (3) Calculating the impedance control parameter change and controller acceleration output of the robot relative to each virtual clamp constraint surface; (4) And coupling the constraints of the curved surfaces of the virtual clamps, determining the optimal acceleration output based on a quadratic programming algorithm, and controlling the robot to move based on the obtained optimal acceleration output. The invention improves the operation quality, optimizes the operation feeling of doctors and greatly improves the restraint effect of the virtual clamp.

Description

Virtual clamp control method and system for robot-assisted joint replacement surgery
Technical Field
The invention belongs to the field related to human-computer interaction control, and particularly relates to a virtual clamp control method and system for robot-assisted joint replacement surgery.
Background
Surgical robots have evolved with a rapid leap over the last 20 years. In the orthopedic field, robot-assisted surgical systems based on virtual clamp constraints are widely used, with virtual clamps providing guidance, positioning, disabling, force scaling, etc. Clinical application proves that the virtual clamp can improve the precision and quality of the operation, shorten the recovery time of patients and reduce the operation difficulty of surgeons.
The virtual fixture is an algorithm for real-time constraint of the motion state of the robot in human-computer interaction, and is also called as active constraint or motion constraint. The method is generally divided into three parts of geometric definition, state evaluation and constraint execution. The constraint form of the virtual jig may be divided into a wire guide type constraint and a region prohibition type constraint. The constraint effect of the virtual fixture can be divided into force constraint and position constraint, the force constraint only provides reverse force feedback when the robot violates the virtual fixture, and is usually realized by direct force control or impedance control based on a moment ring, and the position constraint ensures that the virtual fixture has certain constraint position precision and is usually realized based on the position or speed servo of the robot. The force constraint is realized by directly controlling the moment of the robot joint, and the robot joint presents impedance characteristics when contacting with the external environment, and has better stability. The position-constrained virtual fixture has admittance characteristics when contacting with the external environment, and is easy to generate collision rebound or instability.
In the case of hip replacement surgery, the acetabular socket is surgically ground to a size and shape that fits perfectly with the prosthesis. Usually, a conical virtual fixture is constructed with the acetabular socket as the target, and the function includes 1. Before filing, the surgeon is guided to precisely drag a bone drill bit (hereinafter referred to as a robot) fixed to a robot to the operation position (conical vertex). 2. When the grinding is frustrated, the constraint robot cannot cross the vertex of the cone, and excessive grinding is prevented. 3. After the bruise, the robot can be easily pulled back into the awl to perform the next surgical operation. Therefore, the orthopedic surgery scene needs the virtual clamp to have higher position constraint capacity to improve the surgery precision, needs to ensure stability under complex frustration interaction force, and is difficult to achieve a better virtual clamp constraint effect through a single position constraint and force constraint algorithm.
Disclosure of Invention
Aiming at the defects or improvement requirements of the prior art, the invention provides a virtual clamp control method and a virtual clamp control system for a robot-assisted joint replacement operation, which can meet the requirements of high position precision and high environment interaction stability of virtual clamp constraint in an orthopedic joint replacement operation scene, improve operation quality, optimize doctor operation feeling, greatly improve virtual clamp constraint effect, and meet the requirements of robot guidance, prohibition and free dragging in the orthopedic operation.
To achieve the above objects, according to one aspect of the present invention, there is provided a virtual jig control method for robot-assisted joint replacement surgery, the control method including the steps of:
(1) Calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane where the virtual fixture curved surface closest to the position of the robot is located and the curvature of the robot in the tangential speed direction;
(2) Dividing the area where the robot is located and an impedance control mode based on the calculation result of the step (1);
(3) Calculating the impedance control parameter change and controller acceleration output of the robot relative to each virtual clamp constraint surface;
(4) And coupling the constraints of the curved surfaces of the virtual clamps, determining the optimal acceleration output based on a quadratic programming algorithm, and controlling the motion of the robot based on the obtained optimal acceleration output.
Further, the optimal acceleration output is transmitted to the robot, joint torque is calculated based on a robot dynamic model to serve as an instruction to be transmitted to the robot, and then the robot is controlled to move to achieve virtual clamp constraint; obtaining a joint torque output value tau through robot dynamics based on optimal acceleration output c Comprises the following steps:
Figure BDA0003777537830000021
wherein J T (q) is the Jacobian transpose of robot velocity, M (q) is the robot inertia matrix,
Figure BDA0003777537830000031
is the robot centripetal and coriolis force matrix and G (q) is the robot gravity matrix.
Further, the virtual fixture divides the cartesian working space of the robot into five regions, which correspond to the five regions
Figure BDA0003777537830000032
Dividing impedance control into five modes in a phase diagram, wherein each area corresponds to a control target and a control strategy responding to the impedance mode; the five regions include: a free area, a constraint area, a forbidden area, a buffer area and a return area; the corresponding five impedance control modes include: a free mode, a constrained mode, a disabled mode, a buffered mode, and a return mode; and by dividing the areas, corresponding impedance control modes are applied to different areas, so that the virtual clamp constraint target is realized.
Further, in the free area, the robot appears compliant with the human and the external environment, the control right is held in the human hand, the robot acts as a force relay between the human and the external environment;
the constraint area is near the boundary of the virtual fixture, and the normal external force delta F faces the boundary of the virtual fixture, so that the situation that a person tries to drag the robot to break through the boundary of the virtual fixture is shown, and at the moment, the control right along the normal direction of the boundary of the virtual fixture is on the robot, and the dynamic balance of the force is kept;
a forbidden area, wherein the robot needs to quickly return to the boundary of the virtual fixture in the boundary of the virtual fixture, and the safety of human-computer interaction is ensured;
a buffer area established between the free area and the constrained area to increase the space and time for the robot to contact the boundary of the virtual fixture;
the return zone, also established between the free zone and the constraint zone, is intended to allow the system to quickly and progressively regain human control.
Further, the basic impedance control principle adopted is as follows:
Figure BDA0003777537830000033
Figure BDA0003777537830000034
wherein the values of the variables Δ x,
Figure BDA0003777537830000035
the position and acceleration of the robot tool center point TCP relative to the virtual fixture boundary, respectively.
Figure BDA0003777537830000036
For impedance control acceleration output, H, D and K are respectively a virtual inertia term, a velocity damping term and a position rigidity term,
Figure BDA0003777537830000037
acceleration output as force term in impedance control, wherein F L Is a force input limit, wherein F L As force input limit, sat (Δ F, F) L ) Is a saturation function, limiting the force input Δ F to not exceed F L ，
Figure BDA0003777537830000041
Figure BDA0003777537830000042
Acceleration outputs of a damping term and a stiffness term respectively,
Figure BDA0003777537830000043
the amount is directly compensated for acceleration.
Further, the switching conditions of the impedance control mode are:
when the controller is in free mode, when
Figure BDA0003777537830000044
When the controller is switched from the free mode to the buffer mode;
Figure BDA0003777537830000045
the variable used to measure the boundary state between the robot and the virtual fixture, called the equivalent decay rate,
Figure BDA0003777537830000046
switching a boundary constant for the set buffer mode;
when the controller is in the buffer mode, at
Figure BDA0003777537830000047
When the controller is switched from the trimming die mode to the prohibition mode;
Figure BDA0003777537830000048
is the maximum acceleration limit of the robot;
when the controller is in the constraint mode, the robot is subjected to an external force F e Towards the boundary of the virtual fixture, the operation intention of the robot is to try to make the robot enter the virtual fixture, the controller is in a constraint mode, and the robot is controlled to stop on the boundary of the virtual fixture; when F is e When the robot moves towards the free area, the operation intention of the robot is attempted to make the robot leave the virtual clamp, and the controller enters a return mode;
controller output when in constrained mode
Figure BDA0003777537830000049
When the position of the robot enters the virtual fixture, the controller is switched to a forbidden mode;
when the controller is in the return mode,
Figure BDA00037775378300000410
this means that the robot must enter the free zone even if it decelerates at the maximum deceleration, and the controller enters the free mode.
Further, the target is achieved by adjusting the rate of change of the damping and the restraining force in the damping mode, specifically:
determining intermediate variations in robot state and controller parameters
Figure BDA00037775378300000411
The damping and restraining force rate of change is then:
Figure BDA00037775378300000412
Figure BDA0003777537830000051
in the formula (I), the compound is shown in the specification,
Figure BDA0003777537830000052
K v is the desired impedance stiffness in the constrained mode.
Further, from F L =0 the slope of the mode switching line for constrained mode and return mode is:
Figure BDA0003777537830000053
the damping and restraining force variation formula is:
Figure BDA0003777537830000054
Figure BDA0003777537830000055
in the formula, D c ,K c Damping, stiffness, D for present impedance controllers f For the desired damping of the impedance controller in free mode,
Figure BDA0003777537830000056
is the maximum speed limit of the robotA value;
when the robot slides on the surface of the virtual clamp, in order to enable the motion direction of the robot to be always along the tangential direction of the curved surface of the virtual clamp, the centripetal force needs to be compensated in the normal direction, which is expressed by compensating the centripetal acceleration:
Figure BDA0003777537830000057
where p is the radius of curvature at the virtual gripper closest to the robot,
Figure BDA0003777537830000058
and projecting the speed of the robot in the tangential direction of the curved surface of the virtual clamp.
Further, the acceleration domain is divided into a feasible domain and a forbidden domain by the acceleration calculated by the curved surface of the single virtual clamp, and the intersection of the feasible domains obtained by the curved surfaces of all the virtual clamps forms a combined feasible domain; the feasible point closest to the optimal target is found in the combined feasible domain and is expressed as a typical quadratic programming problem:
Figure BDA0003777537830000059
Figure BDA00037775378300000510
where n represents the normal unit vector towards the exclusion area,
Figure BDA0003777537830000061
representing the acceleration scalar output by the single virtual fixture surface constraint controller, and N representing the number of virtual fixture surfaces.
The invention also provides a virtual clamp control system for the robot-assisted joint replacement surgery, which controls the robot by the virtual clamp control method for the robot-assisted joint replacement surgery, and comprises the following steps:
the control system comprises a virtual clamp definition module, a relative position resolving module, a multi-mode impedance control module, a multi-virtual clamp curved surface constraint coupling module and a robot joint torque command output module;
the virtual clamp definition module is used for defining the shape of a virtual clamp and deconstructing the complex virtual clamp into a plurality of intersected virtual clamp curved surfaces;
the relative position resolving module is used for calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane at the virtual fixture curved surface closest to the position of the robot and the curvature of the robot in the tangential speed direction;
the multi-modal impedance control module is used for dividing the area where the robot is located and an impedance control mode according to the state of the robot relative to the virtual clamp, and calculating the impedance control parameter change and the controller acceleration output relative to each virtual clamp constraint surface;
the multi-virtual fixture curved surface constraint coupling module is used for coupling the constraints of a plurality of virtual fixture surfaces and searching the optimal acceleration output through a quadratic programming algorithm;
and the robot joint torque command output module is used for outputting and calculating robot joint torque based on a robot dynamic model and the optimal acceleration from the multi-virtual-fixture curved-surface constraint coupling module, and further controlling the robot to move so as to realize virtual fixture constraint.
Generally, compared with the prior art, the virtual clamp control method and system for the robot-assisted joint replacement surgery provided by the invention mainly have the following beneficial effects:
1. the robot joint torque is directly controlled based on robot dynamics, and high position gain is not arranged in the control ring, so that the robot has the capability of stably contacting with a high-rigidity environment, and the requirements of orthopedic surgery scenes are met.
2. Five impedance control modes are adopted to implement virtual clamp constraint, the control strategies of a free area and a constraint area are independent, and the free area and the constraint area are decoupled through a buffer area and a return area; the control right of the free area is in the human body, and other compliant dragging algorithms can be further applied; the control right of the constraint area is controlled by the robot, the customized constraint effect can be realized, and the application range of the method is greatly widened.
3. The strategy of automatic switching between control modes is formulated, so that the robot can have a better constraint effect under any motion state and stress condition.
4. And applying a self-adaptive variable impedance control algorithm in the buffer area and the return area to realize virtual clamp constraint with high position precision.
5. And coupling the curved surface constraint effects of the virtual fixtures by adopting a quadratic programming algorithm, and coupling the curved surface constraints of any virtual fixture to realize the high-precision constraint of the complex virtual fixture geometry.
Drawings
FIG. 1 is a flow chart of a virtual clamp control method for robotic-assisted joint replacement surgery provided by the present invention;
FIG. 2 is a view showing a scene of a conical virtual fixture involved in a virtual fixture control method for a robot-assisted joint replacement surgery according to the present invention;
FIG. 3 is a schematic diagram of the division of the virtual fixture in Cartesian space according to the present invention;
FIG. 4 is a graph of impedance control pattern partitioning provided by the present invention
Figure BDA0003777537830000071
And (4) phase diagrams.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not limit the invention. In addition, the technical features involved in the respective embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
Referring to fig. 1, 2, 3 and 4, the invention provides a virtual clamp control method for robot-assisted joint replacement surgery, which divides a virtual clamp action region in a cartesian space, designs virtual clamp control with high position constraint accuracy based on multi-mode adaptive variable impedance control, realizes virtual clamp constraint of complex geometric configuration through centripetal force compensation and multi-virtual clamp curved surface constraint coupling algorithm, and solves joint control force based on robot dynamics to implement virtual clamp constraint. And the control of operation requirements such as guiding, forbidding, free dragging and the like under the constraint of a complex virtual clamp in the joint replacement operation is completed.
The control method mainly comprises the following steps:
step one, determining the shape of a virtual clamp, and deconstructing the virtual clamp with a complex geometric configuration into a plurality of intersected virtual clamp curved surfaces.
Specifically, a virtual fixture constraint surface is constructed, a complex virtual fixture can be deconstructed into a plurality of intersected constraint surfaces, and a solution algorithm for a point of any point in space closest to the virtual fixture surface and a tangent plane at the point is determined.
In this embodiment, the conical virtual fixture is deconstructed into two curved surfaces intersecting at opposite sides to solve the geometrical discontinuity of the conical virtual fixture near the vertex. When the robot is positioned near the vertex of the cone and the robot is far away from the curved surface of the near-side virtual clamp, the constraint of the curved surface of the near-side virtual clamp fails, and the constraint of the curved surface of the opposite-side virtual clamp takes effect, so that the robot is effectively limited in the cone.
And step two, calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane where the virtual fixture curved surface closest to the position of the robot is located and the curvature of the robot in the tangential speed direction.
Specifically, the position x of the robot is acquired r Speed of the motor
Figure BDA0003777537830000081
And the external force F borne by the robot e Calculating the position of the nearest point on the curved surface of the virtual fixture relative to the TCP of the robot, and obtaining the normal position and the normal speed of the robot relative to the curved surface of the virtual fixture according to the space projection principle
Figure BDA0003777537830000082
And an outward normal force Δ F.
And step three, dividing the area where the robot is located and the impedance control mode based on the calculation result of the step two.
The virtual fixture divides the Cartesian working space of the robot into five areas which are correspondingly located
Figure BDA0003777537830000083
The phase diagram divides the impedance control into five modes. Each zone corresponds to a control objective and control strategy responsive to the impedance mode.
The five regions include: a free area, a constraint area, a forbidden area, a buffer area and a return area; the corresponding five impedance control modes include: free mode, constrained mode, disabled mode, buffered mode, and return mode. And by dividing the areas, corresponding impedance control modes are applied to different areas, so that the virtual clamp constraint target is realized.
Free area, the robot appears to be compliant with the human and the external environment, the control authority is held in the human hand, and the robot acts as a force relay between the human and the external environment. The robot has no position control target, so the stiffness K of the impedance controller f =0. To accommodate human motion, there should be low damping D f The general value range is [10,50 ]]kg/s, in a human-machine operation scenario, and set up
Figure BDA0003777537830000091
The value of the virtual inertia H is [0.5]And (kg). Wherein
Figure BDA0003777537830000092
The device is used for limiting the maximum speed and the maximum acceleration of the robot and ensuring the safety of human-computer interaction;
Figure BDA0003777537830000093
has a general value range of [0.1,1%]m/s，
Figure BDA0003777537830000094
The general value range is [3,15%]m/s 2 。
A constraint region near the virtual fixture boundary, with Δ F towards the virtual fixture boundary. The robot is dragged to break through the boundary of the virtual clamp by a person, the control right along the normal direction of the boundary of the virtual clamp is kept on the robot, and the dynamic balance of the force is kept so that F L And =0. By means of high rigidity K c ，K c Has a general value of [1000,5000]N/m. Automatic fast convergence of position error, coupled with over-damping
Figure BDA0003777537830000095
Prevent oscillation in convergence error. The robot can move freely in the tangential direction of the virtual clamp boundary, the robot still keeps the low impedance parameter of the free mode under the control of the palm of the person.
And in the forbidden area, the robot needs to quickly return to the boundary of the virtual fixture in the boundary of the virtual fixture, and meanwhile, the safety of human-computer interaction is ensured. Limiting robot to maximum acceleration toward nearest virtual fixture boundary
Figure BDA0003777537830000096
The purpose is to accelerate the robot back to the virtual gripper boundary with maximum force.
And a buffer area established between the free area and the constraint area, and increasing the space and time for the robot to contact the boundary of the virtual fixture. The buffer mode has a definite control target, the initial state and control parameters of the robot in the buffer mode are low impedance in the free mode, and the termination state is high impedance in the constraint mode. The buffer area controls the movement of the robot based on the self-adaptive variable impedance, and the aim is that when the robot passes through the buffer area and is buffered to the boundary of the virtual clamp, the movement in the normal direction can be stably stopped, namely delta x n →0，
Figure BDA0003777537830000101
For the controller, it is necessary to adjust the impedance parameter K in this process f →K c ，D f →D c And F L →0。
The return zone, also established between the free zone and the constraint zone, is aimed at allowing the system to quickly and progressively regain human control. Control parameters are rapidly adjusted from high impedance to low impedance in the constrained mode c →0，D c →D f Simultaneously adjust
Figure BDA0003777537830000102
And fourthly, calculating the impedance control parameter change of the robot relative to each virtual clamp constraint surface and the acceleration output of the controller by adopting a self-adaptive impedance control algorithm.
The basic impedance control principle adopted is as follows:
Figure BDA0003777537830000103
Figure BDA0003777537830000104
wherein the values of the variables Δ x,
Figure BDA0003777537830000105
the position and acceleration of the robot tool center point TCP relative to the virtual fixture boundary, respectively.
Figure BDA0003777537830000106
For impedance control of acceleration output, H, D, K are terms of virtual inertia, velocity damping and position stiffness,
Figure BDA0003777537830000107
acceleration output as force term in impedance control, wherein F L As force input limit, sat (Δ F, F) L ) Is a saturation function, limiting the force input Δ F to not exceed F L ，
Figure BDA0003777537830000108
Acceleration input in terms of damping and stiffness respectivelyAnd then the mixture is discharged out of the furnace,
Figure BDA0003777537830000109
directly compensate for the amount of acceleration.
The switching conditions among the five control modes can be reasonably set as follows:
when the controller is in free mode, when
Figure BDA00037775378300001010
The controller switches from free mode to buffered mode.
Figure BDA00037775378300001011
The thickness of the buffer area can be adjusted by autonomous setting.
When the controller is in the buffer mode, at
Figure BDA00037775378300001012
When the controller switches from the slave mode to the disable mode.
When the controller is in the constrained mode, when F e Towards the virtual clamp boundary, the operation intention of the robot is to make the robot enter the virtual clamp, and the controller is in a constraint mode to control the robot to stop on the virtual clamp boundary. When F is present e Towards the free area, the human-to-robot operation intent attempts to move the robot away from the virtual gripper and the controller enters a return mode. Controller output when in constrained mode
Figure BDA0003777537830000111
When the position of the robot has penetrated into the virtual gripper, the controller switches to the disabled mode.
When the controller is in the return mode,
Figure BDA0003777537830000112
this means that the robot must enter the free zone even if it decelerates at the maximum deceleration, and the controller enters the free mode.
The goal is achieved by adjusting the rate of change of the damping and restraining forces in the buff mode. The specific implementation mode is as follows:
determining a variable which can measure the boundary state of the robot and the virtual clamp and is called as an equivalent attenuation speed:
Figure BDA0003777537830000113
determining intermediate variations in robot state and controller parameters
Figure BDA0003777537830000114
The damping and restraining force rate of change is then:
Figure BDA0003777537830000115
Figure BDA0003777537830000116
the practical implementation method of the return mode comprises the following steps:
from F L =0 the slope of the mode switching line for constrained mode and return mode can be found as:
Figure BDA0003777537830000117
the damping and restraining force variation equations are:
Figure BDA0003777537830000118
Figure BDA0003777537830000121
when the robot slides on the surface of the virtual clamp, in order to enable the motion direction of the robot to be always along the tangential direction of the curved surface of the virtual clamp, a proper centripetal force needs to be compensated in the normal direction, and in the embodiment, the centripetal acceleration is compensated:
Figure BDA0003777537830000122
where p is the radius of curvature at the virtual gripper closest to the robot,
Figure BDA0003777537830000123
the robot speed is projected tangentially on the curved surface of the virtual fixture.
And step five, coupling the constraints of the curved surfaces of the virtual clamps, and determining the optimal acceleration output based on a quadratic programming algorithm.
In the embodiment, the intersection angle of the curved surfaces of the two virtual clamps is a cone vertex angle and may be non-orthogonal, so that the complex virtual clamp in a three-dimensional space cannot be realized simply through dimension extension of orthogonal direction decoupling of the controller. Therefore, the present embodiment proposes a constraint of coupling a plurality of non-orthogonal virtual fixture curved surfaces by using a quadratic programming algorithm.
Specifically, the method comprises the following steps: first, an optimal target is defined as the acceleration output by the controller in the free mode. The acceleration domain is divided into a feasible domain and a forbidden domain by the acceleration calculated by the curved surface of the single virtual clamp. And the intersection of the feasible regions obtained by all the virtual fixture curved surfaces forms a combined feasible region. The feasible point closest to the optimal target is found in the combined feasible domain, which can be expressed as a typical quadratic programming problem:
Figure BDA0003777537830000124
Figure BDA0003777537830000125
where n represents the normal unit vector towards the exclusion area,
Figure BDA0003777537830000126
representing the acceleration scalar output by a single virtual fixture surface constraint controller, N represents the number of virtual fixture surfaces, N =2 in this example. The method ensures that the constraints of the curved surfaces of the virtual clamps are effective at the same time and conforms to the movement intention of the human body as much as possible.
Impedance output item
Figure BDA0003777537830000127
Have different physical meanings. For the virtual clamp constraint task, the motion is always towards the energy attenuation direction or the direction capable of compensating the error. Therefore, it is not only easy to use
Figure BDA0003777537830000131
Toward the origin side, and
Figure BDA0003777537830000132
back to the origin side.
Figure BDA0003777537830000133
And the optimal values of all the items are directly superposed to obtain the acceleration output result of the controller, so that the rationality of the secondary optimization result is ensured.
And step six, transmitting the optimal acceleration output to the robot, calculating a joint torque command based on the robot dynamic model, and further controlling the robot to move so as to realize virtual clamp constraint.
The acceleration value can not be directly used as a control command to control the robot, and a joint torque output value tau is obtained through the dynamics of the robot c Comprises the following steps:
Figure BDA0003777537830000134
wherein J T (q) is the Jacobian transpose of robot velocity, M (q) is the robot inertia matrix,
Figure BDA0003777537830000135
is a machineHuman centripetal and coriolis force matrices, G (q) is a robot gravity matrix, which can be directly solved from the underlying robot dynamics. The joint torque value is directly transmitted to the robot as a control instruction, and the robot bottom controller performs torque servo control to realize the virtual clamp constraint effect.
The invention also provides a virtual clamp control system for the robot-assisted joint replacement surgery, which comprises a virtual clamp definition module, a relative position resolving module, a multi-mode impedance control module, a multi-virtual clamp curved surface constraint coupling module and a robot joint torque command output module.
The virtual clamp definition module is used for defining the shape of a virtual clamp and deconstructing the complex virtual clamp into a plurality of intersected virtual clamp curved surfaces.
The relative position calculating module is used for calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane at the virtual fixture curved surface closest to the position of the robot and the curvature of the robot in the tangential speed direction.
The multi-mode impedance control module is used for dividing the area where the robot is located and an impedance control mode according to the state of the robot relative to the virtual clamp, and calculating the impedance control parameter change and the controller acceleration output relative to each virtual clamp constraint surface by applying a self-adaptive variable impedance control algorithm.
The multi-virtual fixture curved surface constraint coupling module is used for coupling the constraints of the multiple virtual fixture surfaces and finding the optimal acceleration output through a quadratic programming algorithm.
And the robot joint torque command output module is used for outputting and calculating a robot joint torque command based on a robot dynamic model and the optimal acceleration from the multi-virtual-fixture curved-surface constraint coupling module, and further controlling the robot to move so as to realize virtual fixture constraint.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (10)

1. A virtual clamp control method for robotic assisted joint replacement surgery, the method comprising the steps of:
(1) Calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane where the virtual fixture curved surface closest to the position of the robot is located and the curvature of the robot in the tangential speed direction;
(2) Dividing the area where the robot is located and an impedance control mode based on the calculation result of the step (1);
(3) Calculating the impedance control parameter change and controller acceleration output of the robot relative to each virtual clamp constraint surface;
(4) And coupling the constraints of the curved surfaces of the virtual clamps, determining the optimal acceleration output based on a quadratic programming algorithm, and controlling the motion of the robot based on the obtained optimal acceleration output.
2. The virtual jig control method for robot-assisted joint replacement surgery of claim 1, wherein: the optimal acceleration output is transmitted to the robot, joint torque is calculated based on a robot dynamic model to serve as an instruction to be transmitted to the robot, and then the robot is controlled to move to achieve virtual clamp constraint; obtaining a joint torque output value tau through robot dynamics based on optimal acceleration output c Comprises the following steps:
Figure FDA0003777537820000011
wherein J T (q) is the Jacobian transpose of robot velocity, M (q) is the robot inertia matrix,
Figure FDA0003777537820000012
is the robot centripetal and coriolis force matrix and G (q) is the robot gravity matrix.
3. The virtual jig control method for robot-assisted joint replacement surgery of claim 1, wherein: the virtual fixture divides the Cartesian working space of the robot into five areas which are correspondingly located
Figure FDA0003777537820000013
In the phase diagram, impedance control is divided into five modes, and each area corresponds to a control target and a control strategy responding to the impedance mode; the five regions include: a free area, a constraint area, a forbidden area, a buffer area and a return area; the corresponding five impedance control modes include: a free mode, a constrained mode, a disabled mode, a buffered mode, and a return mode; and by dividing the areas, corresponding impedance control modes are applied to different areas, so that the virtual clamp constraint target is realized.
4. The virtual jig control method for robot-assisted joint replacement surgery of claim 3, wherein: in the free area, the robot appears to be compliant with the human and external environment, the control right is held in the human hand, the robot acts as a force relay between the human and the external environment;
the constraint area is near the boundary of the virtual clamp, and the normal external force delta F faces the boundary of the virtual clamp, so that the situation that a person tries to drag the robot to break through the boundary of the virtual clamp is shown, at the moment, the control right along the normal direction of the boundary of the virtual clamp is on the robot, and the dynamic balance of the force is kept;
in the forbidden region, the robot needs to quickly return to the boundary of the virtual clamp in the boundary of the virtual clamp, and meanwhile, the safety of human-computer interaction is ensured;
a buffer area established between the free area and the constrained area to increase the space and time for the robot to contact the boundary of the virtual fixture;
the return zone, also established between the free zone and the constraint zone, is aimed at allowing the system to quickly and progressively regain human control.
5. The virtual jig control method for robot-assisted joint replacement surgery of claim 3, wherein: the basic impedance control principle adopted is as follows:
Figure FDA0003777537820000021
Figure FDA0003777537820000022
wherein the values of the variables Δ x,
Figure FDA0003777537820000023
the position and acceleration of the robot tool centre point TCP relative to the virtual fixture boundary,
Figure FDA0003777537820000024
for impedance control acceleration output, H, D and K are respectively a virtual inertia term, a velocity damping term and a position rigidity term,
Figure FDA0003777537820000025
acceleration output as force term in impedance control, wherein F L As force input limit, sat (Δ F, F) L ) Is a saturation function, limiting the force input Δ F to not exceed F L ，
Figure FDA0003777537820000026
Acceleration outputs of a damping term and a stiffness term respectively,
Figure FDA0003777537820000027
the amount is directly compensated for acceleration.
6. The virtual jig control method for robot-assisted joint replacement surgery of claim 5, wherein: the switching conditions of the impedance control mode are as follows:
when the controller is in selfFrom a mode when
Figure FDA0003777537820000031
When the controller is switched from the free mode to the buffer mode;
Figure FDA0003777537820000032
the variable used to measure the boundary state between the robot and the virtual fixture, called the equivalent decay rate,
Figure FDA0003777537820000033
switching a boundary constant for the set buffer mode;
when the controller is in the buffer mode, at
Figure FDA0003777537820000034
When the controller is switched from the trimming die mode to the prohibition mode;
Figure FDA0003777537820000035
is the maximum acceleration limit of the robot;
when the controller is in the constraint mode, the robot is subjected to an external force F e Towards the boundary of the virtual fixture, the operation intention of the robot is to try to make the robot enter the virtual fixture, the controller is in a constraint mode, and the robot is controlled to stop on the boundary of the virtual fixture; when F is e When the robot moves towards the free area, the operation intention of the robot is attempted to make the robot leave the virtual clamp, and the controller enters a return mode;
controller output when in constrained mode
Figure FDA0003777537820000036
When the position of the robot is deep into the virtual fixture, the controller is switched to a forbidden mode;
when the controller is in the return mode,
Figure FDA0003777537820000037
indicates that the robot is in factThe maximum deceleration must also enter the free zone, when the controller enters the free mode.
7. The virtual jig control method for robot-assisted joint replacement surgery of claim 6, wherein: the target is achieved by adjusting the change rate of the damping and the limiting force in the buffer mode, and the method specifically comprises the following steps:
determining intermediate variations in robot state and controller parameters
Figure FDA0003777537820000038
The damping and restraining force rate of change is then:
Figure FDA0003777537820000039
Figure FDA00037775378200000310
in the formula (I), the compound is shown in the specification,
Figure FDA0003777537820000041
K v stiffness is desired for impedance in the constrained mode.
8. The virtual jig control method for robot-assisted joint replacement surgery of claim 7, wherein: from F L =0 the slope of the mode switching line for constrained mode and return mode is:
Figure FDA0003777537820000042
the damping and restraining force variation formula is:
Figure FDA0003777537820000043
Figure FDA0003777537820000044
in the formula, D c ,K c Damping and stiffness, D, respectively, of the present impedance controller f For the desired damping of the impedance controller in free mode,
Figure FDA0003777537820000045
is the maximum speed limit of the robot;
when the robot slides on the surface of the virtual clamp, in order to enable the motion direction of the robot to be always along the tangential direction of the curved surface of the virtual clamp, the centripetal force needs to be compensated in the normal direction, which is expressed by compensating the centripetal acceleration:
Figure FDA0003777537820000046
where p is the radius of curvature at the virtual gripper closest to the robot,
Figure FDA0003777537820000047
the robot speed is projected tangentially on the curved surface of the virtual fixture.
9. The virtual jig control method for robot-assisted joint replacement surgery of claim 1, wherein: the acceleration domain is divided into a feasible domain and a forbidden domain by the acceleration obtained by the calculation of the curved surface of the single virtual clamp, and the intersection of the feasible domains obtained by the curved surfaces of all the virtual clamps forms a combined feasible domain; the feasible point closest to the optimal target is found in the combined feasible domain and is expressed as a typical quadratic programming problem:
Figure FDA0003777537820000051
Figure FDA0003777537820000052
where n represents a normal unit vector towards the exclusion area,
Figure FDA0003777537820000053
representing the acceleration scalar output by the single virtual fixture surface constraint controller, and N representing the number of virtual fixture surfaces.
10. A virtual jig control system for robot-assisted joint replacement surgery for controlling a robot using the virtual jig control method for robot-assisted joint replacement surgery according to any one of claims 1 to 9, characterized in that:
the control system comprises a virtual clamp definition module, a relative position resolving module, a multi-mode impedance control module, a multi-virtual clamp curved surface constraint coupling module and a robot joint torque command output module;
the virtual clamp definition module is used for defining the shape of a virtual clamp and deconstructing the complex virtual clamp into a plurality of intersected virtual clamp curved surfaces;
the relative position resolving module is used for calculating the position, the speed and the force of the robot relative to each virtual fixture curved surface in real time, and calculating a tangent plane at the virtual fixture curved surface closest to the position of the robot and the curvature of the robot in the tangential speed direction;
the multi-modal impedance control module is used for dividing the area where the robot is located and an impedance control mode according to the state of the robot relative to the virtual clamp, and calculating the impedance control parameter change and the controller acceleration output relative to each virtual clamp constraint surface;
the multi-virtual fixture curved surface constraint coupling module is used for coupling the constraints of a plurality of virtual fixture surfaces and searching the optimal acceleration output through a quadratic programming algorithm;
and the robot joint torque command output module is used for outputting and calculating robot joint torque based on a robot dynamic model and the optimal acceleration from the multi-virtual-fixture curved-surface constraint coupling module, and further controlling the robot to move so as to realize virtual fixture constraint.
CN202210920977.XA 2022-08-02 2022-08-02 Virtual clamp control method and system for robot-assisted joint replacement surgery Pending CN115317132A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210920977.XA CN115317132A (en) 2022-08-02 2022-08-02 Virtual clamp control method and system for robot-assisted joint replacement surgery

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210920977.XA CN115317132A (en) 2022-08-02 2022-08-02 Virtual clamp control method and system for robot-assisted joint replacement surgery

Publications (1)

Publication Number Publication Date
CN115317132A true CN115317132A (en) 2022-11-11

Family

ID=83918690

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210920977.XA Pending CN115317132A (en) 2022-08-02 2022-08-02 Virtual clamp control method and system for robot-assisted joint replacement surgery

Country Status (1)

Country Link
CN (1) CN115317132A (en)

Similar Documents

Publication Publication Date Title
US11173598B2 (en) System and methods for managing multiple null-space objectives and SLI behaviors
EP3119319B1 (en) Tele-operative surgical systems and methods of control at joint limits using inverse kinematics
US10188471B2 (en) Tele-operative surgical systems and methods of control at joint limits using inverse kinematics
US10029367B2 (en) System and method for managing multiple null-space objectives and constraints
US9510911B2 (en) System and methods for managing multiple null-space objectives and SLI behaviors
CN109700535B (en) Software centric and highly configurable robotic system for surgical and other applications
Bowyer et al. Active constraints/virtual fixtures: A survey
Kazanzides et al. Force sensing and control for a surgical robot.
US20150375394A1 (en) Dynamic physical constraint for hard surface emulation
CN109171965B (en) Control method and system of robot based on virtual fixture
Marayong et al. Speed-accuracy characteristics of human-machine cooperative manipulation using virtual fixtures with variable admittance
US8244386B2 (en) Machine tool system control having automatic safe repositioning
CN108115690A (en) A kind of robot adaptive control system and method
CN115317132A (en) Virtual clamp control method and system for robot-assisted joint replacement surgery
García et al. Bimanual robot control for surface treatment tasks
Chen et al. A Novel Microsurgical Robot with Double-Parallelogram RCM Mechanism and Back-Driven Instrument Translation
Dahroug et al. Unilaterally constrained motion of a curved surgical tool
CN113474130A (en) Method and system for executing preset task by using robot
US20210260760A1 (en) Method and system for transferring an end effector of a robot between one end effector pose and a further end effector pose
Hsu et al. A new adaptive fuzzy hybrid force/position control for intelligent robot deburring
CN116276995B (en) Impedance control-based man-machine cooperation method with zero-space obstacle avoidance capability
Yiwei et al. An Adaptive Auxiliary Framework for Teleoperated Laparoscopic Surgery
Marayong Motion control methods for human-machine cooperative systems
JPH03184786A (en) Track formation system for robot
Ajoudani et al. Replicating Human Stiffness Profile with a Cartesian Impedance Controller in Realtime

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