CN112809686A - Robot body state follow-up control method and device - Google Patents

Robot body state follow-up control method and device Download PDF

Info

Publication number
CN112809686A
CN112809686A CN202110146573.5A CN202110146573A CN112809686A CN 112809686 A CN112809686 A CN 112809686A CN 202110146573 A CN202110146573 A CN 202110146573A CN 112809686 A CN112809686 A CN 112809686A
Authority
CN
China
Prior art keywords
target
posture
follow
current
point
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
CN202110146573.5A
Other languages
Chinese (zh)
Other versions
CN112809686B (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.)
Lancet Robotics 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 CN202110146573.5A priority Critical patent/CN112809686B/en
Publication of CN112809686A publication Critical patent/CN112809686A/en
Application granted granted Critical
Publication of CN112809686B publication Critical patent/CN112809686B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • A61B17/16—Bone cutting, breaking or removal means other than saws, e.g. Osteoclasts; Drills or chisels for bones; Trepans
    • A61B17/17—Guides or aligning means for drills, mills, pins or wires
    • A61B17/1739—Guides or aligning means for drills, mills, pins or wires specially adapted for particular parts of the body
    • A61B17/1742—Guides or aligning means for drills, mills, pins or wires specially adapted for particular parts of the body for the hip
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • A61B17/16—Bone cutting, breaking or removal means other than saws, e.g. Osteoclasts; Drills or chisels for bones; Trepans
    • A61B17/17—Guides or aligning means for drills, mills, pins or wires
    • A61B17/1739—Guides or aligning means for drills, mills, pins or wires specially adapted for particular parts of the body
    • A61B17/1742—Guides or aligning means for drills, mills, pins or wires specially adapted for particular parts of the body for the hip
    • A61B17/1746—Guides or aligning means for drills, mills, pins or wires specially adapted for particular parts of the body for the hip for the acetabulum
    • 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
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in 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
    • 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/4607—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 hip femoral endoprostheses
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1612—Programme controls characterised by the hand, wrist, grip control
    • 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

Landscapes

  • Health & Medical Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • General Health & Medical Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • Veterinary Medicine (AREA)
  • Public Health (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Oral & Maxillofacial Surgery (AREA)
  • Transplantation (AREA)
  • Robotics (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Vascular Medicine (AREA)
  • Cardiology (AREA)
  • Mechanical Engineering (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Dentistry (AREA)
  • Manipulator (AREA)

Abstract

The application provides a robot body state follow-up control method and device, which are applied to the field of machine control, and the method comprises the following steps: acquiring a target pose corresponding to the target point and acquiring a current pose corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished; judging whether the poses of the target point and the cusp are consistent or not; and if the positions of the target point and the pointed point are overlapped but the postures of the target point and the pointed point are not consistent, controlling the grinding tool to move to the target posture according to the follow-up posture information and the impedance posture information. In the above scheme, in the operation process, when the sharpening tool point is located the axis of waiting to polish, but the gesture of sharpening tool point is inconsistent with the gesture of waiting to polish target point on the axis, can adopt follow-up control to control sharpening tool with impedance control's mode to can control sharpening tool and follow the patient and remove when patient's health takes place the displacement, improve sharpening tool's positioning accuracy at the in-process of polishing.

Description

Robot body state follow-up control method and device
Technical Field
The application relates to the field of machine control, in particular to a robot body state follow-up control method and device.
Background
For patients with advanced hip joint diseases, treatment with total hip replacement surgery can effectively improve joint function, thereby alleviating pain symptoms. However, the total hip replacement surgery has relatively high technical requirements on operators, and the problems of fracture, failure in fixing the prosthesis, joint movement disorder, poor function of abductor muscles, prosthesis impact, dislocation of the prosthesis, unequal lower limbs, prosthesis abrasion and the like in the operation can be caused by the poor installation position or angle of the prosthesis.
In the operation process, the operation of an operator can be assisted by adopting the operation robot, so that the fineness, the accuracy and the stability of the operation can be improved. However, because the patient is easy to move in the body position during the operation, especially in the process of acetabular bone contusion or prosthesis placement, the body of the patient is easy to displace along the grinding direction of the grinding device because the counterforce of the contusion and the knocking is large. That is, the robot may be displaced due to the body of the patient, resulting in a lower accuracy of positioning during the grinding process.
Disclosure of Invention
An object of the embodiments of the present application is to provide a robot posture follow-up control method and device, so as to solve the technical problem that the positioning accuracy of a robot is low in the polishing process due to the displacement of the body of a patient.
In order to achieve the above purpose, the technical solutions provided in the embodiments of the present application are as follows:
in a first aspect, an embodiment of the present application provides a robot posture follow-up control method, including: acquiring a target posture and a target position corresponding to the target point, and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished; judging whether the current position is coincident with the target position or not, and judging whether the current posture is consistent with the target posture or not; if the current position is coincident with the target position but the current posture is not consistent with the target posture, determining follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode according to the target posture, the target position, the current posture and the current position; and controlling the grinding tool to move to the target posture according to the follow-up posture information and the impedance posture information. In the above scheme, in the operation process, when the sharpening tool point is located the axis of waiting to polish, but the gesture of sharpening tool point is inconsistent with the gesture of waiting to polish target point on the axis, can adopt follow-up control to control sharpening tool with impedance control's mode to can control sharpening tool and follow the patient and remove when patient's health takes place the displacement, improve sharpening tool's positioning accuracy at the in-process of polishing.
In an optional embodiment of the present application, the determining, according to the target posture, the target position, the current posture and the current position, the follow-up posture information output by the follow-up control mode and the impedance posture information output by the impedance control mode includes: determining a spatial position relationship between the target point and the cusp according to the target posture, the target position, the current posture and the current position; and determining the follow-up attitude information and the impedance attitude information according to the spatial position relationship. In the scheme, the position relation of the point and the target point in the space can be determined according to the current position information of the point of the grinding tool and the position information of the target point, so that the follow-up attitude information corresponding to the follow-up control and the impedance attitude information corresponding to the impedance control can be directly determined according to the position relation of the point and the target point, and the operation process can be simplified on the basis of improving the positioning accuracy of the grinding tool in the grinding process.
In an optional embodiment of the present application, the determining the impedance pose information according to the spatial position relationship includes: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the impedance attitude information according to the attitude error, the current attitude, the current position and an impedance control formula. In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and the impedance attitude information corresponding to the impedance control is determined based on the pose error, the current pose of the cusp and an impedance control formula, so that the impedance control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
In an optional embodiment of the present application, the determining the impedance pose information according to the pose error, the current pose, the current position, and an impedance control formula includes: determining the impedance pose information according to the pose error, the current pose, the current position, and the following formula:
Figure BDA0002930101770000031
wherein M is an inertia matrix corresponding to the grinding tool, q is the current attitude and the current position,
Figure BDA0002930101770000032
is the first derivative of q and is,
Figure BDA0002930101770000033
is the second derivative of q, DdFor a predetermined damping matrix, KdIs a preset spring matrix and is characterized in that,
Figure BDA0002930101770000034
for the pose error, τextIncluding the impedance pose information.
In an optional embodiment of the present application, the determining the follow-up posture information according to the spatial position relationship includes: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the follow-up attitude information according to the pose error, the current attitude and a follow-up control formula. In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and follow-up attitude information corresponding to follow-up control is determined based on the pose error, the current pose of the cusp and a follow-up control formula, so that the follow-up control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
In an optional embodiment of the present application, the obtaining of the target pose and the target position corresponding to the target point includes: acquiring the position coordinates of the target point in an image coordinate system; determining a coordinate system transformation relation between the image coordinate system and a robot coordinate system through an NDI coordinate system; and determining the target posture and the target position according to the position coordinates of the projection point in the image coordinate system and the coordinate system transformation relation. In the above scheme, after the position coordinates of the target point in the image coordinate system are acquired, the image coordinate system and the robot coordinate system may be unified based on the NDI coordinate system, so as to convert the position coordinates in the image coordinate system into pose information in the robot coordinate system.
In an optional embodiment of the present application, after determining whether the current position coincides with the target position, and determining whether the current posture and the target posture coincide with each other, the method further includes: if the current position is not overlapped with the target position, determining follow-up attitude information output by a follow-up control mode according to the target attitude, the target position, the current attitude and the current position; and controlling the grinding tool to move to the target posture and the target position according to the sensor control information and the follow-up posture information. In the above scheme, at the operation in-process, when the axis of polishing is treated in the skew of grinding tool point, can adopt the mode that follow-up control combines sensor control to control grinding tool to can control grinding tool and follow the patient and remove when patient's health takes place the displacement, improve grinding tool's positioning accuracy at the in-process of polishing.
In a second aspect, an embodiment of the present application provides a robot posture follow-up control device, including: the acquisition module is used for acquiring a target posture and a target position corresponding to the target point and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished; the judging module is used for judging whether the current position is superposed with the target position or not and judging whether the current posture is consistent with the target posture or not; a first determining module, configured to determine, according to the target posture, the target position, the current posture and the current position, follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode if the current position coincides with the target position but the current posture and the target posture are not consistent; a first control module for controlling the grinding tool to move to the target attitude according to the follow-up attitude information and the impedance attitude information. In the above scheme, in the operation process, when the sharpening tool point is located the axis of waiting to polish, but the gesture of sharpening tool point is inconsistent with the gesture of waiting to polish target point on the axis, can adopt follow-up control to control sharpening tool with impedance control's mode to can control sharpening tool and follow the patient and remove when patient's health takes place the displacement, improve sharpening tool's positioning accuracy at the in-process of polishing.
In an optional embodiment of the present application, the first determining module is further configured to: determining a spatial position relationship between the target point and the cusp according to the target posture, the target position, the current posture and the current position; and determining the follow-up attitude information and the impedance attitude information according to the spatial position relationship. In the scheme, the position relation of the point and the target point in the space can be determined according to the current position information of the point of the grinding tool and the position information of the target point, so that the follow-up attitude information corresponding to the follow-up control and the impedance attitude information corresponding to the impedance control can be directly determined according to the position relation of the point and the target point, and the operation process can be simplified on the basis of improving the positioning accuracy of the grinding tool in the grinding process.
In an optional embodiment of the present application, the first determining module is further configured to: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the impedance attitude information according to the attitude error, the current attitude, the current position and an impedance control formula. In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and the impedance attitude information corresponding to the impedance control is determined based on the pose error, the current pose of the cusp and an impedance control formula, so that the impedance control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
In an optional embodiment of the present application, the first determining module is further configured to: determining the impedance pose information according to the pose error, the current pose, the current position, and the following formula:
Figure BDA0002930101770000051
wherein M is an inertia matrix corresponding to the grinding tool, q is the current attitude and the current position,
Figure BDA0002930101770000052
is the first derivative of q and is,
Figure BDA0002930101770000053
is the second derivative of q, DdFor a predetermined damping matrix, KdIs a preset spring matrix and is characterized in that,
Figure BDA0002930101770000054
for the pose error, τextIncluding the impedance pose information.
In an optional embodiment of the present application, the first determining module is further configured to: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the follow-up attitude information according to the pose error, the current attitude and a follow-up control formula. In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and follow-up attitude information corresponding to follow-up control is determined based on the pose error, the current pose of the cusp and a follow-up control formula, so that the follow-up control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
In an optional embodiment of the present application, the obtaining module is further configured to: acquiring the position coordinates of the target point in an image coordinate system; determining a coordinate system transformation relation between the image coordinate system and a robot coordinate system through an NDI coordinate system; and determining the target posture and the target position according to the position coordinates of the projection point in the image coordinate system and the coordinate system transformation relation. In the above scheme, after the position coordinates of the target point in the image coordinate system are acquired, the image coordinate system and the robot coordinate system may be unified based on the NDI coordinate system, so as to convert the position coordinates in the image coordinate system into pose information in the robot coordinate system.
In an optional embodiment of the present application, the robot body state following control device further includes: a second determining module, configured to determine, if the current position does not coincide with the target position, follow-up posture information output by a follow-up control mode according to the target posture, the target position, the current posture and the current position; and the second control module is used for controlling the grinding tool to move to the target posture and the target position according to sensor control information and the follow-up posture information. In the above scheme, at the operation in-process, when the axis of polishing is treated in the skew of grinding tool point, can adopt the mode that follow-up control combines sensor control to control grinding tool to can control grinding tool and follow the patient and remove when patient's health takes place the displacement, improve grinding tool's positioning accuracy at the in-process of polishing.
In a third aspect, an embodiment of the present application provides an electronic device, including: a processor, a memory, and a bus; the processor and the memory are communicated with each other through the bus; the memory stores program instructions executable by the processor, the processor invoking the program instructions capable of performing the robot dynamics follow-up control method as in the first aspect.
In a fourth aspect, embodiments of the present application provide a non-transitory computer-readable storage medium storing computer instructions that cause the computer to perform the robot dynamics following control method as in the first aspect.
In order to make the aforementioned objects, features and advantages of the present application more comprehensible, embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and that those skilled in the art can also obtain other related drawings based on the drawings without inventive efforts.
Fig. 1 is a flowchart of a robot body state following control method according to an embodiment of the present disclosure;
fig. 2 is a flowchart of a specific implementation of step S103 provided in an embodiment of the present application;
fig. 3 is a flowchart of a specific implementation of step S101 provided in an embodiment of the present application;
fig. 4 is a block diagram of a robot body state following control device according to an embodiment of the present disclosure;
fig. 5 is a block diagram of an electronic device according to an embodiment of the present disclosure.
Detailed Description
Before introducing the robot body state following control method provided by the embodiment of the present application, an application scenario of the embodiment of the present application is first introduced.
In the course of a total hip replacement surgery, the acetabulum may be abraded with a robotically controlled abrading tool and the prosthesis placed in the abraded acetabulum. The embodiment of the present application does not specifically limit the specific implementation manners of the robot and the polishing tool, for example: the robot can be realized by a complete robot, a mechanical arm and the like; the grinding tool may be a bone drill or the like arranged at the end of the mechanical arm, and those skilled in the art can make appropriate adjustments according to actual conditions.
In the process of polishing the acetabulum and placing the prosthesis, the body of the patient is easy to displace due to the reaction force of polishing and knocking, so that the polishing tool needs to be controlled to move along with the body of the patient in order to ensure smooth polishing.
It should be noted that, if the robot body state following control method provided in the embodiment of the present application can be applied to a controller inside the robot or an external device, the embodiment of the present application also does not specifically limit this, and those skilled in the art can appropriately select the method according to actual situations.
The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.
Referring to fig. 1, fig. 1 is a flowchart of a robot body state following control method according to an embodiment of the present disclosure, where the robot body state following control method includes the following steps:
step S101: and acquiring a target posture and a target position corresponding to the target point, and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool.
Step S102: and judging whether the current position is coincident with the target position or not, and judging whether the current posture is consistent with the target posture or not.
Step S103: if the current position is coincident with the target position but the current posture is not consistent with the target posture, the follow-up posture information output by the follow-up control mode and the impedance posture information output by the impedance control mode are determined according to the target posture, the target position, the current posture and the current position.
Step S104: and controlling the grinding tool to move to the target attitude according to the follow-up attitude information and the impedance attitude information.
Specifically, in the polishing process, the polishing tool generally polishes along an axis to be polished, that is, the point of the polishing tool needs to be located on the axis to be polished in the polishing process, and the posture of the point needs to be consistent with the posture of the axis to be polished.
Among them, there are three cases in which the grinding tool needs to be moved in consideration of the offset of the grinding tool: firstly, before polishing, a sharp point of a polishing tool needs to be moved to an axis to be polished so as to start polishing; secondly, in the polishing process, the sharp point of the polishing tool deviates from the axis to be polished; thirdly, the attitude of the point of the grinding tool is not in accordance with the attitude of the axis to be ground. For the three different situations, different control schemes may be used to control the movement of the abrading tool, as will be described in detail below.
It will be appreciated that to determine whether there is any one of the three aforementioned offset conditions for the abrading tool, the electronic device may first obtain a target pose, a target position corresponding to the target point, and a current pose, a current position corresponding to the point of the abrading tool. Wherein, above-mentioned target point refers to the projected point of the sharp point of burnisher on treating the axis of burnishing, in other words, the purpose of this scheme lies in controlling the sharp point of burnisher and removing to above-mentioned target point on, and guarantees that the gesture of the sharp point of burnisher is unanimous with the gesture of target point.
It can be understood that the target point provided by the embodiment of the present application may be any point on the axis to be polished which satisfies a condition (for example, a condition that the acetabulum is not damaged) in addition to the projection point of the sharp point of the polishing tool on the axis to be polished, and a person skilled in the art may appropriately select the target point according to actual conditions.
In addition, there are various ways for the electronic device to obtain the target posture and the target position corresponding to the target point, and the current posture and the current position corresponding to the sharp point of the polishing tool.
For the target pose and the target position corresponding to the target point, for example, the electronic device may determine the target pose and the target position by using the target pose and target position determination method provided in the embodiment of the present application (which will be described in detail in the following embodiments); or, planning software in other devices (such as an upper computer) can determine the target posture and the target position and send the target posture and the target position to the electronic device; or the electronic device may further read a target posture, a target position, and the like stored in advance from the cloud.
For the current posture and the current position corresponding to the sharp point of the grinding tool, for example, a binocular vision recognition camera can be arranged around the patient for video acquisition, and the electronic equipment can determine the current posture and the current position corresponding to the sharp point of the grinding tool according to the acquired video; or, the electronic device may also receive the current posture and the current position sent by the binocular vision recognition camera or other external devices; or the electronic device may further read a pre-stored current posture and a current position from the cloud.
After the target posture and the target position corresponding to the target point and the current posture and the current position corresponding to the sharp point of the polishing tool are obtained, whether the current position (namely the current position of the sharp point of the polishing tool) is overlapped with the target position (namely the current position of the target point) or not can be judged according to the information, and whether the current posture (namely the current posture of the sharp point of the polishing tool) is consistent with the target posture (namely the current posture of the target point) or not can be judged.
Wherein, the judged result has two kinds of condition, corresponds with first kind and second kind and the third kind condition among the skew condition of above-mentioned three kinds of burnisher respectively: the position of the sharp point does not coincide with the position of the target point according to the first judgment result corresponding to the first deviation condition and the second deviation condition; in the second determination result corresponding to the third shift case, the position of the cusp coincides with the position of the target point but the posture of the cusp does not coincide with the posture of the target point.
In order to solve the three offset situations, the embodiment of the application adopts three control modes to control the position and the posture of the grinding tool under different situations: a sensor control mode, an impedance control mode, and a follow-up control mode.
Wherein, the sensor control mode refers to: the doctor can be according to the position of the burnisher that shows on the display screen, and manual control robot drives the burnisher and removes, and this kind of control mode is not described in detail to this application embodiment, and this control mode can be implemented to the technical staff in the art in combination with prior art.
The impedance control mode refers to: the motion of the grinding tool is not directly controlled, nor is the contact force of the grinding tool with the outside world directly controlled, but rather the dynamic relationship between the two is controlled.
The follow-up control mode is as follows: the output value varies with the variation of the input value.
For the first determination result, the robot body state following control method provided in the embodiment of the present application may further include the following steps:
firstly, if the current position is not overlapped with the target position, determining the follow-up attitude information output by the follow-up control mode according to the target attitude, the target position, the current attitude and the current position.
And secondly, controlling the grinding tool to move to a target posture and a target position according to the sensor control information and the follow-up posture information.
That is, in the case of the first determination result, the attitude of the grinding tool may be controlled using the follow-up control mode, and the position of the grinding tool may be controlled using the sensor control mode.
Therefore, in the operation process, when the grinding tool sharp point shifts to treat the axis of polishing, can adopt the mode that follow-up control combines sensor control to control grinding tool to can control grinding tool and follow the patient and remove when patient's health takes place the displacement, improve grinding tool's positioning accuracy at the in-process of polishing.
As for the second determination result, the robot body state following control method provided in the embodiment of the present application may further include the above-mentioned step S103 to step S104. That is, in the case of the second determination result, the attitude of the grinding tool may be controlled in common using the follow-up control mode and the impedance control mode.
Therefore, in the operation process, when the point of the polishing tool is positioned on the axis to be polished, but the gesture of the point of the polishing tool is inconsistent with the gesture of a target point on the axis to be polished, the polishing tool can be controlled in a mode of combining follow-up control and impedance control, so that the polishing tool can be controlled to move along with a patient when the body of the patient displaces, and the positioning accuracy of the polishing tool in the polishing process is improved.
The following describes an embodiment of the impedance control mode in detail.
Referring to fig. 2, fig. 2 is a flowchart illustrating a specific implementation manner of step S103 according to an embodiment of the present application, where the step S103 may include the following steps:
step S201: and determining the spatial position relation between the target point and the cusp according to the target posture, the target position, the current posture and the current position.
Step S202: and determining follow-up attitude information and impedance attitude information according to the spatial position relationship.
Specifically, as an implementation manner, the target posture and the target position corresponding to the acquired target point and the current posture and the current position corresponding to the sharp point of the polishing tool may be data in a robot coordinate system.
For example, the point of the grinding tool can be used as the origin, the direction of the gun body of the grinding tool pointing to the point is the z-axis coordinate, the y-axis direction is defined to coincide with the y-direction of the flange tool, and then the x-axis direction can be known according to the right-hand spiral rule, so as to obtain a newly-established coordinate system.
After the spatial position relationship between the target point and the cusp is determined, follow-up attitude information and impedance attitude information can be determined according to the spatial position relationship, so that follow-up control is performed according to the follow-up attitude information and impedance control is performed according to the impedance attitude information.
As described in detail below for the manner of determining the impedance posture information in step S202, step S202 may include the following steps:
firstly, determining the pose error between the cusp and the target point according to the spatial position relation.
And a second unit for determining impedance attitude information based on the attitude error, the current attitude, the current position, and the impedance control formula.
The pose error between the cusp and the target point comprises position deviation and posture deviation between the cusp and the target point. That is to say:
Figure BDA0002930101770000121
wherein the content of the first and second substances,
Figure BDA0002930101770000122
for pose error, qdThe target pose and the target position are shown, q is the current pose and the current position, Δ v is the position deviation between the cusp and the target point, and Δ ω is the pose deviation between the cusp and the target point.
As an embodiment, for the position deviation between the cusp and the target point, the position coordinates of the cusp and the target point may be directly subtracted, that is, the spatial position relationship in step S201 is described above; as another embodiment, for the attitude deviation between the cusp and the target point, the operation may be performed by first using attitude subtraction, and then converted into the attitude deviation described by the equivalent rotation axis.
Since the kinetic equation of the sanding tool can be expressed as follows:
Figure BDA0002930101770000123
wherein M is the inertia corresponding to the grinding toolMatrix, C is Coriolis force effect matrix, g is gravity effect matrix, tau is torque generated by driving motor of robot, tauextIncluding the impedance pose information for the patient and,
Figure BDA0002930101770000124
is the first derivative of q and is,
Figure BDA0002930101770000125
the second derivative of q.
And the second order characteristic equation of the spring-damper-mass composition is expressed as follows:
Figure BDA0002930101770000126
wherein M isdFor controlling the mass of the system for a predetermined inertia matrix, DdFor a predetermined damping matrix, KdIs a preset spring matrix and is characterized in that,
Figure BDA0002930101770000127
is composed of
Figure BDA0002930101770000128
The first derivative of (a) is,
Figure BDA0002930101770000129
is composed of
Figure BDA00029301017700001210
The second derivative of (a) is,
Figure BDA00029301017700001211
is qdThe first derivative of (a) is,
Figure BDA00029301017700001212
is qdThe second derivative of (a).
Tau in second-order characteristic formula of spring-damping-mass compositionextThe ideal impedance control can be obtained by introducing into the dynamics equation of the grinding toolThe model formula is as follows:
Figure BDA0002930101770000131
wherein I is an identity matrix.
Can be simplified so that
Figure BDA0002930101770000132
And
Figure BDA0002930101770000133
while ignoring MdAnd m (q), the following formula can be obtained:
Figure BDA0002930101770000134
the impedance control equation can be derived by substituting into the dynamics equation for the sanding tool:
Figure BDA0002930101770000135
wherein the content of the first and second substances,
Figure BDA0002930101770000136
approximately equal to 0, adjusting DdAnd KdValue of (2), input
Figure BDA0002930101770000137
And
Figure BDA0002930101770000138
corresponding τ can be obtainedext。
As an embodiment, τextThe output form of (a) may be { x, y, z, rx, ry, rz }, { x, y, z } controlling the position velocity of the grinding tool in the robot coordinate system, and { rx, ry, rz } controlling the attitude velocity of the grinding tool in the robot coordinate system.
Since in this case only the sharpening tool tip is neededThe pose of the point is adjusted, so that only τ can be taken care ofext{ rx, ry, rz } in (1), the robot can be based on τextThe output torque controls the sharp point of the grinding tool to move to the target posture.
In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and the impedance attitude information corresponding to the impedance control is determined based on the pose error, the current pose of the cusp and an impedance control formula, so that the impedance control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
As described in detail below for the manner of determining the follow-up posture information in step S202, step S202 may further include the following steps:
firstly, determining the pose error between the cusp and the target point according to the spatial position relation.
And secondly, determining follow-up attitude information according to the pose error, the current attitude and a follow-up control formula.
Wherein, similar to the manner of determining impedance pose information described above, the pose error between the cusp and the target point
Figure BDA0002930101770000141
Based on the kinetic equation of the grinding tool, a follow-up control formula can be obtained:
Figure BDA0002930101770000142
wherein the content of the first and second substances,
Figure BDA0002930101770000143
approximately equal to 0, adjusting DdAnd KdValue of (2), input
Figure BDA0002930101770000144
And
Figure BDA0002930101770000145
the corresponding τ is obtained.
Similarly, as an embodiment, the output form of τ may be { x, y, z, rx, ry, rz }, { x, y, z } controlling the position velocity of the grinding tool in the robot coordinate system, and { rx, ry, rz } controlling the attitude velocity of the grinding tool in the robot coordinate system.
In this case, only the posture of the sharpening tool tip needs to be adjusted, so that only { rx, ry, rz } in τ can be focused, and the robot can control the sharpening tool tip to move to the target posture according to τ output torque.
In the scheme, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and follow-up attitude information corresponding to follow-up control is determined based on the pose error, the current pose of the cusp and a follow-up control formula, so that the follow-up control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
Therefore, the movement of the point of the grinding tool to the target posture can be controlled by combining the follow-up control information corresponding to the follow-up control mode and the impedance control information corresponding to the impedance control mode.
The method for determining the target attitude and the target position in step S101 will be described in detail below. Referring to fig. 3, fig. 3 is a flowchart illustrating a specific implementation manner of step S101 according to an embodiment of the present application, where the step S101 may include the following steps:
step S301: and acquiring the position coordinates of the target point in the image coordinate system.
Step S302: and determining a coordinate system transformation relation between the image coordinate system and the robot coordinate system through the NDI coordinate system.
Step S303: and determining the target posture and the target position according to the position coordinates of the projection point in the image coordinate system and the coordinate system transformation relation.
Specifically, firstly, the position coordinates of a point a and the position coordinates of a point b on the axis to be polished in the image coordinate system can be obtained; then theTaking a point a and a point b to form a vector
Figure BDA0002930101770000151
The current position c of the point of the grinding tool and the point b form a vector
Figure BDA0002930101770000152
According to the vector dot product formula:
Figure BDA0002930101770000153
and the cosine formula:
Figure BDA0002930101770000154
can calculate to obtain the vector
Figure BDA0002930101770000155
In the vector
Figure BDA0002930101770000156
Is projected to the projection length L.
At this time, the position coordinates of the target point may be calculated using the following formula:
Figure BDA0002930101770000157
wherein, TdIs the position coordinate of the target point, a is the position coordinate of the point a, and L is the vector
Figure BDA0002930101770000158
In the vector
Figure BDA0002930101770000159
The projected length of (c).
The NDI coordinate system can then be used to pass through Robot retroreflector ball RTRNAnd Tool reflective ball RTTNCompleting the registration with the robot coordinate system to obtain a coordinate system transformation relation of RTN2R(ii) a RT (reverse transcription) by a Pelvis reflecting ball by utilizing an ND coordinate system IPNAnd Probe reflective ball RTONCompleting the registration with the image coordinate system to obtain the coordinate system transformation relation of RTN2P。
Finally, the pose matrix of the prosthesis under the image coordinate system is planned to be RCAnd then:
RR＝RC×RN2P -1×RPN×RRN -1×RN2R -1；
TR＝TD×TN2P -1×TPN×TRN -1×TN2R -1；
wherein R isRIs attitude information, R, of a target point in a robot coordinate systemCAttitude information, T, of target point under image coordinate systemRFor positional information of target points in the robot coordinate system, TDIs the position information of the target point in the image coordinate system, RTRN＝RRN+TRN，RTN2R＝RN2R+TN2R，
RTPN＝RPN+TPN，RTN2P＝RN2P+TN2P。
In the above scheme, after the position coordinates of the target point in the image coordinate system are acquired, the image coordinate system and the robot coordinate system may be unified based on the NDI coordinate system, so as to convert the position coordinates in the image coordinate system into pose information in the robot coordinate system.
Referring to fig. 4, fig. 4 is a block diagram of a robot body state following control device according to an embodiment of the present disclosure, where the robot body state following control device 400 may include: an obtaining module 401, configured to obtain a target posture and a target position corresponding to a target point, and obtain a current posture and a current position corresponding to a sharp point of a polishing tool; the target point is a projection point of the sharp point on the axis to be polished; a determining module 402, configured to determine whether the current position coincides with the target position, and determine whether the current posture and the target posture are consistent; a first determining module 403, configured to determine, according to the target posture, the target position, the current posture and the current position, follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode if the current position coincides with the target position but the current posture and the target posture are not consistent; a first control module 404 for controlling the abrading tool to move to the target pose based on the follow-up pose information and the impedance pose information.
In the embodiment of the application, in the operation process, when the sharpening tool point is located the axis of waiting to polish, but the gesture of sharpening tool point is inconsistent with the gesture of waiting to polish the epaxial target point of axis, can adopt follow-up control to combine impedance control's mode to control sharpening tool to can control sharpening tool to follow the patient and remove when patient's health takes place the displacement, improve sharpening tool's positioning accuracy at the in-process of polishing.
Further, the first determining module 403 is further configured to: determining a spatial position relationship between the target point and the cusp according to the target posture, the target position, the current posture and the current position; and determining the follow-up attitude information and the impedance attitude information according to the spatial position relationship.
In the embodiment of the application, the position relation of the point and the target point in the space can be determined according to the current position and posture information of the point of the grinding tool and the position and posture information of the target point, so that the follow-up posture information corresponding to the follow-up control and the impedance posture information corresponding to the impedance control can be directly determined according to the position relation of the point and the target point, and the operation process can be simplified on the basis of improving the positioning accuracy of the grinding tool in the grinding process.
Further, the first determining module 403 is further configured to: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the impedance attitude information according to the attitude error, the current attitude, the current position and an impedance control formula.
In the embodiment of the application, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and the impedance attitude information corresponding to impedance control is determined based on the pose error, the current pose of the cusp and an impedance control formula, so that the impedance control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
Further, the first determining module 403 is further configured to: determining the impedance pose information according to the pose error, the current pose, the current position, and the following formula:
Figure BDA0002930101770000171
wherein M is an inertia matrix corresponding to the grinding tool, q is the current attitude and the current position,
Figure BDA0002930101770000172
is the first derivative of q and is,
Figure BDA0002930101770000173
is the second derivative of q, DdFor a predetermined damping matrix, KdIs a preset spring matrix and is characterized in that,
Figure BDA0002930101770000174
for the pose error, τextIncluding the impedance pose information.
Further, the first determining module 403 is further configured to: determining a pose error between the cusp and the target point according to the spatial position relation; and determining the follow-up attitude information according to the pose error, the current attitude and a follow-up control formula.
In the embodiment of the application, the pose error of the cusp can be determined according to the position relation between the cusp and the target point, and follow-up attitude information corresponding to follow-up control is determined based on the pose error, the current pose of the cusp and a follow-up control formula, so that the follow-up control of the grinding tool is realized. Therefore, the grinding tool can be controlled to move along with the patient when the body of the patient is displaced, and the positioning accuracy of the grinding tool in the grinding process is improved.
Further, the obtaining module 401 is further configured to: acquiring the position coordinates of the target point in an image coordinate system; determining a coordinate system transformation relation between the image coordinate system and a robot coordinate system through an NDI coordinate system; and determining the target posture and the target position according to the position coordinates of the projection point in the image coordinate system and the coordinate system transformation relation.
In this embodiment of the application, after the position coordinates of the target point in the image coordinate system are acquired, the image coordinate system and the robot coordinate system may be unified based on the NDI coordinate system, so as to convert the position coordinates in the image coordinate system into pose information in the robot coordinate system.
Further, the robot body state following control device 400 further includes: a second determining module, configured to determine, if the current position does not coincide with the target position, follow-up posture information output by a follow-up control mode according to the target posture, the target position, the current posture and the current position; and the second control module is used for controlling the grinding tool to move to the target posture and the target position according to sensor control information and the follow-up posture information.
In this application embodiment, in the operation process, when the axis of polishing is treated in the skew of grinding tool point, can adopt follow-up control to control grinding tool in combination with the mode of sensor control to can control grinding tool and follow the patient and remove when patient's health takes place the displacement, improve grinding tool's positioning accuracy at the in-process of polishing.
Referring to fig. 5, fig. 5 is a block diagram of an electronic device according to an embodiment of the present disclosure, where the electronic device 500 includes: at least one processor 501, at least one communication interface 502, at least one memory 503, and at least one communication bus 504. Wherein, the communication bus 504 is used for realizing direct connection communication of these components, the communication interface 502 is used for communicating signaling or data with other node devices, and the memory 503 stores machine readable instructions executable by the processor 501. When the electronic device 500 is running, the processor 501 communicates with the memory 503 through the communication bus 504, and the machine-readable instructions are called by the processor 501 to execute the robot body state following control method.
For example, the processor 501 of the embodiment of the present application may read the computer program from the memory 503 through the communication bus 504 and execute the computer program to implement the following method: step S101: and acquiring a target posture and a target position corresponding to the target point, and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool. Step S102: and judging whether the current position is coincident with the target position or not, and judging whether the current posture is consistent with the target posture or not. Step S103: if the current position is coincident with the target position but the current posture is not consistent with the target posture, the follow-up posture information output by the follow-up control mode and the impedance posture information output by the impedance control mode are determined according to the target posture, the target position, the current posture and the current position. Step S104: and controlling the grinding tool to move to the target attitude according to the follow-up attitude information and the impedance attitude information.
The processor 501 may be an integrated circuit chip having signal processing capabilities. The Processor 501 may be a general-purpose Processor, including a Central Processing Unit (CPU), a Network Processor (NP), and the like; but also Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate arrays (FPGAs) or other Programmable logic devices, discrete Gate or transistor logic devices, discrete hardware components. Which may implement or perform the various methods, steps, and logic blocks disclosed in the embodiments of the present application. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The Memory 503 may include, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read Only Memory (PROM), Erasable Read Only Memory (EPROM), electrically Erasable Read Only Memory (EEPROM), and the like.
It will be appreciated that the configuration shown in FIG. 5 is merely illustrative and that electronic device 500 may include more or fewer components than shown in FIG. 5 or have a different configuration than shown in FIG. 5. The components shown in fig. 5 may be implemented in hardware, software, or a combination thereof. In this embodiment, the electronic device 500 may be, but is not limited to, an entity device such as a desktop, a laptop, a smart phone, an intelligent wearable device, and a vehicle-mounted device, and may also be a virtual device such as a virtual machine. In addition, the electronic device 500 is not necessarily a single device, but may also be a combination of multiple devices, such as a server cluster, and the like. In the embodiment of the present application, the external device in the robot body state following control method, the controller in the robot, and the like may all be implemented by using the electronic device 500 shown in fig. 5.
Embodiments of the present application further provide a computer program product, including a computer program stored on a non-transitory computer-readable storage medium, where the computer program includes program instructions, and when the program instructions are executed by a computer, the computer is capable of executing the steps of the robot body dynamic following control method in the foregoing embodiments, for example, including: acquiring a target posture and a target position corresponding to the target point, and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished; judging whether the current position is coincident with the target position or not, and judging whether the current posture is consistent with the target posture or not; if the current position is coincident with the target position but the current posture is not consistent with the target posture, determining follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode according to the target posture, the target position, the current posture and the current position; and controlling the grinding tool to move to the target posture according to the follow-up posture information and the impedance posture information.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other ways. The above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only one logical division, and there may be other divisions when actually implemented, and for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of devices or units through some communication interfaces, and may be in an electrical, mechanical or other form.
In addition, units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
Furthermore, the functional modules in the embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims (10)

1. A robot posture follow-up control method is characterized by comprising the following steps:
acquiring a target posture and a target position corresponding to the target point, and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished;
judging whether the current position is coincident with the target position or not, and judging whether the current posture is consistent with the target posture or not;
if the current position is coincident with the target position but the current posture is not consistent with the target posture, determining follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode according to the target posture, the target position, the current posture and the current position;
and controlling the grinding tool to move to the target posture according to the follow-up posture information and the impedance posture information.
2. The robot configuration follow-up control method according to claim 1, wherein the determining follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode from the target posture, the target position, the current posture and the current position includes:
determining a spatial position relationship between the target point and the cusp according to the target posture, the target position, the current posture and the current position;
and determining the follow-up attitude information and the impedance attitude information according to the spatial position relationship.
3. The robot dynamics follow-up control method according to claim 2, wherein the determining the impedance pose information according to the spatial positional relationship includes:
determining a pose error between the cusp and the target point according to the spatial position relation;
and determining the impedance attitude information according to the attitude error, the current attitude, the current position and an impedance control formula.
4. The robot dynamics follow-up control method of claim 3, wherein the determining the impedance pose information from the pose error, the current pose, the current position, and an impedance control formula comprises:
determining the impedance pose information according to the pose error, the current pose, the current position, and the following formula:
Figure FDA0002930101760000021
wherein M is an inertia matrix corresponding to the grinding tool, q is the current attitude and the current position,
Figure FDA0002930101760000022
is the first derivative of q and is,
Figure FDA0002930101760000023
is the second derivative of q, DdFor a predetermined damping matrix, KdIs a preset spring matrix and is characterized in that,
Figure FDA0002930101760000024
for the pose error, τextIncluding the impedance pose information.
5. The robot dynamic following control method according to claim 2, wherein the determining the following attitude information according to the spatial position relationship includes:
determining a pose error between the cusp and the target point according to the spatial position relation;
and determining the follow-up attitude information according to the pose error, the current attitude and a follow-up control formula.
6. The robot posture follow-up control method according to claim 1, wherein the acquiring of the target posture and the target position corresponding to the target point comprises:
acquiring the position coordinates of the target point in an image coordinate system;
determining a coordinate system transformation relation between the image coordinate system and a robot coordinate system through an NDI coordinate system;
and determining the target posture and the target position according to the position coordinates of the projection point in the image coordinate system and the coordinate system transformation relation.
7. The robot dynamics follow-up control method according to claim 1, characterized in that after the determination of whether the current position coincides with the target position and the determination of whether the current attitude and the target attitude coincide, the method further comprises:
if the current position is not overlapped with the target position, determining follow-up attitude information output by a follow-up control mode according to the target attitude, the target position, the current attitude and the current position;
and controlling the grinding tool to move to the target posture and the target position according to the sensor control information and the follow-up posture information.
8. A robot posture follow-up control device is characterized by comprising:
the acquisition module is used for acquiring a target posture and a target position corresponding to the target point and acquiring a current posture and a current position corresponding to a sharp point of the polishing tool; the target point is a projection point of the sharp point on the axis to be polished;
the judging module is used for judging whether the current position is superposed with the target position or not and judging whether the current posture is consistent with the target posture or not;
a first determining module, configured to determine, according to the target posture, the target position, the current posture and the current position, follow-up posture information output by a follow-up control mode and impedance posture information output by an impedance control mode if the current position coincides with the target position but the current posture and the target posture are not consistent;
a first control module for controlling the grinding tool to move to the target attitude according to the follow-up attitude information and the impedance attitude information.
9. An electronic device, comprising: a processor, a memory, and a bus;
the processor and the memory are communicated with each other through the bus;
the memory stores program instructions executable by the processor, the processor invoking the program instructions to enable execution of the robot dynamics follow-up control method of any of claims 1-7.
10. A non-transitory computer-readable storage medium storing computer instructions which, when executed by a computer, cause the computer to perform the robot dynamics follow-up control method according to any one of claims 1-7.
CN202110146573.5A 2021-02-02 2021-02-02 Robot body state follow-up control method and device Active CN112809686B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110146573.5A CN112809686B (en) 2021-02-02 2021-02-02 Robot body state follow-up control method and device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110146573.5A CN112809686B (en) 2021-02-02 2021-02-02 Robot body state follow-up control method and device

Publications (2)

Publication Number Publication Date
CN112809686A true CN112809686A (en) 2021-05-18
CN112809686B CN112809686B (en) 2022-08-16

Family

ID=75860767

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110146573.5A Active CN112809686B (en) 2021-02-02 2021-02-02 Robot body state follow-up control method and device

Country Status (1)

Country Link
CN (1) CN112809686B (en)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113509296A (en) * 2021-07-09 2021-10-19 武汉联影智融医疗科技有限公司 Method and system for automatically adjusting position and posture of acetabular cup and surgical robot
CN113520601A (en) * 2021-07-09 2021-10-22 武汉联影智融医疗科技有限公司 Real-time correction method and system for pose of end tool and surgical robot
CN113893035A (en) * 2021-10-27 2022-01-07 杭州柳叶刀机器人有限公司 Joint replacement surgery navigation system and computer readable storage medium
CN113907830A (en) * 2021-10-13 2022-01-11 杭州柳叶刀机器人有限公司 Polishing control method and device, electronic equipment and storage medium
CN114407021A (en) * 2022-02-28 2022-04-29 杭州柳叶刀机器人有限公司 Dental surgery mechanical arm control method and device, computer terminal and storage medium
CN114504385A (en) * 2022-01-18 2022-05-17 上海术航机器人有限公司 Surgical robot tip attitude adjustment method, system, device, and medium
WO2022257473A1 (en) * 2021-06-07 2022-12-15 北京长木谷医疗科技有限公司 Control method and apparatus for reaming tool of surgical robot
WO2023280310A1 (en) * 2021-07-09 2023-01-12 武汉联影智融医疗科技有限公司 Surgical robot and control method therefor
CN116277007A (en) * 2023-03-28 2023-06-23 北京维卓致远医疗科技发展有限责任公司 Pose control method, pose control device, storage medium and controller

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101646534A (en) * 2007-06-27 2010-02-10 松下电器产业株式会社 Apparatus and method for controlling robot arm, and robot and program
KR20100062653A (en) * 2008-12-02 2010-06-10 삼성전자주식회사 Robot hand and method of controlling robot hand
CN108883533A (en) * 2016-03-24 2018-11-23 Cmr外科有限公司 robot control
CN111249005A (en) * 2020-03-20 2020-06-09 苏州新医智越机器人科技有限公司 Puncture surgical robot compliance control system
CN111803213A (en) * 2020-07-07 2020-10-23 武汉联影智融医疗科技有限公司 Cooperative robot guiding and positioning method and device
CN112171673A (en) * 2020-09-24 2021-01-05 哈尔滨工业大学（深圳） Robot arm operation control method, control apparatus, and computer-readable storage medium

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101646534A (en) * 2007-06-27 2010-02-10 松下电器产业株式会社 Apparatus and method for controlling robot arm, and robot and program
KR20100062653A (en) * 2008-12-02 2010-06-10 삼성전자주식회사 Robot hand and method of controlling robot hand
CN108883533A (en) * 2016-03-24 2018-11-23 Cmr外科有限公司 robot control
CN111249005A (en) * 2020-03-20 2020-06-09 苏州新医智越机器人科技有限公司 Puncture surgical robot compliance control system
CN111803213A (en) * 2020-07-07 2020-10-23 武汉联影智融医疗科技有限公司 Cooperative robot guiding and positioning method and device
CN112171673A (en) * 2020-09-24 2021-01-05 哈尔滨工业大学（深圳） Robot arm operation control method, control apparatus, and computer-readable storage medium

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022257473A1 (en) * 2021-06-07 2022-12-15 北京长木谷医疗科技有限公司 Control method and apparatus for reaming tool of surgical robot
CN113509296A (en) * 2021-07-09 2021-10-19 武汉联影智融医疗科技有限公司 Method and system for automatically adjusting position and posture of acetabular cup and surgical robot
CN113520601A (en) * 2021-07-09 2021-10-22 武汉联影智融医疗科技有限公司 Real-time correction method and system for pose of end tool and surgical robot
WO2023280310A1 (en) * 2021-07-09 2023-01-12 武汉联影智融医疗科技有限公司 Surgical robot and control method therefor
CN113509296B (en) * 2021-07-09 2023-08-11 武汉联影智融医疗科技有限公司 Method and system for automatically adjusting position and posture of acetabular cup and surgical robot
CN113907830A (en) * 2021-10-13 2022-01-11 杭州柳叶刀机器人有限公司 Polishing control method and device, electronic equipment and storage medium
CN113893035A (en) * 2021-10-27 2022-01-07 杭州柳叶刀机器人有限公司 Joint replacement surgery navigation system and computer readable storage medium
CN114504385A (en) * 2022-01-18 2022-05-17 上海术航机器人有限公司 Surgical robot tip attitude adjustment method, system, device, and medium
CN114407021A (en) * 2022-02-28 2022-04-29 杭州柳叶刀机器人有限公司 Dental surgery mechanical arm control method and device, computer terminal and storage medium
CN116277007A (en) * 2023-03-28 2023-06-23 北京维卓致远医疗科技发展有限责任公司 Pose control method, pose control device, storage medium and controller
CN116277007B (en) * 2023-03-28 2023-12-19 北京维卓致远医疗科技发展有限责任公司 Pose control method, pose control device, storage medium and controller

Also Published As

Publication number Publication date
CN112809686B (en) 2022-08-16

Similar Documents

Publication Publication Date Title
CN112809686B (en) Robot body state follow-up control method and device
US8996176B2 (en) Robot apparatus, method for controlling the same, and computer program
KR102268286B1 (en) An inverse kinematics calculation method for mass center of humanoid robot
CN109804220B (en) Apparatus and method for tracking head movement
CN113442169B (en) Method and device for calibrating hands and eyes of robot, computer equipment and readable storage medium
JP2020055075A (en) Robot control device using augmented reality and composite reality and display device
US20210197384A1 (en) Robot control method and apparatus and robot using the same
CN108927801B (en) Method and device for adjusting tail end attitude of mechanical arm
CN108066008B (en) Medical instrument control method and system for assisting operation
CN113635311A (en) Method and system for out-of-hand calibration of eye for fixing calibration plate
CN114404047A (en) Positioning method, system, device, computer equipment and storage medium
CN116919590A (en) Surgical robot control method, device and medium for hallux valgus minimally invasive surgery
CN113524201B (en) Active adjusting method and device for pose of mechanical arm, mechanical arm and readable storage medium
Li et al. Free head movement eye gaze contingent ultrasound interfaces for the da Vinci surgical system
CN113907830A (en) Polishing control method and device, electronic equipment and storage medium
WO2023030035A1 (en) Dynamic picture dynamic display method for position of mechanical arm and control terminal
KR20190019658A (en) System and method for controlling bone position using robot
CN116630382A (en) Nerve regulation and control image monitoring registration system and control method
CN116135169A (en) Positioning method, positioning device, electronic equipment and computer readable storage medium
WO2022176928A1 (en) Teaching device
CN115317136A (en) Control method and control device for surgical robot and robot
CN115494938A (en) Non-contact interaction method and device
EP3794554A1 (en) An alignment system for liver surgery
JP2022507471A (en) Cutting device for installing the knee prosthesis
US20240036648A1 (en) Multiple-magnet hand-mounted position-tracking device

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