CN117679181A - Mechanical arm pose control method and surgical robot system - Google Patents
Mechanical arm pose control method and surgical robot system Download PDFInfo
- Publication number
- CN117679181A CN117679181A CN202410105285.9A CN202410105285A CN117679181A CN 117679181 A CN117679181 A CN 117679181A CN 202410105285 A CN202410105285 A CN 202410105285A CN 117679181 A CN117679181 A CN 117679181A
- Authority
- CN
- China
- Prior art keywords
- control point
- mechanical arm
- pose
- acquiring
- deviation
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 62
- 210000003484 anatomy Anatomy 0.000 claims abstract description 31
- 210000000988 bone and bone Anatomy 0.000 claims description 52
- 238000012937 correction Methods 0.000 claims description 47
- 239000011159 matrix material Substances 0.000 claims description 31
- 238000012545 processing Methods 0.000 claims description 24
- 238000000227 grinding Methods 0.000 claims description 23
- 238000013016 damping Methods 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000002159 abnormal effect Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 description 14
- 238000003801 milling Methods 0.000 description 9
- 230000036961 partial effect Effects 0.000 description 9
- 230000009466 transformation Effects 0.000 description 6
- 238000006243 chemical reaction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 238000001356 surgical procedure Methods 0.000 description 4
- 230000005856 abnormality Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- JTERLNYVBOZRHI-RIIGGKATSA-N [(2r)-3-[2-aminoethoxy(hydroxy)phosphoryl]oxy-2-[(5e,8e,11e,14e)-icosa-5,8,11,14-tetraenoyl]oxypropyl] (5e,8e,11e,14e)-icosa-5,8,11,14-tetraenoate Chemical compound CCCCC\C=C\C\C=C\C\C=C\C\C=C\CCCC(=O)OC[C@H](COP(O)(=O)OCCN)OC(=O)CCC\C=C\C\C=C\C\C=C\C\C=C\CCCCC JTERLNYVBOZRHI-RIIGGKATSA-N 0.000 description 2
- 230000004075 alteration Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000012938 design process Methods 0.000 description 2
- 238000011540 hip replacement Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 210000004197 pelvis Anatomy 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000011882 arthroplasty Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 210000001624 hip Anatomy 0.000 description 1
- 210000004394 hip joint Anatomy 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 210000003049 pelvic bone Anatomy 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
The invention provides a mechanical arm pose adjusting method and a surgical robot system, wherein the mechanical arm pose adjusting method comprises the following steps: acquiring the current positions of a first control point and a second control point; the connecting line of the first control point and the second control point forms a tool axis, and the tool axis is overlapped with the tail end axis of the mechanical arm; acquiring target positions of a first control point and the second control point; acquiring a first deviation between the current position of the first control point and the target position, and acquiring a second deviation between the current position of the second control point and the target position; impedance control is provided to the robotic arm based on the first deviation and the second deviation such that the tool axis coincides with the central axis of the anatomical structure. The control rigidity of the mechanical arm is effectively improved by adopting a control mode with double control points.
Description
Technical Field
The invention belongs to the technical field of medical instruments, and particularly relates to a mechanical arm pose control method and a surgical robot system.
Background
When performing hip replacement surgery using a surgical robot, it is necessary to mount a bone grinding tool on the distal end of the mechanical arm and perform a bone grinding operation on the acetabular fossa using the bone grinding tool. In the bone milling process, it is desirable that the bone milling tool be fed along the axis of the acetabular fossa, and for this purpose, the prior art generally employs cartesian impedance control of the robotic arm based on the location of the end tool control points of the robotic arm. However, the mechanical arm pose control method has the disadvantages of poor control rigidity and poor precision.
Disclosure of Invention
The invention aims to provide a mechanical arm pose control method and an operation robot system, which aim to improve the control precision of a mechanical arm and further improve the bone grinding effect in the execution process of hip joint replacement.
In order to achieve the above object, the present invention provides a method for controlling the pose of a mechanical arm, comprising:
respectively acquiring the current positions of a first control point and a second control point; the first control point is a center point of a tool loading part connected to the tail end of the mechanical arm, the second control point is a center point of an operation tool connected to the tool loading part, a connecting line of the second control point and the first control point forms a tool axis, and the tool axis is overlapped with the axis of the tail end of the mechanical arm;
respectively acquiring target positions of the first control point and the second control point; when the first control point and the second control point are respectively located at corresponding target positions, the tool axis coincides with a central axis of an anatomical structure;
acquiring a first deviation between the current position of the first control point and a corresponding target position, and acquiring a second deviation between the current position of the second control point and the corresponding target position; the method comprises the steps of,
Impedance control is provided based on the first deviation and the second deviation to constrain the robotic arm such that the tool axis coincides with a central axis of the anatomical structure.
Optionally, the step of providing impedance control based on the first deviation and the second deviation comprises:
acquiring a first sub-impedance moment based on the first deviation, and acquiring a second sub-impedance moment based on the second deviation;
acquiring a first impedance moment based on the first sub-impedance moment and the second sub-impedance moment; the first resistance moment is used for being applied to the mechanical arm to restrain the mechanical arm.
Optionally, the step of obtaining the first sub-impedance moment based on the first deviation includes:
calculating a first correction moment related to the stiffness of the mechanical arm, a second correction moment related to the damping of the mechanical arm, and an inertia compensation moment related to the inertia of the mechanical arm based on the first deviation;
acquiring the first sub-impedance moment based on the first correction moment, the second correction moment and the inertia compensation moment corresponding to the first deviation; and/or the number of the groups of groups,
The step of obtaining a second sub-impedance moment based on the second deviation includes:
calculating a first correction torque related to the stiffness of the mechanical arm, a second correction torque related to the damping of the mechanical arm, and an inertia compensation torque related to the inertia of the mechanical arm based on the second deviation;
the second sub-impedance moment is obtained based on the first correction moment, the second correction moment, and the inertia compensation moment corresponding to the second deviation.
Optionally, the step of obtaining the current positions of the first control point and the second control point includes:
acquiring a current pose of a center point of the anatomical structure;
correcting the pose of a mechanical arm base coordinate system based on the current pose of the central point of the anatomical structure, wherein the Y axis of the mechanical arm base coordinate system after correction is parallel to the central axis of the anatomical structure;
acquiring mechanical arm kinematics parameters corresponding to the first control point based on the corrected pose of the mechanical arm base coordinate system, and executing mechanical arm kinematics calculation based on the mechanical arm kinematics parameters corresponding to the first control point to obtain the current absolute position of the first control point; the method comprises the steps of,
And acquiring mechanical arm kinematics parameters corresponding to the second control point based on the corrected pose of the mechanical arm base coordinate system, and executing mechanical arm kinematics calculation based on the mechanical arm kinematics parameters corresponding to the second control point to obtain the current absolute position of the first control point.
Optionally, the step of correcting the pose of the manipulator base coordinate system based on the current pose of the center point of the anatomical structure comprises:
acquiring a component of a current posture of a center point of the anatomical structure in a Y direction as a first vector, and acquiring a component of a posture of the mechanical arm base coordinate system before correction in the Y direction as a second vector; the Y-direction is parallel to a central axis of the anatomical structure;
acquiring an included angle between the first vector and the second vector, and acquiring a normal vector of a plane where the first vector and the second vector are located;
acquiring an axial angle matrix based on the included angle and the normal vector;
obtaining a difference matrix of the current gesture of the target point and the gesture of the mechanical arm base coordinate system before correction based on the axis angle matrix;
And obtaining the corrected pose of the mechanical arm base coordinate system based on the difference matrix and the pose of the mechanical arm base coordinate system before correction.
Optionally, the step of obtaining the target position of the first control point includes:
acquiring the current position of the first control point and the positions of n previous moments;
obtaining a predicted position of the first control point based on the current position of the first control point and the previous n times of positions;
obtaining the target position of the first control point based on the current position of the first control point and the predicted position of the first control point; and/or the number of the groups of groups,
the step of obtaining the target position of the second control point includes:
acquiring the current position of the second control point and the positions of n previous moments;
obtaining a predicted position of the second control point based on the current position of the second control point and the previous n times of positions;
and obtaining the target position of the second control point based on the current position of the second control point and the predicted position of the second control point.
Optionally, n is an even number;
the step of obtaining the predicted position of the first control point based on the current position of the first control point and the previous n times of positions includes:
The first control point is precededInputting the positions at each moment into a linear position prediction model to obtain a primary predicted position of the first control point;
the primary predicted position of the first control point, the first control point being followed byThe positions of the moments and the current position of the first control point are input into a polynomial position prediction model to obtain the predicted position of the first control point; and/or the number of the groups of groups,
the step of obtaining the predicted position of the second control point based on the current position of the second control point and the previous n times of positions includes:
the second control point is arranged in frontInputting the positions at each moment into a linear position prediction model to obtain the primary pre-prediction of the second control pointMeasuring the position;
the primary predicted position of the second control point, the second control point being followed byAnd (3) inputting the positions of the moments and the current position of the second control point into a polynomial position prediction model to obtain the predicted position of the second control point.
Optionally, the step of obtaining the first deviation includes:
obtaining a chebyshev distance between the current position of the first control point and a corresponding target position;
Performing second-order processing on the Chebyshev distance between the current position of the first control point and the corresponding target position to obtain a first processing distance;
projecting the first processing distance to a Y direction, and taking the projected value of the first processing distance in the Y direction as the first deviation and/or,
the step of obtaining the second deviation comprises:
obtaining a chebyshev distance between the current position of the second control point and the corresponding target position;
performing second-order processing on the Chebyshev distance between the current position of the second control point and the corresponding target position to obtain a second processing distance;
projecting the second processing distance to a Y direction, and taking the projected value of the second processing distance in the Y direction as the second deviation;
the Y-direction is parallel to a central axis of the anatomical structure.
Optionally, the method for controlling the pose of the mechanical arm further comprises:
planning a virtual safety boundary based on the pose of the center point of the anatomical structure;
judging whether the tail end of the mechanical arm touches the virtual safety boundary, if so, generating and controlling a display device to display alarm information, and generating a second impedance moment; the second impedance moment is used for being applied to the mechanical arm so as to drive the mechanical arm to move until the tail end of the mechanical arm returns to the virtual safety boundary; and/or, the mechanical arm pose control method further comprises the following steps:
Acquiring target information;
judging whether the target information is abnormal, if so, generating and controlling a display device to display alarm information, and locking the mechanical arm;
the target information is at least one of a current pose of a center point of the anatomical structure, a deviation between the current pose of the tail end of the mechanical arm and a previous moment pose, and a projection of the deviation between the current pose of the tail end of the mechanical arm and an initial pose in a direction perpendicular to a Y direction; the Y-direction is parallel to a central axis of the anatomical structure.
To achieve the above object, the present invention also provides a surgical robot system comprising:
the tail end of the mechanical arm is connected with the bone grinding tool through the tool loading piece;
the navigation device comprises a locator and a tool target, wherein the tool target is used for being arranged at the tail end of the mechanical arm; the positioner is used for identifying the tool target to acquire the pose of the tail end of the mechanical arm, and the pose of the tail end of the mechanical arm is used for acquiring the center point of the tool loading piece and the center point of the operating tool; the method comprises the steps of,
and the control unit is in communication connection with the positioning device and is configured to execute the mechanical arm pose control method.
Compared with the prior art, the mechanical arm pose control method and the surgical robot system have the following advantages:
the method for controlling the pose of the mechanical arm comprises the following steps: respectively acquiring the current positions of a first control point and a second control point; the first control point is a center point of a tool loading part connected to the tail end of the mechanical arm, the second control point is a center point of a bone grinding tool connected to the tool loading part, a connecting line of the second control point and the first control point forms a tool axis, and the tool axis is overlapped with the axis of the tail end of the mechanical arm; respectively acquiring target positions of the first control point and the second control point; when the first control point and the second control point are respectively located at corresponding target positions, the tool axis coincides with a central axis of an anatomical structure; acquiring a first deviation between the current position of the first control point and the target position and a second deviation between the current position of the second control point and the target position; and providing impedance control based on the first and second deviations to constrain the robotic arm such that the tool axis coincides with a central axis of the anatomical structure. The mechanical arm is subjected to impedance control through the two control points (namely the first control point and the second control point), so that the tool axis is kept in a state of being coincident with the central axis of the anatomical structure as much as possible, the control rigidity and the control precision are high, and the bone grinding effect can be effectively improved.
Drawings
The drawings are included to provide a better understanding of the invention and are not to be construed as unduly limiting the invention. Wherein:
fig. 1 is a schematic view of an application scenario of a surgical robot system according to an embodiment of the present invention;
FIG. 2 is a partial schematic view of an application scenario of a surgical robotic system provided according to an embodiment of the present invention;
FIG. 3 is a flow chart of a surgical robotic system according to one embodiment of the present invention for bone registration and mapping of the central axis of a planned anatomical structure to a realistic scenario during application;
FIG. 4 is a flow chart of the initial positioning of a robotic arm during application of a surgical robotic system according to one embodiment of the present invention;
FIG. 5 is a flowchart illustrating a method for controlling the pose of a robotic arm performed by a control unit during an application process of a surgical robotic system according to an embodiment of the present invention;
FIG. 6 is a partial flow chart of a method of controlling the pose of a robotic arm performed by a control unit of a surgical robotic system according to an embodiment of the present invention, showing steps for correcting a robotic arm-based coordinate system;
FIG. 7 is a partial flow chart of a method for controlling the pose of a manipulator, which is executed by a control unit, in an application process of a surgical robot system according to an embodiment of the present invention, wherein the flow chart mainly shows a flow chart for correcting the pose of a manipulator base coordinate system;
FIG. 8 is a partial flowchart of a method for controlling the pose of a mechanical arm executed by a control unit in an application process of a surgical robot system according to an embodiment of the present invention, where the flowchart mainly illustrates a flowchart for acquiring the current positions of a first control point and a second control point;
FIG. 9 is a partial flowchart of a method for controlling the pose of a mechanical arm executed by a control unit in an application process of a surgical robot system according to an embodiment of the present invention, where the flowchart mainly illustrates a flowchart for acquiring target positions of any one of a first control point and a second control point;
fig. 10 is a partial flowchart of a method for controlling the pose of a mechanical arm executed by a control unit in an application process of the surgical robot system according to an embodiment of the present invention, where the diagram mainly illustrates a schematic diagram for acquiring deviations between the current positions of any first control point and second control point and a target position;
FIG. 11 is a schematic diagram of an impedance controller pre-stored in a control unit of a surgical robotic system provided in accordance with an embodiment of the present invention;
FIG. 12 is a schematic view of parameter tuning of an impedance controller pre-stored in a control unit of a surgical robotic system according to one embodiment of the present invention during a design process;
FIG. 13 is a partial flowchart of a method for controlling the pose of a manipulator performed by a control unit in an application process of a surgical robot system according to an embodiment of the present invention, wherein the flowchart illustrates steps for implementing a first protection strategy;
FIG. 14 is a schematic view of virtual safety boundaries planned by a control unit of a surgical robotic system provided in accordance with an embodiment of the present invention;
fig. 15 is a partial flowchart of a method for controlling the pose of a mechanical arm executed by a control unit in an application process of the surgical robot system according to an embodiment of the present invention, where the implementation steps of the second protection strategy are shown.
Detailed Description
Other advantages and effects of the present invention will become apparent to those skilled in the art from the following disclosure, which describes the embodiments of the present invention with reference to specific examples. The invention may be practiced or carried out in other embodiments that depart from the specific details, and the details of the present description may be modified or varied from the spirit and scope of the present invention. It should be noted that, the illustrations provided in the present embodiment merely illustrate the basic concept of the present invention by way of illustration, and only the components related to the present invention are shown in the drawings and are not drawn according to the number, shape and size of the components in actual implementation, and the form, number and proportion of the components in actual implementation may be arbitrarily changed, and the layout of the components may be more complex. In addition, each embodiment of the following description has one or more features, respectively, which does not mean that the inventor must implement all features of any embodiment at the same time, or that only some or all of the features of different embodiments can be implemented separately. In other words, those skilled in the art can implement some or all of the features of any one embodiment or a combination of some or all of the features of multiple embodiments selectively, depending on the design specifications or implementation requirements, thereby increasing the flexibility of the implementation of the invention where implemented as possible.
The invention will be further described in detail with reference to the accompanying drawings, in order to make the objects, advantages and features of the invention more apparent. It should be noted that the drawings are in a very simplified form and are all to a non-precise scale, merely for convenience and clarity in aiding in the description of embodiments of the invention. The same or similar reference numbers in the drawings refer to the same or similar parts.
Fig. 1 and fig. 2 show an application scenario schematic diagram of a surgical robot system provided by an embodiment of the present invention. As shown in fig. 1 and 2, the surgical robot system includes a robot arm 100, a navigation device, and a control unit. The tip of the robot arm 100 is used to arrange the tool load 300 such that the tip of the robot arm 100 can be connected to the operation tool by the tool load 300, and such that the central axis of the operation tool coincides with the axis of the tip of the robot arm 100, the central axis of the operation tool passing through the center point of the tool load 300 and the center point of the operation tool, that is, the line connecting the center point of the tool load 300 and the center point of the operation tool constitutes the central axis of the operation tool. For simplicity of description, the central axis of the operating tool will be referred to hereinafter as the tool axis. The manipulation tool is used for performing a predetermined manipulation of the anatomical structure of the target object 1. In the embodiment of the present invention, the surgical robot system is applied to hip arthroplasty, the anatomical structure is an acetabular fossa, the operation tool is a bone grinding tool 400, and specifically may include an electric driving part 410 and an acetabular file 420 connected to the electric driving part 410, and the electric driving part 410 drives the acetabular file 420 to act so as to perform bone grinding operation on the acetabular fossa. The navigation device includes a bone target 210, a tool target 220, and a locator 230. The bone target 210 is for placement on the pelvic bone of the target object 1, and the tool target 220 is for placement on the distal end of the robotic arm 100. The localizer 230 is used for acquiring pose information of the bone target 210 and pose information of the tool target 220, and transmitting the pose information of the bone target 210 and the pose information of the tool target 220 to the control unit. Those skilled in the art will appreciate that based on the pose information of the bone target 210, pose information of the center point of each actual registration point, e.g., acetabular fossa, located on the pelvis may be obtained, and based on the pose information of the tool target 220, pose information of the center point of the tool load 300 and pose information of the center point of the bone grinding tool 400 may be obtained. The pose information includes position information and pose information. Bone target 210 and tool target 220 may be optical targets, with locator 230 being an optical locator accordingly. In addition, in actual surgery, the target object 1 is a patient, and in simulated surgery, the target object 1 is a human body model.
The partial procedure for performing hip replacement surgery using the surgical robotic system is as follows: first, performing bone registration, and mapping a central axis 01 of an acetabular fossa planned before operation into a real scene based on a result of the bone registration to obtain an initial pose of the central axis 01 of the acetabular fossa in the real scene. The robotic arm 100 is then initially positioned such that the tool axis number (i.e., the central axis of the bone grinding tool 400) coincides with the central axis 01 of the acetabular fossa in a real world scenario. Thereafter, the operator may push the running bone grinding tool 400 to feed along the central axis 01 of the acetabular socket in a real world scenario to perform a bone grinding operation on the acetabular socket. The bone grinding tool 400 is fed along the central axis 01 of the acetabular socket in the real world, which means that the bone grinding tool 400 moves in a direction approaching the central point of the acetabular socket during bone grinding, and the tool axis coincides with the central axis 01 of the acetabular socket in the real world.
The bone registration process may be as shown in fig. 3, and includes:
in step S01, the operator holds the registration target, and makes the registration target abut against the actual registration point on the pelvis, and at the same time, the locator 230 obtains the pose of the registration target, so as to obtain the position of the actual registration point. It will be appreciated that the actual registration points are in one-to-one correspondence with the planned registration points of the preoperative plan. The registration target may be a pointed target.
And S02, registering the planning registration points and the actual registration points by using a control unit to obtain a first conversion matrix between the planning registration points and the actual registration points.
After the bone registration is finished, step S03 may be performed, where step S03 includes mapping the central axis 01 of the preoperatively planned acetabular fossa to a real scene using the first transformation matrix, to obtain the central axis 01 of the acetabular fossa in the real scene.
The operation of initially positioning the robot arm 100 may include, as shown in fig. 4:
step S04, acquiring the original pose of the tool target 220.
Step S05, acquiring the original pose of the tail end of the mechanical arm 100 according to the original pose of the tool target 220.
Step S06, a second transformation matrix between the original pose of the tool target 220 and the initial pose of the central axis 01 of the acetabular fossa in the real scene is calculated.
Step S07, acquiring a target pose of the end of the mechanical arm 100 based on the original pose of the end of the mechanical arm 100 and the second transformation matrix. When the tip of the robotic arm 100 reaches the target pose, the tool axis coincides with the central axis 01 of the acetabular fossa in the real world scenario.
Step S08, controlling the mechanical arm 100 to move until the end of the mechanical arm 100 reaches the target pose, so as to complete the initial positioning of the mechanical arm 100. The pose of the tip of the robot arm 100 at this time is referred to as an initial pose of the tip of the robot arm 100.
Those skilled in the art will appreciate that during the bone milling process, the feed direction of the bone milling tool 400 tends to deviate from the central axis 01 of the acetabular fossa in the displayed view for various reasons. Thus, during the bone milling process, the control unit is configured to perform a robotic arm pose control method to coincide as much as possible the tool axis with the central axis 01 of the acetabular socket in real world, such that the bone milling tool 400 is fed along the central axis 01 of the acetabular socket in real world. The basic flow of the mechanical arm pose control method is shown in fig. 5, and the method comprises the following steps:
step S100, the current position of the first control point and the current position of the second control point are obtained. The first control point is the center point of the tool carrier 300 and the second control point is the center point of the bone tool 400, such that the lines connecting the first and second control points form the tool axis.
Step S200, acquiring a target position of the first control point and a target position of the second control point. It will be appreciated that when the first control point and the second control point are at respective target positions, the tool axis coincides with the central axis 01 of the acetabular fossa in a real scene.
Step S300, acquiring a first deviation between the target position and the current position of the first control point, and acquiring a second deviation between the target position and the current position of the second control point.
Step S400 provides impedance control based on the first deviation and the second deviation to constrain the robotic arm 100 such that the tool axis coincides with the central axis 01 of the acetabular socket in a real world scenario.
It is to be understood that "the tool axis coincides with the central axis 01 of the acetabular fossa in the real scene" herein includes both the case where the tool axis completely coincides with the central axis 01 of the acetabular fossa in the real scene and the case where the tool axis substantially coincides with the central axis 01 of the acetabular fossa in the real scene. It will also be appreciated that the robotic arm pose control method is repeatedly performed throughout the bone milling process.
Compared with the mode of performing single-point control by only utilizing the tail end control points of the mechanical arm in the prior art, the method for controlling the pose of the mechanical arm changes the single-point control stiffness into the linear control stiffness, effectively improves the control precision and improves the bone grinding effect.
Next, each step of the method for controlling the pose of the mechanical arm will be described in detail.
The operation of step S100 actually includes acquiring the current pose of the tool target 220, and then performing a kinematic solution on the mechanical arm 100 based on the current pose of the tool target 220 to obtain the current position of the first control point and the current position of the second control point.
In practice, during the bone milling operation performed on the acetabular fossa, the patient may slightly move under the action of the bone milling tool 400, resulting in a change in the pose of the center point of the acetabular fossa and a deviation of the pose of the center axis 01 of the acetabular fossa from its original pose in a real-world scenario. At this time, if the original pose of the mechanical arm base coordinate system (i.e., the pose of the mechanical arm base coordinate system before bone grinding) is still used to perform the kinematic solution, the obtained position of the first control point and the obtained position of the second control point are based on the relative position of the current pose of the central axis 01 of the acetabular fossa in the real scene, and the relative positions of the first control point and the second control point are used to control the pose of the mechanical arm, which is not beneficial to control the improvement of the rigidity.
In view of this, step S100 is preferably performed based on the real-time pose of the center point of the acetabular fossa. Specifically, as shown in fig. 6, step S100 includes step S110, step S120, and step S130.
Step S110 includes acquiring a current pose of a center point of an acetabular fossa. Optionally, a specific implementation of this step is to obtain the current pose of the central axis 01 of the acetabular fossa in the real scene based on the current pose of the bone target 210 and a third transformation matrix between the pose of the bone target 210 and the pose of the central axis 01 of the acetabular fossa in the real scene. It will be appreciated that the pose of the central axis 01 of the acetabular fossa in a real scene is related to the pose of the central point of the acetabular fossa, i.e. after the current pose of the central axis 01 of the acetabular fossa in a real scene is obtained, the current pose of the central point of the acetabular fossa is obtained. Further, the third transformation matrix may be performed at any time after the central axis 01 of the preoperatively planned acetabular fossa is mapped to the real scene, before the bone grinding, in such a manner that the pose of the bone target 210 before the bone grinding is acquired, and then the third transformation matrix is calculated based on the pose of the bone target 210 before the bone grinding and the initial pose of the central axis 01 of the acetabular fossa in the real scene.
Step S120 includes correcting the robot arm base coordinate system based on the current posture of the center point of the acetabular fossa such that the Y-axis of the corrected robot arm base coordinate system is parallel to the center axis 01 of the acetabular fossa in the real scene. In an exemplary implementation, the specific operation of this step is shown in fig. 7, including:
step S121, acquiring a component of the current posture of the center point of the acetabular fossa in the Y direction as a first vector. And acquiring a component of the posture of the mechanical arm base coordinate system before correction in the Y direction as a second vector. The Y-direction is parallel to the central axis 01 of the acetabular fossa in the current real world scenario.
Step S122, calculating an included angle between the first vector and the second vector, and a normal vector of a plane where the first vector and the second vector are located.
Step S123, based on the included angle and the normal vector obtained in step S122, calculating an axial angle matrix between the current posture of the central point of the acetabular fossa and the posture of the mechanical arm base coordinate system before correction, wherein the axial angle matrix can represent the difference between the current posture of the central point of the acetabular fossa and the posture of the mechanical arm base coordinate system before correction.
And step S124, converting the shaft angle matrix obtained in the step S123 into a rotation matrix through a Rodrigues formula, and taking the rotation matrix as a difference matrix between the current posture of the central point of the acetabular fossa and the posture of the mechanical arm base coordinate system before correction.
Step S125, calculating the pose of the corrected mechanical arm base coordinate system based on the pose of the mechanical arm base coordinate system before correction and the difference matrix obtained in step S124. The specific calculation formula is as follows: poseBase=PoseBase0.Terr, where PoseBase represents the pose of the corrected robot arm base coordinate system, poseBase0 represents the pose of the robot arm base coordinate system before correction, and Terr represents the difference matrix between the current pose of the center point of the acetabular fossa and the pose of the robot arm base coordinate system before correction.
Step S130 includes acquiring a current absolute position of the first control point as a current position of the first control point based on the corrected robot arm base coordinate system, and acquiring a current absolute position of the second control point as a current position of the second control point. The specific operation procedure is shown in fig. 8, and includes step S131, step S132 and step S133. Step S131 includes acquiring a first kinematic parameter corresponding to the first control point and a second kinematic parameter corresponding to the second control point, respectively, based on the corrected pose of the robot arm base coordinate system. It will be appreciated by those skilled in the art that the first control point corresponds to the end tool control point of the robotic arm 100 and the second control point is located on the bone tool 400, such that the portion of the bone tool 400 located between the second control point and the first control point may be considered as a link of the robotic arm, such that the robotic arm model corresponding to the first control point is different from the robotic arm model corresponding to the second control point, such that the first kinematic parameter and the second kinematic parameter are different. Step S132 includes performing a robot kinematic solution based on the first kinematic parameter to obtain a pose conversion matrix of the first control point, and performing a robot kinematic solution based on the second kinematic parameter to obtain a pose conversion matrix of the second control point. Step S132 includes obtaining an inverse matrix of the jacobian matrix of the first control point based on the pose conversion matrix of the first control point, thereby obtaining a current absolute position of the first control point, and obtaining an inverse matrix of the jacobian matrix of the second control point based on the pose conversion matrix of the second control point, thereby obtaining a current absolute position of the second control point.
Thus, the absolute positions of the first control point and the second control point can be used to control the pose of the end of the mechanical arm 100 in the subsequent operation, so as to further improve the control rigidity. Moreover, by correcting the mechanical arm base coordinate system, the decoupling of the position and the gesture of the tool axis is realized, and gesture deviation items are not existed any more, so that the problems of abrupt change of Euler angle gesture, failure of integral of shaft angle gesture, nonlinear mapping of quaternion gesture and the like are avoided when the step S400 is executed to acquire the impedance moment, the impedance parameter is reduced, the coupling of gesture control parameter and position control parameter is avoided, and the problems of limited output and reverse disturbance of the impedance moment are avoided.
The embodiment of the present invention is not particularly limited to the specific implementation manner of step S200. In a preferred implementation, however, step S200 is performed by using a location predictor, and the specific flow is shown in fig. 9, and includes:
step S210, acquiring the current position of the first control point and the previous n time positions, and acquiring the current position of the second control point and the previous n time positions.
Step S220, obtaining a predicted position of the first control point based on the current position of the first control point, the previous n times of positions, and the predetermined position prediction model, and obtaining a predicted position of the second control point based on the current position of the second control point, the previous n times of positions, and the predetermined position prediction model.
Step S230, obtaining a target position of the first control point based on the current position and the predicted position of the first control point, and obtaining a target position of the second control point based on the current position and the predicted position of the second control point.
When n is an even number, the predetermined prediction model includes a linear position prediction model and a polynomial position prediction model, the step S220 more specifically includes:
step S221, preceding the first control pointInputting the position at each moment into a linear position prediction model to obtain a primary predicted position of a first control point, and preceding a second control point>And inputting the positions at each moment into a linear position prediction model to obtain the primary predicted position of the second control point.
Step S222, the primary predicted position of the first control point is followed by the first control pointThe position of each moment and the current position of the second control point are input into a polynomial position prediction model to obtain the predicted position of the first control point, and the primary predicted position of the second control point and the second control point are behind->And inputting the positions at each moment and the current position of the second control point into a polynomial position prediction model to obtain the predicted position of the second control point.
In a specific embodiment, n may be 4. Thus, step S210 acquires the positions of the first control point at five consecutive times, and acquires the positions of the second control point at five consecutive times. Five positions of the first control point are respectively designated as P11, P12, P13, P14, P15, and five positions of the second control point are respectively designated as P21, P22, P23, P24, P25 in order from first to last. It will be appreciated that P15 is the current position of the first control point and P25 is the current position of the second control point.
Step S221 includes inputting P11, P12 and P13 into the linear position prediction model to obtain a primary predicted position of the first control point, denoted as prediction 11, and inputting 21, bits P22 and P23 into the linear position prediction model to obtain a primary predicted position of the second control point, denoted as prediction 21.
Step S222 includes inputting the predictions 11, P14 and P15 into a polynomial position prediction model to obtain a predicted position of the first control point, and recording the predicted position as the prediction 12; and inputting the predictions 21, P24 and P25 into a polynomial position prediction model to obtain a predicted position of the second control point, and recording the predicted position as the prediction 22.
Step S230 may calculate the target position of the first control point based on the formula TP1 = confidence p15+ (1-confidence) x prediction 12, and calculate the target position of the second control point based on the formula TP2 = confidence p25+ (1-confidence) x prediction 22. In the formula, TP1 represents the target position of the first control point, TP2 represents the target position of the second control point, and confidence represents a preset confidence.
The specific operation of step S300 is as shown in fig. 10, and includes:
step S310, obtaining a Chebyshev distance between the current position of the first control point and the target position; and obtaining the Chebyshev distance between the current position of the second control point and the target position.
Step S320, performing second-order processing on the Chebyshev distance between the current position of the first control point and the target position to obtain a first processing distance; and performing second-order processing on the Chebyshev distance between the current position of the second control point and the target position to obtain a second processing distance.
Step S330, the first processing distance and the second processing distance are projected to the Y direction, respectively, and the projected value of the first processing distance in the Y direction is used as a first deviation, and the projected value of the second processing distance in the Y direction is used as a second deviation.
In an alternative embodiment, step S310 obtains the euclidean distance between the current position of the first control point and the target position, and the euclidean distance between the current position of the second control point and the target position. The first and second deviations obtained using chebyshev distances are more accurate than the way in which the first and second deviations are obtained using euclidean distances. And when the step S200 is executed by the position predictor and the step S300 is executed based on the Chebyshev distance, the problem of numerical value jump can be effectively avoided, and the accuracy of the acquired first deviation and second deviation is further improved.
Referring to fig. 5 and 6, step S400 includes:
step S410, acquiring a first sub-impedance moment based on the first deviation, and acquiring a second sub-impedance moment based on the second deviation.
In step S420, a first resisting moment is obtained based on the first sub-resisting moment and the second sub-resisting moment, and the first resisting moment is applied to the mechanical arm 100 to constrain the mechanical arm 100.
In step S430, a power mechanism is controlled to apply a first resistance moment to the mechanical arm 100, so that the tool axis coincides with the central axis 01 of the acetabular fossa in the real scene.
The specific operation of step S410 includes calculating a first correction moment related to the stiffness of the mechanical arm 100, a second correction moment related to the damping of the mechanical arm 100, and an inertia compensation moment related to the inertia of the mechanical arm 100 according to the first deviation; and calculating a first correction moment related to the stiffness of the mechanical arm 100, a second correction moment related to the damping of the mechanical arm 100, and an inertia compensation moment related to the inertia of the mechanical arm 100 from the second deviation. Then, a first sub-impedance moment is obtained based on the first correction moment, the second correction moment and the inertia compensation moment corresponding to the first deviation; and deriving a second sub-impedance moment based on the first correction moment, the second correction moment, and the inertia compensation moment corresponding to the second deviation.
In practice, step S410 is to input the first deviation into a first impedance controller pre-stored in the control unit and output the first sub-impedance moment by the first impedance controller, and to input the second deviation into a second impedance controller pre-stored in the control unit and output the second sub-impedance moment by the second impedance controller.
In the embodiment of the present invention, the first impedance controller and the second impedance controller have the same structure, and each of the first impedance controller and the second impedance controller includes an input module 501, a first computing module 502, a second computing module 503, a third computing module 504, and a fourth computing module 505 as shown in fig. 11, where the first computing module 502, the second computing module 503, and the third computing module 504 are all communicatively connected to the input module 501, and the fourth computing module 505 is communicatively connected to the first computing module 502, the second computing module 503, and the third computing module 504.
In operation, the input module 501 is configured to input a corresponding deviation (first deviation or second deviation). The first calculation module 502 is configured to calculate a first correction torque related to the stiffness of the mechanical arm 100 based on the deviation, and the second calculation module 503 is configured to calculate a differential based on the input deviation, and calculate a second correction torque related to the damping of the mechanical arm 100 based on the differential, the differential being a difference between the deviation input at the previous time and the deviation input at the current time. The third calculation module 504 is configured to calculate an inertial amount based on the input deviation, and calculate an inertia compensation moment related to the inertia of the mechanical arm 100 based on the inertial amount, which is well known to those skilled in the art, and is not described herein. The fourth calculation module 504 is configured to calculate a sub-resistance moment based on the first correction moment, the second correction moment, and the inertia compensation moment.
The calculation formula of the first correction moment is as follows: f1 The =stiffnesscoe x jointstiffnesssratio x APE. The calculation formula of the second correction torque is as follows: f2 =campingee jointcampingratio DAPE. The calculation formula of the inertia compensation moment is as follows: f3 =compcoe×oape. The calculation formula of the sub-impedance moment is as follows: f=f1+f2+f3. Where f1 represents a first correction torque, stiffnesscue represents a stiffness parameter of the arm 100, jointstiffnesscratio represents a stiffness coefficient of a joint of the arm 100, APE represents an input deviation (i.e., a first deviation or a second deviation), f2 represents a second correction torque, dammingcom represents a damping coefficient of the arm 100, jointdammingratio represents a damping coefficient of a joint of the arm 100, and DAPE represents differentiation; f3 represents the inertia compensation torque, comp ce represents the inertia parameter of the robot arm 100, OAPE represents the inertia amount, and F represents the sub-impedance torque.
Those skilled in the art will recognize that the impedance controller requires parameter tuning during the design process. As can be seen from the above description, the first impedance controller and the second impedance controller each have three parameters including a stiffness term, a damping term, and an inertia compensation term, and thus the three parameters need to be set in the process of designing the impedance controllers. Fig. 12 is a schematic diagram showing a process of parameter tuning by the impedance controller according to the embodiment of the present invention, please refer to fig. 12, wherein the parameter tuning includes steps S001, S002 and S003 executed in sequence. Step S001 includes designing a stiffness term parameter, fixing a step length, and setting a stiffness reference value of the mechanical arm 100 until the impedance controller is stable without overshoot. The design of the stiffness term parameter satisfies the formula: stiffnesscoe=baseval =motorlnertia, where Baseval represents a stiffness reference value of the robot arm 100, and motorlnerta represents an intrinsic property of an articulation motor of the robot arm 100. Step S002 includes designing damping term parameters by using the adjusted stiffness parameters of the mechanical arm 100, and adjusting the damping scaling constant ratio to the impedance controller to be stable without oscillation. The damping term parameter design satisfies the formula: dammingcoe=stiffnessccoe, where scalratio represents the damping scaling constant ratio. Step S003 includes designing the inertia compensation term parameters and tuning the bandwidth until the impedance controller is stable without static difference. The inertia compensation term parameter design satisfies the formula: compCoe = Bandwidth CutoffFreq/Cycletime, where Bandwidth represents Bandwidth, cutoffFreq represents cut-off Bandwidth, and Cycletime represents sampling time. Such a setting can avoid the problem of static instability. Moreover, after the current pose of the central point of the acetabular fossa is utilized to correct the mechanical arm base coordinate system, the impedance parameters are reduced (namely, the pose deviation term is not provided any more), so that the parameter setting efficiency of the impedance controller is improved. It is understood that at least one of the stiffness term, the damping term, and the inertia compensation term of the first and second impedance controllers is different.
Step S420 is to calculate the sum of the first sub-impedance moment and the second sub-impedance moment as the first impedance moment.
Further, the control unit also executes the first protection strategy and/or the second protection strategy in real time during the bone grinding process, so as to ensure the operation safety of the mechanical arm 100 and reduce the occurrence of accidents.
Referring to fig. 13 and 14, the first protection policy includes:
step S510, planning a virtual safety boundary 610 based on the current pose of the center point of the acetabular fossa.
Step S520, determine whether the end of the mechanical arm 100 touches the virtual safety boundary 610, if yes, execute step S530 and step S540.
Step S530 includes calculating a second resistance moment, and controlling a power mechanism to apply the second resistance moment to the arm 100 to drive the arm 100 to move to the end of the arm 100 back to the virtual safety boundary 610.
Step S540 includes generating first alarm information and controlling a display device to display the first alarm information.
In one exemplary embodiment, virtual safety boundary 610 includes a first circular surface 611, a second circular surface 612, and a side surface 613, side surface 613 being a portion of a conical surface. The diameter of the first circular surface 611 is smaller than that of the second circular surface 612, the center of the first circular surface 611 is the center point of the acetabular fossa, the second circular surface 612 is coaxial with the first circular surface 611, and the second circular surface 612 is close to the tail end of the mechanical arm 100. In this way, the central axis 01 of the acetabular fossa in the real scene forms the central axis of the virtual safety boundary 610, and the space surrounded by the virtual safety boundary 610 is in the shape of a truncated cone. It should be understood that the diameter of the first circular surface 611, the diameter of the second circular surface 612, and the distance between the first circular surface 611 and the second circular surface 612 are all set according to actual needs, and the spatial pose of the virtual safety boundary 610 can be calculated according to the pose of the center point of the acetabular fossa, and a specific calculation method is known to those skilled in the art, and is not described herein.
The specific step of step S520 includes obtaining a distance vector between the first control point and the second control point as a third vector, and obtaining a distance vector from the first control point to the virtual safety boundary 610 as a fourth vector, wherein the third vector is directed to the second control point by the first control point, the fourth vector is directed to the first target point on the virtual safety boundary 610 by the first control point, and the fourth vector is perpendicular to a tangent line at the first target point. Then calculating the included angle between the third vector and the fourth vector, and calculating the vector modulus of the fourth vector. Then, whether the included angle between the third vector and the fourth vector is larger than a first preset value and whether the vector modulus of the fourth vector is larger than a second preset value are determined, and if the included angle between the third vector and the fourth vector is larger than the first preset value and the vector modulus of the fourth vector is larger than the second preset value, then it is determined that the end of the mechanical arm 100 touches the virtual boundary 610. It should be noted that, when the end of the mechanical arm 100 touches the virtual safety boundary, at least one of the first control point and the second control point is located outside the virtual safety boundary.
Step S530 includes obtaining a distance vector from the second control point to the virtual safety boundary 610 as a fifth vector, the fifth vector pointing from the second control point to the second target point on the virtual safety boundary 610, and the fifth vector being perpendicular to a tangent line at the second target point. The vector modulus of the fifth vector is then calculated. And inputting the vector modulus of the fourth vector into a third impedance controller pre-stored in the control unit to obtain a third sub-impedance moment, inputting the vector modulus of the fifth vector into the fourth impedance controller pre-stored in the control unit to obtain a fourth sub-impedance moment, and finally calculating the sum of the third sub-impedance moment and the fourth sub-impedance moment to be used as a second impedance moment.
The third impedance controller and the fourth impedance controller have the same construction as the first impedance controller in the foregoing description, and the parameter setting process is also the same, except that specific values of at least one of the stiffness term, the damping term, and the inertia compensation term are different.
Referring to fig. 15, the second security policy includes:
step S710, obtaining target information.
Step S720, judging whether the target information is abnormal, if so, executing step S730 and step S740.
Step S730, generating second alarm information, and controlling a display device to display the second alarm information.
Step S740, locking the mechanical arm 100 to prevent the mechanical arm 100 from moving.
The target information includes at least one of a current pose of a center point of the acetabular fossa, a deviation between the current pose of the distal end of the mechanical arm 100 and a pose at a previous time, and a projection of the deviation between the current pose of the distal end of the mechanical arm 100 and its initial pose in a direction perpendicular to the Y direction.
When the target information comprises the current pose of the central point of the acetabular fossa, the abnormal target information refers to the situation that the current pose of the central point of the acetabular fossa acquired by the control unit is any one of a non-numerical value, a blank signal and an infinite numerical value. Here, the reason why the current pose of the center point of the acetabular fossa acquired by the control unit is non-numeric may be an abnormality in solving the pose of the center of the acetabular fossa based on the current pose of the bone target 210; the reason why the current pose of the center point of the acetabular fossa acquired by the control unit is a blank signal may be that the locator 230 loses a frame or the bone target 210 is blocked; the reason why the current pose obtained by the control unit is an infinite number may be that a zero-division situation occurs in the process of calculating the pose of the center point of the acetabular fossa based on the tool target 210. When the pose of the center point of the acetabular fossa acquired by the control unit is abnormal, that is, the control unit does not acquire the actual pose of the center point of the acetabular fossa, in this case, if the mechanical arm 100 is still allowed to move, a safety accident may be caused.
When the target information includes a deviation between the current pose of the distal end of the robot arm 100 and the pose at the previous time, the target information abnormality means that the deviation between the current pose of the distal end of the robot arm 100 and the pose at the previous time is greater than a third preset value. When the target information includes a projection of a deviation between the current pose of the distal end of the robot arm 100 and the initial pose in the vertical direction of the Y direction, the target information abnormality means that a projection of a deviation between the current pose of the distal end of the robot arm 100 and the initial pose in the vertical direction of the Y direction is greater than a fourth preset value.
Further, the embodiment of the invention also provides a computer readable storage medium, on which a program is stored, and when the program is executed, the foregoing robot pose adjustment method is executed.
Although the present invention is disclosed above, it is not limited thereto. Various modifications and alterations of this invention may be made by those skilled in the art without departing from the spirit and scope of this invention. Thus, it is intended that the present invention also include such modifications and alterations insofar as they come within the scope of the appended claims or the equivalents thereof.
Claims (10)
1. The method for controlling the pose of the mechanical arm is characterized by comprising the following steps of:
respectively acquiring the current positions of a first control point and a second control point; the first control point is a center point of a tool loading part connected to the tail end of the mechanical arm, the second control point is a center point of an operation tool connected to the tool loading part, a connecting line of the second control point and the first control point forms a tool axis, and the tool axis is overlapped with the axis of the tail end of the mechanical arm;
respectively acquiring target positions of the first control point and the second control point; when the first control point and the second control point are respectively located at corresponding target positions, the tool axis coincides with a central axis of an anatomical structure;
acquiring a first deviation between the current position of the first control point and a corresponding target position, and acquiring a second deviation between the current position of the second control point and the corresponding target position; the method comprises the steps of,
impedance control is provided based on the first deviation and the second deviation to constrain the robotic arm such that the tool axis coincides with a central axis of the anatomical structure.
2. The robot pose control method according to claim 1, wherein the step of providing impedance control based on the first deviation and the second deviation comprises:
acquiring a first sub-impedance moment based on the first deviation, and acquiring a second sub-impedance moment based on the second deviation;
acquiring a first impedance moment based on the first sub-impedance moment and the second sub-impedance moment; the first resistance moment is used for being applied to the mechanical arm to restrain the mechanical arm.
3. The method according to claim 2, wherein the step of acquiring the first sub-impedance moment based on the first deviation includes:
calculating a first correction moment related to the stiffness of the mechanical arm, a second correction moment related to the damping of the mechanical arm, and an inertia compensation moment related to the inertia of the mechanical arm based on the first deviation;
acquiring the first sub-impedance moment based on the first correction moment, the second correction moment and the inertia compensation moment corresponding to the first deviation; and/or the number of the groups of groups,
the step of obtaining a second sub-impedance moment based on the second deviation includes:
Calculating a first correction torque related to the stiffness of the mechanical arm, a second correction torque related to the damping of the mechanical arm, and an inertia compensation torque related to the inertia of the mechanical arm based on the second deviation;
the second sub-impedance moment is obtained based on the first correction moment, the second correction moment, and the inertia compensation moment corresponding to the second deviation.
4. The method according to claim 1, wherein the step of acquiring the current positions of the first control point and the second control point includes:
acquiring a current pose of a center point of the anatomical structure;
correcting the pose of a mechanical arm base coordinate system based on the current pose of the central point of the anatomical structure, wherein the Y axis of the mechanical arm base coordinate system after correction is parallel to the central axis of the anatomical structure;
acquiring mechanical arm kinematics parameters corresponding to the first control point based on the corrected pose of the mechanical arm base coordinate system, and executing mechanical arm kinematics calculation based on the mechanical arm kinematics parameters corresponding to the first control point to obtain the current absolute position of the first control point; the method comprises the steps of,
And acquiring mechanical arm kinematics parameters corresponding to the second control point based on the corrected pose of the mechanical arm base coordinate system, and executing mechanical arm kinematics calculation based on the mechanical arm kinematics parameters corresponding to the second control point to obtain the current absolute position of the first control point.
5. The method according to claim 4, wherein the step of correcting the pose of the robot base coordinate system based on the current pose of the center point of the anatomical structure includes:
acquiring a component of a current posture of a center point of the anatomical structure in a Y direction as a first vector, and acquiring a component of a posture of the mechanical arm base coordinate system before correction in the Y direction as a second vector; the Y-direction is parallel to a central axis of the anatomical structure;
acquiring an included angle between the first vector and the second vector, and acquiring a normal vector of a plane where the first vector and the second vector are located;
acquiring an axial angle matrix based on the included angle and the normal vector;
obtaining a difference matrix of the current gesture of the target point and the gesture of the mechanical arm base coordinate system before correction based on the axis angle matrix;
And obtaining the corrected pose of the mechanical arm base coordinate system based on the difference matrix and the pose of the mechanical arm base coordinate system before correction.
6. The robot pose control method according to claim 1, wherein the step of acquiring the target position of the first control point includes:
acquiring the current position of the first control point and the positions of n previous moments;
obtaining a predicted position of the first control point based on the current position of the first control point and the previous n times of positions;
obtaining the target position of the first control point based on the current position of the first control point and the predicted position of the first control point; and/or the number of the groups of groups,
the step of obtaining the target position of the second control point includes:
acquiring the current position of the second control point and the positions of n previous moments;
obtaining a predicted position of the second control point based on the current position of the second control point and the previous n times of positions;
and obtaining the target position of the second control point based on the current position of the second control point and the predicted position of the second control point.
7. The method for controlling the pose of a mechanical arm according to claim 6, wherein n is an even number;
The step of obtaining the predicted position of the first control point based on the current position of the first control point and the previous n times of positions includes:
the first control point is precededInputting the positions at each moment into a linear position prediction model to obtain a primary predicted position of the first control point;
the primary predicted position of the first control point, the first control point being followed byThe positions of the moments and the current position of the first control point are input into a polynomial position prediction model to obtain the predicted position of the first control point; and/or the number of the groups of groups,
the step of obtaining the predicted position of the second control point based on the current position of the second control point and the previous n times of positions includes:
will be spentThe second control point is in frontInputting the positions at each moment into a linear position prediction model to obtain a primary predicted position of the second control point;
the primary predicted position of the second control point, the second control point being followed byAnd (3) inputting the positions of the moments and the current position of the second control point into a polynomial position prediction model to obtain the predicted position of the second control point.
8. The robot arm pose control method according to any one of claims 1 to 7, wherein the step of acquiring the first deviation comprises:
obtaining a chebyshev distance between the current position of the first control point and a corresponding target position;
performing second-order processing on the Chebyshev distance between the current position of the first control point and the corresponding target position to obtain a first processing distance;
projecting the first processing distance to a Y direction, and taking the projected value of the first processing distance in the Y direction as the first deviation and/or,
the step of obtaining the second deviation comprises:
obtaining a chebyshev distance between the current position of the second control point and the corresponding target position;
performing second-order processing on the Chebyshev distance between the current position of the second control point and the corresponding target position to obtain a second processing distance;
projecting the second processing distance to a Y direction, and taking the projected value of the second processing distance in the Y direction as the second deviation;
the Y-direction is parallel to a central axis of the anatomical structure.
9. The robot pose control method according to claim 1, characterized in that the robot pose control method further comprises:
Planning a virtual safety boundary based on the pose of the center point of the anatomical structure;
judging whether the tail end of the mechanical arm touches the virtual safety boundary, if so, generating and controlling a display device to display alarm information, and generating a second impedance moment; the second impedance moment is used for being applied to the mechanical arm so as to drive the mechanical arm to move until the tail end of the mechanical arm returns to the virtual safety boundary; and/or, the mechanical arm pose control method further comprises the following steps:
acquiring target information;
judging whether the target information is abnormal, if so, generating and controlling a display device to display alarm information, and locking the mechanical arm;
the target information is at least one of a current pose of a center point of the anatomical structure, a deviation between the current pose of the tail end of the mechanical arm and a previous moment pose, and a projection of the deviation between the current pose of the tail end of the mechanical arm and an initial pose in a direction perpendicular to a Y direction; the Y-direction is parallel to a central axis of the anatomical structure.
10. A surgical robotic system, comprising:
the tail end of the mechanical arm is connected with the bone grinding tool through the tool loading piece;
The navigation device comprises a locator and a tool target, wherein the tool target is used for being arranged at the tail end of the mechanical arm; the positioner is used for identifying the tool target to acquire the pose of the tail end of the mechanical arm, and the pose of the tail end of the mechanical arm is used for acquiring the center point of the tool loading piece and the center point of the operating tool; the method comprises the steps of,
a control unit communicatively connected to the positioning device and configured to perform the robot pose control method according to any of claims 1-9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410105285.9A CN117679181A (en) | 2024-01-25 | 2024-01-25 | Mechanical arm pose control method and surgical robot system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410105285.9A CN117679181A (en) | 2024-01-25 | 2024-01-25 | Mechanical arm pose control method and surgical robot system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117679181A true CN117679181A (en) | 2024-03-12 |
Family
ID=90137381
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410105285.9A Pending CN117679181A (en) | 2024-01-25 | 2024-01-25 | Mechanical arm pose control method and surgical robot system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117679181A (en) |
-
2024
- 2024-01-25 CN CN202410105285.9A patent/CN117679181A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3342561B1 (en) | Remote control robot system | |
| US7646161B2 (en) | Method for controlling a robot arm, and robot for implementing the method | |
| Funda et al. | Constrained Cartesian motion control for teleoperated surgical robots | |
| EP1462224B1 (en) | Taught position modification device | |
| US5399951A (en) | Robot for guiding movements and control method thereof | |
| CN107430389B (en) | Ultra smart system user interface | |
| EP1728600B1 (en) | Controlling the trajectory of an effector | |
| CN112245014B (en) | Medical robot, method for detecting collision of mechanical arm and storage medium | |
| CN110464470B (en) | Surgical robot and control method and control device for arm body of surgical robot | |
| Xia et al. | Model-based telerobotic control with virtual fixtures for satellite servicing tasks | |
| EP3946833B1 (en) | Reducing energy buildup in servo-controlled mechanisms | |
| CN110625611A (en) | Mechanical arm auxiliary component assembling method and system based on laser tracking measurement and force sensing combined control | |
| US11547495B2 (en) | System and method for reducing interference in positional sensors for robotic surgery | |
| US20060287772A1 (en) | Device and method for programming an industrial robot | |
| US20210229279A1 (en) | Hybrid control of a robotic system | |
| EP3871841A1 (en) | Robot control | |
| CN111803213B (en) | Cooperative robot guiding and positioning method and device | |
| CN117679181A (en) | Mechanical arm pose control method and surgical robot system | |
| CN115337072A (en) | Acetabulum rasp angle calibration method and system based on optical positioning | |
| CN113855257A (en) | Self-adaptive adjustment method for surgical visual field | |
| US20210260760A1 (en) | Method and system for transferring an end effector of a robot between one end effector pose and a further end effector pose | |
| Ranne et al. | Augmented reality-assisted reconfiguration and workspace visualization of malleable robots: Workspace modification through holographic guidance | |
| CA2974996C (en) | Navigation by bending forces | |
| CN117770967A (en) | Surgical robot system motion control device, method and computer readable medium | |
| CN113703461B (en) | Path determination method, path determination device, robot, and readable storage medium |
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 |