CN114391958B - Effective working space calculation method of mechanical arm and control method thereof - Google Patents
Effective working space calculation method of mechanical arm and control method thereof Download PDFInfo
- Publication number
- CN114391958B CN114391958B CN202210136042.2A CN202210136042A CN114391958B CN 114391958 B CN114391958 B CN 114391958B CN 202210136042 A CN202210136042 A CN 202210136042A CN 114391958 B CN114391958 B CN 114391958B
- Authority
- CN
- China
- Prior art keywords
- joint
- coordinate system
- mechanical arm
- tail end
- maximum motion
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- 238000004364 calculation method Methods 0.000 title abstract description 8
- 230000009466 transformation Effects 0.000 claims abstract description 53
- 230000007306 turnover Effects 0.000 claims abstract description 8
- 239000000700 radioactive tracer Substances 0.000 claims description 21
- 230000003287 optical effect Effects 0.000 claims description 8
- 230000001154 acute effect Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000009434 installation Methods 0.000 claims description 2
- 238000002591 computed tomography Methods 0.000 description 12
- 230000003902 lesion Effects 0.000 description 5
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000001574 biopsy Methods 0.000 description 2
- 210000004072 lung Anatomy 0.000 description 2
- 208000020816 lung neoplasm Diseases 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 206010058467 Lung neoplasm malignant Diseases 0.000 description 1
- 206010028980 Neoplasm Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000994 depressogenic effect Effects 0.000 description 1
- 238000003745 diagnosis Methods 0.000 description 1
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 201000005202 lung cancer Diseases 0.000 description 1
- 208000037841 lung tumor Diseases 0.000 description 1
- 238000010827 pathological analysis Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- 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
Landscapes
- Health & Medical Sciences (AREA)
- Surgery (AREA)
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Biomedical Technology (AREA)
- Robotics (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a calculation method and a control method for an effective working space of a mechanical arm, wherein the tail end of the mechanical arm realizes pitching through two parallel moving joints, overturning is realized through an overturning joint, and a pitching axis is mutually perpendicular to an overturning axis; solving the transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system; obtaining a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly obtaining a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly obtaining a pitch angle and a turnover angle of the tail end of the mechanical arm; and combining the transformation relation between the joint coordinate system of the mechanical arm and the robot base coordinate system and calculating the design parameters of the mechanical arm to obtain the maximum movement range of the tail end of the mechanical arm. The method for calculating the effective working space of the mechanical arm is simple in calculation, the distance required to move the CT bed can be calculated based on the method and the pose of a planning channel, and a doctor can move the CT bed according to the distance, so that the operation is simple and convenient.
Description
Technical Field
The invention relates to the technical field of surgical robots, in particular to a method for calculating an effective working space of a mechanical arm and a control method thereof.
Background
Lung cancer is one of the most life and health threatening cancers, and lung biopsy is the gold standard for lung tumor diagnosis. The lung biopsy operation requires a doctor to take out a lesion for pathological analysis, and generally, the doctor adopts a robot to extract the lesion. In order to realize accurate puncture operation, the current operation robot has complex structure and complicated control procedure, so the operation robot has higher manufacturing cost and has higher space requirement on an operation room.
In order to reduce the production and manufacturing costs and the requirements on the room space of the operation, the surgical robot is improved in the miniaturization and simplification directions, the effective working space of the improved surgical robot is reduced, and in order to realize accurate operation, a method for clearly obtaining the effective working space of the surgical robot and the corresponding control flow before the actual operation is needed to be provided.
Disclosure of Invention
The invention aims to: aiming at the problems, the invention provides a method for calculating the effective working space of a mechanical arm of a surgical robot with simple structure and convenient operation and control and a control method thereof.
The technical scheme is as follows:
A mechanical arm effective working space calculating method comprises the steps that pitching is achieved at the tail end of a mechanical arm of the mechanical arm through two parallel moving joints, overturning is achieved through the overturning joints, and a pitching axis is perpendicular to an overturning axis;
Solving the transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system;
Obtaining a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly obtaining a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly obtaining a pitch angle and a turnover angle of the tail end of the mechanical arm;
And combining the transformation relation between the joint coordinate system of the mechanical arm and the robot base coordinate system and calculating the design parameters of the mechanical arm to obtain the maximum movement range of the tail end of the mechanical arm.
The pitch angle and the roll angle are calculated as follows:
The direction vector of the planning channel under the robot base coordinate system is cd (x, y, z), the pitch angle theta=90-arccos (cd.x) is calculated, wherein arccos is an inverse cosine function, and cd.x represents the scalar of the direction vector cd (x, y, z) of the planning channel under the robot base in the x-axis direction;
Let dx be the unit vector (1, 0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) of the planned channel under the robot base mark and the vector dx; u.y is a scalar quantity of u in the y-axis direction, u.z is a scalar quantity of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α= -arccos (u.y), and if u.z is 0 or more, the flip angle α= arccos (u.y).
The two parallel movable joints are a sixth joint and a fifth joint arranged below the sixth joint, the sixth joint is hinged with the tail end of the mechanical arm through a connecting piece, the hinge point is B, the hinge point of the fifth joint and the tail end of the mechanical arm is A, and when the fifth joint and the sixth joint are in an initial position, the connecting line between the hinge point A and the hinge point B is perpendicular to the moving directions of the fifth joint and the sixth joint.
The maximum motion range of the tail end of the mechanical arm is calculated as follows:
Knowing a pitch angle theta, knowing that the distance between the tail ends of two parallel joints is m and the vertical distance is L when the two parallel joints are at an initial position, and after the two parallel joints move asynchronously, the distance between the tail ends is s, H 1 is the length of a connecting piece, and H 2 is the distance from a hinge point A to a hinge point B;
When the tail end of the mechanical arm is in the upward state, at this time, the acute angle formed by the connecting line of the tail ends of the two parallel joints and the moving direction of the two joints is theta 1, and the moving difference d 1 of the two parallel joints is obtained as follows:
the minimum limit position and the maximum limit position of the tail end of the mechanical arm are calculated by combining the transformation relation between the coordinate systems of all joints of the mechanical arm and the base coordinate system of the robot;
When the tail end of the mechanical arm is in a depression state, at this time, an acute angle formed by a connecting line of the tail ends of the two parallel joints and the moving direction of the two joints is theta 2, and then a moving difference d 2 of the two parallel joints is obtained as follows:
d2＝m-L×tanθ2
And calculating the minimum limit position and the maximum limit position of each joint of the mechanical arm when the tail end of the mechanical arm is in a depression state by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system.
The space covered by the tail end of the mechanical arm moving from the maximum limit position in the depression state to the maximum limit position in the elevation state is the maximum movement range of the mechanical arm.
The mechanical arm further comprises a first joint moving along a first direction perpendicular to the moving direction of the fifth joint, and a second joint and a third joint moving along a second direction perpendicular to the first direction and the moving direction of the fifth joint;
The joint parameter corresponding to the minimum limit position of the tail end of the mechanical arm is i min{0,0,0,α,d1, 0 when the tail end of the mechanical arm is in the upward state, the joint parameter corresponding to the maximum limit position is i max, if the maximum motion stroke of the sixth joint Z6 minus the maximum motion stroke of the fifth joint Z5 is less than or equal to d 1,imax, the joint parameter is (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, alpha, the maximum motion stroke of the fifth joint Z5 +d 1), otherwise, i max is (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, alpha, the maximum motion stroke of the sixth joint Z6-d 1, the maximum motion stroke of the sixth joint Z6), and alpha are the flip angles;
When the tail end of the mechanical arm is in a depression state, the joint parameters j min{0,0,0,α,d2 and 0 corresponding to the minimum limit position and the joint parameters corresponding to the maximum limit position of the tail end of the mechanical arm are j max, if the maximum motion stroke of the fifth joint Z5 minus the maximum motion stroke of the sixth joint Z6 is greater than or equal to d 2,jmax, the joint parameters are (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, alpha, the maximum motion stroke of the sixth joint Z6+d 2, the maximum motion stroke of the sixth joint Z6), otherwise, j max is (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, alpha, the maximum motion stroke of the fifth joint Z5-d 2), and alpha are flip angles.
The transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system is calculated as follows:
defining a robot base coordinate system as follows:
Then:
Wherein dz 1 is the travel of the first joint and dy 2 is the travel of the second joint; y 2 is the zero difference in the y direction of the coordinate system origin of the second joint relative to the coordinate system origin of the first joint, z 2 is the zero difference in the z direction of the coordinate system origin of the second joint relative to the coordinate system origin of the first joint, x 5 is the zero difference in the x direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, dx 5 is the travel of the fifth joint, z 5 is the zero difference in the z direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, x 6 is the zero difference in the x direction of the coordinate system origin of the sixth joint relative to the coordinate system origin of the fourth joint, dx 6 is the travel of the sixth joint, z 6 is the zero difference in the z direction of the coordinate system origin of the sixth joint relative to the coordinate system origin of the fourth joint, x 7 is the zero difference in the x direction of the coordinate system origin of the seventh joint relative to the coordinate system origin of the sixth joint, and z 7 is the zero difference in the z direction of the coordinate system origin of the seventh joint relative to the coordinate system origin of the sixth joint; t 1 is a first joint coordinate system, T 2 is a second joint coordinate system, T 3 is a third joint coordinate system, T 4 is a fourth joint coordinate system, T 5 is a fifth joint coordinate system, T 6 is a sixth joint coordinate system, T 7 is a mechanical arm end coordinate system, θ is a pitch angle of a mechanical arm end, and α is a flip angle of the mechanical arm end.
The transformation relation between the image coordinate system and the robot base coordinate system is specifically:
Solving according to the posture parameters of each joint of the mechanical arm to obtain a transformation relation of the tail end of the mechanical arm relative to a robot base coordinate system, and marking the transformation relation as C base_tool;
obtaining a transformation relation between the tail end tracer and the tail end of the mechanical arm according to the installation parameters of the tail end tracer installed on the mechanical arm, and marking the transformation relation as C tool_et;
The conversion relation between the end tracer and the registered tracer of the affected part of the patient and the optical navigation equipment is obtained according to the identification of the optical navigation equipment and is respectively marked as C ots_et and C ots_regist;
Obtaining a transformation relation between an image coordinate system and the registration tracer according to the pose of the registration tracer in the CT image, and marking the transformation relation as C regist_img;
Calculating to obtain a transformation relation C base_img between the image coordinate system and the robot base coordinate system:
Cbase_img＝Cbase_tool*Ctool_et*(Cots_et)inv*Cots_regist*Cregist_img
Wherein inv is matrix inversion operation.
A method of controlling a surgical robot, comprising:
calculating to obtain a center point of the effective working space of the mechanical arm according to the effective working space calculating method of the mechanical arm;
Calculating the positions of a starting point and an end point of a planning channel under the image coordinate system under the robot base coordinate system according to the transformation relation between the image coordinate system and the robot base coordinate system, and taking the starting point as a target point of the end of the mechanical arm, wherein the point extends by a set length from the starting point to the end point to the starting point direction;
The calculated difference value between the target point and the central point in the x, y and z axis directions of the robot base coordinate system is used for obtaining the distance required to move by the CT bed;
and the doctor moves the CT bed according to the calculated distance, and the planning result is executed after the CT bed moves in place.
The set length is 150mm.
The beneficial effects are that: the invention provides a corresponding method for calculating the effective working space of a mechanical arm, and the distance required to move a CT (computed tomography) bed is calculated by combining the pose of a planning channel based on the calculation result, so that a doctor only needs to move the CT bed according to the distance, and the operation is simple and convenient.
Drawings
FIG. 1 is a schematic view of a surgical robot according to the present invention;
FIG. 2 is a partial side view of the surgical robotic arm of the present invention;
FIG. 3 is a schematic diagram of the invention with the end of the arm rotated upward;
FIG. 4 is a schematic diagram of the invention with the end of the arm rotated downward;
fig. 5 is a schematic diagram of the kinematic parameters of the robotic arm joint.
Wherein 1 is a base, 2 is a mechanical arm, 21 is a mechanical arm tail end, 22 is a tail end tracer, 23 is an executing instrument, and 3 is a puncture needle.
Detailed Description
The invention is further elucidated below in connection with the drawings and the specific embodiments.
Fig. 1 is a schematic structural view of a surgical robot according to the present invention, and referring to fig. 2, the surgical robot according to the present invention includes a base 1, a robot arm 2, and a robot arm end 21. The mechanical arm tail end 21 is provided with a tail end tracer 22 and an execution instrument 23, and the execution instrument 23 is provided with an execution channel for penetrating the puncture needle 3. The robot arm 2 includes a first driving part connected to the base 1, a first link driven by the first driving part to move relative to the base 1, a second driving part connected to the first link, a second link driven by the second driving part, a third driving part connected to the second link, a third link driven by the third driving part, a fourth driving part connected to the third link, a fourth link driven by the fourth driving part, a fifth driving part and a sixth driving part connected to the fourth link, and a fifth link and a sixth link connected to the fifth driving part and the sixth driving part, respectively.
The first joint Z1 is formed by the first driving part and the first connecting rod, the second joint Z2 is formed by the second driving part and the second connecting rod, the third joint Z3 is formed by the third driving part and the third connecting rod, the fourth joint Z4 is formed by the fourth driving part and the fourth connecting rod, the sixth joint Z5 is formed by the fifth driving part and the fifth connecting rod, the sixth joint Z6 is formed by the sixth driving part and the sixth connecting rod, and the movement amount of each connecting rod forms the travel of each corresponding joint.
With continued reference to fig. 1 and 2, the first joint Z1, the second joint Z2, the third joint Z3, the fifth joint Z5, and the sixth joint Z6 are all straight translational joints, and the fourth joint Z4 is a rotational joint. Specifically, the first driving part drives the first connecting rod to move along a first direction, the second driving part and the third driving part respectively drive the second connecting rod and the third connecting rod to move along a second direction perpendicular to the first direction, the fourth driving part drives the fourth connecting rod to rotate around a first axis perpendicular to the first direction and the second direction, and the fifth driving part and the sixth driving part respectively drive the fifth connecting rod and the sixth connecting rod to move along a third direction parallel to the first axis. The first direction is perpendicular to the ground, and the second direction is parallel to the ground.
More specifically, the fifth joint Z5 and the sixth joint Z6 are disposed in parallel along the first direction and the sixth joint Z6 is located above the fifth joint Z5, with a distance L therebetween.
Referring to fig. 3, the end of the fifth connecting rod is hinged to the end 21 of the mechanical arm, and the hinge point is a; the sixth connecting rod is hinged with the tail end 21 of the mechanical arm through a connecting piece, and the hinge point is B. In the initial position, the distance between the tail ends of the fifth connecting rod and the sixth connecting rod is m, the length of the connecting piece is H 1, the distance from the hinge point A to the hinge point B is H 2, the connecting line of the hinge point A and the hinge point B is vertical to the ground, and at the moment, the tail end 21 of the mechanical arm is in a horizontal state, namely the tail end 21 of the mechanical arm is parallel to the ground; when the fifth joint Z5 and the sixth joint Z6 move synchronously, that is, the movement amounts of the fifth link and the sixth link are the same, the mechanical arm end 21 is always in a horizontal state and performs translational movement; when the fifth joint Z5 and the sixth joint Z6 move asynchronously, that is, the movement amounts of the fifth link and the sixth link are different, the arm tip 21 is tilted downward or upward. Specifically, in the direction approaching the arm tip 21, when the movement amount of the sixth link is larger than the movement amount of the fifth link, the arm tip 21 is depressed downward; when the movement amount of the sixth link is smaller than the movement amount of the fifth link in the direction away from the arm end 21, the arm end 21 is tilted upward.
The surgical robot realizes 5 degrees of freedom of movement through a simple structure, and the tail end 21 of the mechanical arm realizes the functions of moving and pitching through the cooperation of the fifth joint Z5 and the sixth joint Z6, so that the surgical robot has the advantages of simple and compact whole structure, low manufacturing cost and small surgical required space.
In order to achieve accurate surgery and ensure that a focus is within the effective working space of the surgical robot of the present invention, the present invention also provides an effective working space calculation method of the surgical robot, as shown in fig. 4 and 5, comprising the steps of:
(1) Calculating the transformation relation between the image coordinate system and the robot base coordinate system T;
Solving the transformation relation of the tail end 21 of the mechanical arm relative to a robot base coordinate system T by the posture parameters of each joint of the mechanical arm 2, and marking the transformation relation as C base_tool; the transformation relation of the tail end 21 of the mechanical arm relative to the tail end tracer 22 arranged on the tail end 21 can be obtained through a three-dimensional measuring instrument and is marked as C tool_et; the robot base coordinate system T takes the initial position of the first joint Z1 as an origin, and an X axis, a Y axis and a Z axis of the robot base coordinate system T are respectively parallel to the rotation axis of the fourth joint Z4, the movement direction of the second joint Z2 and the movement direction of the first joint Z1;
The end tracer 22 arranged on the tail end 21 of the mechanical arm can be identified by the optical navigation equipment, so that the transformation relation of the end tracer 22 relative to the coordinate system of the optical navigation equipment can be established and is marked as C ots_et; the registration tracer on the affected part of the patient can be identified by the optical navigation equipment, and the transformation relation of the registration tracer relative to the coordinate system of the optical navigation equipment can be established as C ots_regist; the registered tracer can be identified in the CT image, so that a transformation relation between an image coordinate system and the registered tracer can be established and marked as C regist_img;
from this, the transformation relation C base_img between the image coordinate system and the robot base coordinate system T can be calculated:
Cbase_img＝Cbase_tool*Ctool_et*(Cots_et)inv*Cots_regist*Cregist_img
Wherein inv is matrix inversion operation;
The transformation relation C base_img of the image coordinate system relative to the robot base coordinate system T can be established through the transformation of the coordinate system;
(2) The transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system;
(21) Defining each joint coordinate system, wherein the first joint coordinate system T 1 is obtained by taking the robot base coordinate system T as a reference and taking the moving distance of the first joint as a transformation relation; the second joint coordinate system T 2 is obtained by taking the first joint coordinate system T 1 as a reference and taking the moving distance of the second joint as a transformation relation; the third joint coordinate system T 3 is obtained by taking the second joint coordinate system T 2 as a reference and taking the moving distance of the third joint as a transformation relation; the fourth joint coordinate system T 4 is obtained by transforming with the third joint coordinate system T 3 as a reference and the rotation angle of the fourth joint as a transformation relation; the fifth joint coordinate system T 5 and the sixth joint coordinate system T 6 are obtained by taking the fourth joint coordinate system T 4 as a reference and taking the moving distance of the fifth joint and the sixth joint as a transformation relation;
Specifically, a robot base coordinate system is defined as follows:
Then:
Wherein T 7 is a mechanical arm end coordinate system, θ is a pitch angle of the mechanical arm end, α is a turning angle of the mechanical arm end, dz 1 is a stroke of a first joint, and dy 2 is a stroke of a second joint; y 2 is the zero difference in the y direction of the coordinate system origin of the second joint with respect to the coordinate system origin of the first joint, z 2 is the zero difference in the z direction of the coordinate system origin of the second joint with respect to the coordinate system origin of the first joint, x 5 is the zero difference in the x direction of the coordinate system origin of the fifth joint with respect to the coordinate system origin of the fourth joint, dx 5 is the travel of the fifth joint, z 5 is the zero difference in the z direction of the coordinate system origin of the fifth joint with respect to the coordinate system origin of the fourth joint, x 6 is the zero difference in the x direction of the coordinate system origin of the sixth joint with respect to the coordinate system origin of the fourth joint, dx 6 is the travel of the sixth joint, z 6 is the zero difference in the z direction of the coordinate system origin of the sixth joint with respect to the coordinate system origin of the fourth joint, x 7 is the zero difference in the x direction of the coordinate system origin of the seventh joint with respect to the coordinate system origin of the sixth joint.
The sixth joint Z6 moves to a direction far away from the mechanical arm end 21 by a distance longer than the fifth joint Z5 moves to a direction far away from the mechanical arm end 21, so that the mechanical arm end 21 can be rotated upwards, namely the mechanical arm end 21 is in a upward state; the movement distance of the sixth joint Z6 in the direction close to the mechanical arm end 21 is greater than the movement distance of the fifth joint Z5 in the direction close to the mechanical arm end 21, so that the mechanical arm end 21 can rotate downwards, namely the mechanical arm end 21 is in a depression state;
Calculating to obtain the kinematic parameters of each joint of the mechanical arm according to the design parameters of the mechanical arm and the transformation relation between the coordinate system of each joint and the standard system of the robot base, wherein fig. 5 is the kinematic parameters of each joint of the mechanical arm;
(3) Solving the motion range of the mechanical arm;
(31) Solving the target pose of the tail end of the mechanical arm:
According to the transformation relation C base_img between the image coordinate system and the robot base coordinate system solved in the step (1), transforming a planned channel planned in the image to the robot base coordinate system, calculating to obtain the position of a starting point p t1 and an end point p t2 of the planned channel under the robot base coordinate system T, defining a target point p t of the tail end 21 of the mechanical arm as a point which takes the starting point p t1 as a starting point and extends 150mm in the direction from the end point p t2 to the starting point p t1, calculating to obtain the target pose of the tail end 21 of the mechanical arm according to the position, and solving to obtain a direction vector cd (x, y, z) when the tail end 21 of the mechanical arm is in the target pose, namely the direction vector of the planned channel under the robot base coordinate system;
(32) Solving a pitch angle and a turnover angle corresponding to the tail end of the mechanical arm in the target pose:
According to the invention, the mechanical arm tail end 21 has only two rotation angles, namely a pitch angle generated by asynchronous movement of a fifth joint Z5 and a sixth joint Z6 and a turnover angle rotated around the rotation axis of a fourth joint Z4, the rotation axes of the pitch angle and the turnover angle are mutually perpendicular, and the turnover angle alpha and the pitch angle theta of the mechanical arm tail end in working are solved by combining with a direction vector cd (x, y, Z) of a planning channel under a robot base; specifically, according to the planning channel, the target pose of the tail end of the mechanical arm can be calculated, and the target pose of the tail end of the mechanical arm is located on the extension line of the planning channel, so that the direction vector of the target pose of the tail end of the mechanical arm is consistent with the direction vector of the planning channel.
Θ=90-arccos (cd.x), where arccos is an inverse cosine function, cd.x represents the scalar in the x-axis direction of the direction vector cd (x, y, z) of the planned path under the robot base;
Let dx be the unit vector (1, 0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) of the planned channel under the robot base mark and the vector dx; u.y is a scalar quantity of u in the y-axis direction, u.z is a scalar quantity of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α= -arccos (u.y), and if u.z is 0 or more, the flip angle α= arccos (u.y).
(33) Solving the maximum limit movement position of the tail end of the mechanical arm:
Knowing the pitch angle theta, the horizontal distance difference of the fifth joint Z5 and the sixth joint Z6 at the initial position is m, the vertical distance difference is L, the distance between the hinge points AB is H 2, the length of the connecting piece is H 1, after the fifth joint and the sixth joint asynchronously move, the distance between the tail end of the fifth joint Z5 and the tail end of the sixth joint Z6 is s, and accordingly, the movement difference d between the fifth joint Z5 and the sixth joint Z6 is obtained; wherein the calculation is divided into the following two cases according to the pitch limit position of the arm tip 21:
i) The hinge point of the fifth joint Z5 and the mechanical arm tail end 21 is the point A, the hinge point of the sixth joint Z6 and the mechanical arm tail end 21 moves from the point B to the point B ', the mechanical arm tail end 21 rotates upwards, namely the mechanical arm tail end 21 is in a tilting state, as shown in fig. 3, and the pitch angle theta is the included angle between AB and AB'; at this time, the acute angle formed by the horizontal direction and the line connecting the end of the moved sixth joint Z6 and the end of the fifth joint Z5 is θ 1:
Simplifying to obtain:
From this, it is possible to calculate the joint parameter i min {0, α,0, d1} at the minimum limit position of the arm end 21 and the joint parameter i max at the maximum limit position when the arm end 21 is in the upward state. If the maximum motion of the sixth joint Z6 minus the maximum motion of the fifth joint Z5 is equal to or less than d 1,imax, the maximum motion of the first joint Z1, the maximum motion of the second joint Z2, the maximum motion of the third joint Z3, α, the maximum motion of the fifth joint Z5, the maximum motion of the fifth joint Z5+d 1), whereas i max is the (the maximum motion of the first joint Z1, the maximum motion of the second joint Z2, the maximum motion of the third joint Z3, α, the maximum motion of the sixth joint Z6-d 1, the maximum motion of the sixth joint Z6), α is the flip angle. Combining the transformation relation between the joint coordinate systems and the robot base coordinate system in the step (2) to obtain the minimum limit position and the maximum limit position of the tail end 21 of the mechanical arm in the upward state;
ii) the sixth joint Z6 moves in a direction approaching the arm end 21, so as to realize the downward rotation of the arm end 21, that is, the arm end 21 is in a depression state, at this time, the hinge point between the fifth joint Z5 and the arm end 21 is point a, the hinge point between the sixth joint Z6 and the arm end 21 moves from point B to point B ", as shown in fig. 4, at this time, the acute angle formed by the connecting line between the end of the moved sixth joint Z6 and the end of the fifth joint Z5 and the vertical direction is θ 2, and then:
Simplifying to obtain:
d2＝m-L×tanθ2
accordingly, when the arm end 21 is in the prone state, the joint parameter j min{0,0,0,α,d2, 0 of the minimum limit position of the arm end 21 and the joint parameter j max of the maximum limit position are obtained, and if the maximum motion stroke of the fifth joint Z5 minus the maximum motion stroke of the sixth joint Z6 is equal to or greater than d 2,jmax, the joint parameter is (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, α, the maximum motion stroke of the sixth joint Z6+d 2, the maximum motion stroke of the sixth joint Z6), whereas the joint parameter j max is (the maximum motion stroke of the first joint Z1, the maximum motion stroke of the second joint Z2, the maximum motion stroke of the third joint Z3, α, the maximum motion stroke of the fifth joint Z5, the maximum motion stroke of the fifth joint Z5—d 2), α is the flip angle. Combining the transformation relation between the joint coordinate systems and the robot base coordinate system in the step (2) to obtain the minimum limit position and the maximum limit position of the tail end 21 of the mechanical arm in the depression state;
The space covered by the tail end of the mechanical arm moving from the maximum limit position in the depression state to the maximum limit position in the elevation state is the maximum movement range of the mechanical arm, namely the effective working space.
After the lesion tunnel is planned by the image, if the lesion is not in the effective working space of the mechanical arm, at this time, the CT bed or the patient may be moved so that the lesion moves into the effective working space of the mechanical arm.
Correspondingly, the invention also provides a control method based on the effective working space calculation method of the mechanical arm, which comprises the following steps:
Calculating a center point p c of the effective working space of the mechanical arm by adopting the effective working space calculating method of the mechanical arm;
The doctor plans the channel in the image, transforms the planning channel under the robot base coordinate system according to the transformation relation C base_img between the image coordinate system and the robot base coordinate system T, calculates the position of the starting point p t1 and the end point p t2 of the planning channel under the robot base coordinate system T according to the transformation relation C base_img, defines the target point as p t, and takes the starting point p t1 as a starting point and extends 150mm along the direction from the end point p t2 to the starting point p t1;
The calculated difference value between the target point p t and the central point p c of the effective working space of the mechanical arm under the robot base coordinate system is the distance required to move by the CT bed;
and the doctor moves the CT bed to the proper position according to the calculated distance required to move by the CT bed, and the robot executes the puncturing operation.
The preferred embodiments of the present invention have been described in detail above, but the present invention is not limited to the specific details of the above embodiments, and various equivalent changes (such as number, shape, position, etc.) may be made to the technical solution of the present invention within the scope of the technical concept of the present invention, and these equivalent changes all fall within the scope of the present invention.
Claims (5)
1. The surgical robot comprises a base, a mechanical arm and a mechanical arm tail end, wherein the mechanical arm comprises a first driving part connected with the base, a first connecting rod driven by the first driving part to move relative to the base, a second driving part connected with the first connecting rod, a second connecting rod driven by the second driving part, a third driving part connected with the second connecting rod, a third connecting rod driven by the third driving part, a fourth driving part connected with the third connecting rod, a fourth connecting rod driven by the fourth driving part, a fifth driving part and a sixth driving part connected with the fourth connecting rod, and a fifth connecting rod and a sixth connecting rod respectively connected with the fifth driving part and the sixth driving part; the method is characterized in that: the first driving part and the first connecting rod form a first joint Z1, the second driving part and the second connecting rod form a second joint Z2, the third driving part and the third connecting rod form a third joint Z3, the fourth driving part and the fourth connecting rod form a fourth joint Z4, the fifth driving part and the fifth connecting rod form a fifth joint Z5, the sixth driving part and the sixth connecting rod form a sixth joint Z6, and the motion quantity of each connecting rod forms the stroke of each corresponding joint;
In the mechanical arm, a first joint Z1, a second joint Z2, a third joint Z3, a fifth joint Z5 and a sixth joint Z6 are all linear translation joints, and a fourth joint Z4 is a turnover joint;
The fifth joint Z5 and the sixth joint Z6 are arranged in parallel along a first direction, the sixth joint Z6 is positioned above the fifth joint Z5, the first direction is perpendicular to the ground, the tail end of the mechanical arm realizes pitching through the two joints Z5 and Z6 which are arranged in parallel, overturning is realized through the joint Z4, and the pitching axis is perpendicular to the overturning axis;
Solving the transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system;
Obtaining a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly obtaining a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly obtaining a pitch angle and a turnover angle of the tail end of the mechanical arm;
the maximum motion range of the tail end of the mechanical arm is calculated specifically as follows:
Knowing the pitch angle θ, the distance between the ends of the known joints Z5, Z6 in the initial position is m, the vertical distance is L; after the joints Z5 and Z6 move asynchronously, the distance between the two tail ends is s, H 1 is the length of the connecting piece, and H 2 is the distance from the hinge point A to the hinge point B;
When the tail end of the mechanical arm is in the upward state, at this time, the acute angle formed by the connecting line of the tail ends of the joints Z5 and Z6 and the moving direction of the two joints is θ 1, and the moving difference d 1 of the joints Z5 and Z6 is obtained as follows:
；
；
When the tail end of the mechanical arm is in a depression state, at this time, an acute angle formed by a connecting line of the tail ends of the joints Z5 and Z6 and the moving direction of the two joints is θ 2, and a moving difference d 2 of the joints Z5 and Z6 is obtained as follows:
d2=m-L×tanθ2
；
The joint parameter corresponding to the minimum limit position of the tail end of the mechanical arm is i min{0,0,0,α,d1, 0 when the tail end of the mechanical arm is in the upward state, the joint parameter corresponding to the maximum limit position is i max, if the maximum motion stroke of the joint Z6 minus the maximum motion stroke of the joint Z5 is less than or equal to d 1,imax, the joint parameter is { the maximum motion stroke of the joint Z1, the maximum motion stroke of the joint Z2, the maximum motion stroke of the joint Z3, alpha, the maximum motion stroke of the joint Z5, the maximum motion stroke of the joint Z5+d 1 }, otherwise, i max is { the maximum motion stroke of the joint Z1, the maximum motion stroke of the joint Z2, the maximum motion stroke of the joint Z3, alpha, the maximum motion stroke-d 1 of the joint Z6, the maximum motion stroke of the joint Z6 }, and alpha is the turning angle;
When the tail end of the mechanical arm is in a depression state, joint parameters j min{0,0,0,α,d2, 0 corresponding to the minimum limit position of the tail end of the mechanical arm and joint parameters corresponding to the maximum limit position are j max, if the maximum motion stroke of the joint Z5 minus the maximum motion stroke of the joint Z6 is greater than or equal to d 2,jmax, the maximum motion stroke of the joint Z1, the maximum motion stroke of the joint Z2, the maximum motion stroke of the joint Z3, alpha, the maximum motion stroke of the joint Z6+d 2, the maximum motion stroke of the joint Z6 }, otherwise j max is { the maximum motion stroke of the joint Z1, the maximum motion stroke of the joint Z2, the maximum motion stroke of the joint Z3, alpha, the maximum motion stroke of the joint Z5, the maximum motion stroke-d 2 }, and alpha is the flip angle;
and combining the transformation relation between the joint coordinate systems of the mechanical arm and the robot base coordinate system to obtain the minimum limit position and the maximum limit position of the tail end of the mechanical arm in the upward state, and combining the transformation relation between the joint coordinate systems of the mechanical arm and the robot base coordinate system to obtain the minimum limit position and the maximum limit position of the tail end of the mechanical arm in the downward state, wherein the space covered by the tail end of the mechanical arm moving from the maximum limit position in the downward state to the maximum limit position in the upward state is the maximum movement range of the mechanical arm, namely the effective working space.
2. The method for calculating the effective working space of the mechanical arm according to claim 1, wherein: the pitch angle and the roll angle are calculated as follows:
The direction vector of the planning channel under the robot base coordinate system is cd (x, y, z), the pitch angle theta=90-arccos (cd.x) is calculated, wherein arccos is an inverse cosine function, and cd.x represents the scalar of the direction vector cd (x, y, z) of the planning channel under the robot base in the x-axis direction;
Let dx be the unit vector (1, 0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) of the planned channel under the robot base mark and the vector dx; u.y is a scalar quantity of u in the y-axis direction, u.z is a scalar quantity of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α= -arccos (u.y), and if u.z is 0 or more, the flip angle α= arccos (u.y).
3. The method for calculating the effective working space of the mechanical arm according to claim 2, wherein: the sixth joint is hinged with the tail end of the mechanical arm through a connecting piece, the hinge point is B, the hinge point of the fifth joint and the tail end of the mechanical arm is A, and when the fifth joint and the sixth joint are positioned at the initial positions, the connecting line between the hinge point A and the hinge point B is perpendicular to the moving direction of the fifth joint and the sixth joint.
4. The method for calculating the effective working space of the mechanical arm according to claim 1, wherein: the transformation relation between the coordinate system of each joint of the mechanical arm and the robot base coordinate system is calculated as follows:
defining a robot base coordinate system as follows:
；
Then:
；
；
；
；
；
；
；
Wherein dz 1 is the travel of the first joint and dy 2 is the travel of the second joint; y 2 is the zero difference in the y direction of the coordinate system origin of the second joint relative to the coordinate system origin of the first joint, z 2 is the zero difference in the z direction of the coordinate system origin of the second joint relative to the coordinate system origin of the first joint, x 5 is the zero difference in the x direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, dx 5 is the travel of the fifth joint, z 5 is the zero difference in the z direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, x 6 is the zero difference in the x direction of the coordinate system origin of the sixth joint relative to the coordinate system origin of the fourth joint, dx 6 is the travel of the sixth joint, z 6 is the zero difference in the z direction of the coordinate system origin of the sixth joint relative to the coordinate system origin of the fourth joint, x 7 is the zero difference in the x direction of the coordinate system origin of the seventh joint relative to the coordinate system origin of the sixth joint, and z 7 is the zero difference in the z direction of the coordinate system origin of the seventh joint relative to the coordinate system origin of the sixth joint; t 1 is a first joint coordinate system, T 2 is a second joint coordinate system, T 3 is a third joint coordinate system, T 4 is a fourth joint coordinate system, T 5 is a fifth joint coordinate system, T 6 is a sixth joint coordinate system, T 7 is a mechanical arm end coordinate system, θ is a pitch angle of a mechanical arm end, and α is a flip angle of the mechanical arm end.
5. The method for calculating the effective working space of the mechanical arm according to claim 1, wherein: the transformation relation between the image coordinate system and the robot base coordinate system is specifically:
Solving according to the posture parameters of each joint of the mechanical arm to obtain a transformation relation of the tail end of the mechanical arm relative to a robot base coordinate system, and marking the transformation relation as C base_tool;
obtaining a transformation relation between the tail end tracer and the tail end of the mechanical arm according to the installation parameters of the tail end tracer installed on the mechanical arm, and marking the transformation relation as C tool_et;
The conversion relation between the end tracer and the registered tracer of the affected part of the patient and the optical navigation equipment is obtained according to the identification of the optical navigation equipment and is respectively marked as C ots_et and C ots_regist;
Obtaining a transformation relation between an image coordinate system and the registration tracer according to the pose of the registration tracer in the CT image, and marking the transformation relation as C regist_img;
Calculating to obtain a transformation relation C base_img between the image coordinate system and the robot base coordinate system:
；
Wherein inv is matrix inversion operation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210136042.2A CN114391958B (en) | 2022-02-15 | 2022-02-15 | Effective working space calculation method of mechanical arm and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210136042.2A CN114391958B (en) | 2022-02-15 | 2022-02-15 | Effective working space calculation method of mechanical arm and control method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114391958A CN114391958A (en) | 2022-04-26 |
| CN114391958B true CN114391958B (en) | 2024-06-07 |
Family
ID=81234336
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210136042.2A Active CN114391958B (en) | 2022-02-15 | 2022-02-15 | Effective working space calculation method of mechanical arm and control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114391958B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114872053B (en) * | 2022-06-21 | 2023-10-13 | 实时侠智能控制技术有限公司 | Planning method, device and storage medium for effective working space of end tool |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4680519A (en) * | 1985-09-23 | 1987-07-14 | General Electric Co. | Recursive methods for world-to-joint transformation for a robot manipulator |
| CN1985773A (en) * | 2005-12-22 | 2007-06-27 | 天津市华志计算机应用技术有限公司 | Celebral operating robot system based on optical tracking and closed-loop control and its realizing method |
| CN107738254A (en) * | 2017-08-25 | 2018-02-27 | 中国科学院光电研究院 | The conversion scaling method and system of a kind of mechanical arm coordinate system |
| CN110802578A (en) * | 2019-11-13 | 2020-02-18 | 吉林大学 | Maximum safe operation range detection method for upper limb of wearable robot |
| WO2020262281A1 (en) * | 2019-06-25 | 2020-12-30 | ソニー株式会社 | Surgical microscope system, control device, and control method |
| CN113288429A (en) * | 2021-05-07 | 2021-08-24 | 山东商业职业技术学院 | Space registration and real-time navigation method of breast minimally invasive interventional operation robot |
| WO2021184655A1 (en) * | 2020-03-19 | 2021-09-23 | 南京溧航仿生产业研究院有限公司 | Method for planning motion along trajectory of end of hyper-redundant mechanical arm |
| CN113580141A (en) * | 2021-08-18 | 2021-11-02 | 南京佗道医疗科技有限公司 | Pose solving method for 6-axis mechanical arm |
-
2022
- 2022-02-15 CN CN202210136042.2A patent/CN114391958B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4680519A (en) * | 1985-09-23 | 1987-07-14 | General Electric Co. | Recursive methods for world-to-joint transformation for a robot manipulator |
| CN1985773A (en) * | 2005-12-22 | 2007-06-27 | 天津市华志计算机应用技术有限公司 | Celebral operating robot system based on optical tracking and closed-loop control and its realizing method |
| CN107738254A (en) * | 2017-08-25 | 2018-02-27 | 中国科学院光电研究院 | The conversion scaling method and system of a kind of mechanical arm coordinate system |
| WO2020262281A1 (en) * | 2019-06-25 | 2020-12-30 | ソニー株式会社 | Surgical microscope system, control device, and control method |
| CN110802578A (en) * | 2019-11-13 | 2020-02-18 | 吉林大学 | Maximum safe operation range detection method for upper limb of wearable robot |
| WO2021184655A1 (en) * | 2020-03-19 | 2021-09-23 | 南京溧航仿生产业研究院有限公司 | Method for planning motion along trajectory of end of hyper-redundant mechanical arm |
| CN113288429A (en) * | 2021-05-07 | 2021-08-24 | 山东商业职业技术学院 | Space registration and real-time navigation method of breast minimally invasive interventional operation robot |
| CN113580141A (en) * | 2021-08-18 | 2021-11-02 | 南京佗道医疗科技有限公司 | Pose solving method for 6-axis mechanical arm |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114391958A (en) | 2022-04-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111437011B (en) | Puncture surgery robot system | |
| CN114041875B (en) | Integrated operation positioning navigation system | |
| CN110051436B (en) | Automated cooperative work assembly and application thereof in surgical instrument | |
| CN101980838B (en) | Surgery assistance system for guiding a surgical instrument | |
| US20130190774A1 (en) | Mechanical positioning system for surgical instruments | |
| KR20200078422A (en) | System and method for master/tool matching and control for intuitive movement | |
| CN114391958B (en) | Effective working space calculation method of mechanical arm and control method thereof | |
| WO2022218388A1 (en) | Method and apparatus for performing positioning by means of x-ray image, and x-ray machine and readable storage medium | |
| CN113180828A (en) | Operation robot constrained motion control method based on rotation theory | |
| CN114027980A (en) | Interventional operation robot system and calibration and error compensation method thereof | |
| CN112754616A (en) | Ultrasonic positioning puncture system and storage medium | |
| CN116616873A (en) | Preoperative positioning method, preoperative positioning system and readable storage medium for surgical robot | |
| CN113081277A (en) | Deployable instrument arm | |
| CN112315513A (en) | Novel breast tumor puncture biopsy device | |
| CN115811819A (en) | Intelligent shadowless lamp system of bracelet direction | |
| CN116035705A (en) | Integral kinematic space positioning method for surgical robot | |
| CN114767274A (en) | Three-dimensional positioning micro surgical robot | |
| WO2022100480A1 (en) | Control method for location and orientation of surgical robot end, and control method for surgical robot | |
| CN113334381B (en) | Control method of movable decoupling continuum robot | |
| CN114521967A (en) | Mechanical arm and medical trolley | |
| US20220401168A1 (en) | Slave Device and Control Method Therefor, and Eye Surgery Device and Control Method Therefor | |
| CN111888010A (en) | Mammary tissue fixing device based on ultrasonic guidance and control method thereof | |
| CN113742992A (en) | Master-slave control method based on deep learning and application | |
| CN115089304B (en) | Multi-arm multi-degree-of-freedom minimally invasive surgery platform | |
| CN114452004B (en) | Control method for tail end position and posture of surgical robot |
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 | ||
| CB02 | Change of applicant information |
Address after: 210000 building 3, No. 34, Dazhou Road, Yuhuatai District, Nanjing, Jiangsu Province Applicant after: Tuodao Medical Technology Co.,Ltd. Address before: 210000 building 3, No. 34, Dazhou Road, Yuhuatai District, Nanjing, Jiangsu Province Applicant before: Nanjing Tuodao Medical Technology Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant |