CN114391958A - Method for calculating effective working space of mechanical arm and control method thereof - Google Patents
Method for calculating effective working space of mechanical arm and control method thereof Download PDFInfo
- Publication number
- CN114391958A CN114391958A CN202210136042.2A CN202210136042A CN114391958A CN 114391958 A CN114391958 A CN 114391958A CN 202210136042 A CN202210136042 A CN 202210136042A CN 114391958 A CN114391958 A CN 114391958A
- Authority
- CN
- China
- Prior art keywords
- joint
- coordinate system
- mechanical arm
- tail end
- robot
- 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
Images
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- 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
Abstract
The invention discloses a method for calculating an effective working space of a mechanical arm and a control method thereof, wherein the tail end of the mechanical arm realizes pitching through two moving joints arranged in parallel, overturning is realized through an overturning joint, and a pitching axis is vertical to an overturning axis; solving the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot; acquiring a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly acquiring a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly acquiring a pitch angle and a roll-over angle of the tail end of the mechanical arm; and calculating to obtain the maximum motion range of the tail end of the mechanical arm by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot and the design parameters of the mechanical arm. The method for calculating the effective working space of the mechanical arm is simple to calculate, the distance to be moved of the CT bed can be calculated based on the pose of the 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 in humans, 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 surgery, the existing surgical robot has a complex structure and a complex control procedure, so the manufacturing cost of the surgical robot is high and the space requirement on an operating room is high.
In order to reduce the production and manufacturing costs and the requirements for the space of the operating room, the surgical robot is improved in the direction of miniaturization and simplification, the effective working space of the improved surgical robot is reduced, and in order to realize precise surgery, a method for clearly obtaining the effective working space of the surgical robot and the control flow corresponding to the effective working space before actual surgery needs to be provided.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the problems, the invention provides a method for calculating the effective working space of the mechanical arm of the surgical robot and a control method thereof, wherein the method has the advantages of simple structure and convenience in operation and control.
The technical scheme is as follows:
the method for calculating the effective working space of the mechanical arm comprises the following steps that the tail end of the mechanical arm is pitched through two moving joints arranged in parallel, the end of the mechanical arm is turned over through a turning joint, and the pitching axis and the turning axis are perpendicular to each other;
solving the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot;
acquiring a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly acquiring a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly acquiring a pitch angle and a roll-over angle of the tail end of the mechanical arm;
and calculating to obtain the maximum motion range of the tail end of the mechanical arm by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot and the design parameters 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), and the pitch angle theta is calculated to be 90-arccos (cd.x), wherein arccos is an inverse cosine function, and cd.x represents the scalar quantity 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,0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) and vector dx of the planned channel under the robot base index; u.y denotes a scalar of u in the y-axis direction, u.z denotes a scalar of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α is — arccos (u.y), and if u.z is 0 or larger, the flip angle α is arccos (u.y).
The two parallel moving joints are respectively 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, a 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 located at initial positions, a connecting line between the hinge point A and the hinge point B is vertical to the moving directions of the fifth joint and the sixth joint.
The maximum range of motion of the end of the robotic arm is calculated as follows:
knowing the pitch angle theta, knowing that the distance between the tail ends of the two parallel joints is m and the vertical distance is L when the two parallel joints are at the initial positions, and after the two parallel joints move asynchronously, the distance between the tail ends is s, H1Is the length of the connecting member, H2The distance from the hinge point A to the hinge point B;
when the tail end of the mechanical arm is in a pitching state, an acute angle formed by a connecting line of the tail ends of the two parallel joints and the moving directions of the two joints is theta1Then the movement difference d of two parallel joints is obtained1Comprises the following steps:
calculating to obtain the minimum limit position and the maximum limit position of the tail end of the mechanical arm when the tail end of the mechanical arm is in a pitching state by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot;
when the tail end of the mechanical arm is in a bent state, an acute angle formed by a connecting line of the tail ends of the two parallel joints and the moving directions of the two joints is theta2Then the movement difference d of two parallel joints is obtained2Comprises the following steps:
d2＝m-L×tanθ2
and calculating to obtain 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 movement of the tail end of the mechanical arm from the maximum limit position of the bending state to the maximum limit position of the bending 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 both the first direction and the moving direction of the fifth joint;
when the tail end of the mechanical arm is in the pitch-up state, the joint parameter corresponding to the minimum limit position of the tail end of the mechanical arm is imin{0，0，0，α，d 10 and the maximum limit position is imaxIf 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 d1，imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the fifth joint Z5, maximum motion stroke of the fifth joint Z5 + d)1) Otherwise imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the sixth joint Z6-d)1The maximum movement stroke of the sixth joint Z6), and alpha is a flip angle;
the most extreme of arm end when arm end is in state of bowingJoint parameter j corresponding to small limit positionmin{0，0，0，α，d20} and the maximum limit position is jmaxIf 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 d2，jmaxThen (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)2Sixth joint Z6 maximum motion stroke), otherwise jmaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the fifth joint Z5, maximum motion stroke-d of the fifth joint Z5)2) And alpha is a flip angle.
The transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot is calculated as follows:
defining the base coordinate of the robot as follows:
then:
wherein dz is1Is the stroke of the first joint, dy2Is the stroke of the second joint; y is2Zero difference in y-direction of origin of coordinate system of second joint relative to origin of coordinate system of first joint, z2Zero difference value, x, in the z direction of the origin of the coordinate system of the second joint relative to the origin of the coordinate system of the first joint5Is the zero difference value of the coordinate system origin of the fifth joint relative to the x direction of the coordinate system origin of the fourth joint, dx5Is the stroke of the fifth joint, z5Zero difference value in z direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, x6Is the zero difference value of the origin of the coordinate system of the sixth joint relative to the origin of the coordinate system of the fourth joint in the x direction, dx6Is the stroke of the sixth joint, z6Zero difference value in z direction of origin of coordinate system of sixth joint relative to origin of coordinate system of fourth joint, x7Zero difference in x-direction of origin of coordinate system of the seventh joint with respect to origin of coordinate system of the sixth joint, z7Zero difference value of the origin of the coordinate system of the seventh joint relative to the origin of the coordinate system of the sixth joint in the z direction; t is1Is a first joint coordinate system, T2Is a second joint coordinate system, T3As a third joint coordinate system, T4Is a fourth joint coordinate system, T5Is a fifth joint coordinate system, T6Is a sixth joint coordinate system, T7Is a coordinate system at the tail end of the mechanical arm, theta is a pitch angle at the tail end of the mechanical arm, and alpha is a turnover angle at the tail end of the mechanical arm.
The method for obtaining the transformation relation between the image coordinate system and the robot base coordinate system specifically comprises the following steps:
according to each joint of the mechanical armThe attitude parameters are solved to obtain the transformation relation of the tail end of the mechanical arm relative to the robot base coordinate system, and the transformation relation is recorded as Cbase_tool；
Obtaining the 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 recording as Ctool_et；
The transformation relation between the registered tracer and the optical navigation equipment of the tail end tracer and the affected part of the patient is obtained according to the identification of the optical navigation equipment and is respectively marked as Cots_etAnd Cots_regist；
Obtaining the transformation relation between the image coordinate system and the registration tracer according to the pose of the registration tracer in the CT image, and recording as Cregist_img；
Calculating to obtain a transformation relation C between the image coordinate system and the robot base coordinate systembase_img：
Cbase_img＝Cbase_tool*Ctool_et*(Cots_et)inv*Cots_regist*Cregist_img
Wherein inv is a matrix inversion operation.
A method of controlling a surgical robot, comprising:
calculating to obtain a central point of the effective working space of the mechanical arm according to the effective working space calculation method of the mechanical arm;
calculating the positions of a starting point and an end point of a planned 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 using the starting point as a starting point to extend a set length to the direction from the end point to the starting point as a target point at the tail end of the mechanical arm;
calculating the difference value of the target point and the central point in the x, y and z axis directions of the robot base coordinate system to obtain the distance of the CT bed required to move;
and the doctor moves the CT bed according to the calculated distance, and executes a planning result after the CT bed is moved in place.
The set length is 150 mm.
Has the advantages that: the invention provides a corresponding method for calculating the effective working space of a mechanical arm, the distance to be moved of a CT bed is calculated by combining the pose of a planning channel based on the calculation result, and a doctor only needs to move the CT bed according to the calculation result, so that the operation is simple and convenient.
Drawings
FIG. 1 is a schematic structural view of a surgical robot of the present invention;
FIG. 2 is a partial side view of a surgical robotic arm of the present invention;
FIG. 3 is a schematic view of the upward rotation of the end of the robotic arm of the present invention;
FIG. 4 is a schematic view of the downward rotation of the end of the robotic arm of the present invention;
fig. 5 is a diagram showing kinematic parameters of a robot arm joint.
Wherein, 1 is a base, 2 is a mechanical arm, 21 is the tail end of the mechanical arm, 22 is a tail end tracer, 23 is an execution instrument, and 3 is a puncture needle.
Detailed Description
The invention is further elucidated with reference to the drawings and the embodiments.
Fig. 1 is a schematic structural diagram of the surgical robot of the present invention, and as shown in fig. 2, the surgical robot of the present invention includes a base 1, a robot arm 2, and a robot arm end 21. The tail end 21 of the mechanical arm is provided with a tail end tracer 22 and an executing instrument 23, and the executing instrument 23 is provided with an executing channel for penetrating the puncture needle 3. The robot arm 2 includes a first driving portion connected to the base 1, a first link driven by the first driving portion to move relative to the base 1, a second driving portion connected to the first link, a second link driven by the second driving portion, a third driving portion connected to the second link, a third link driven by the third driving portion, a fourth driving portion connected to the third link, a fourth link driven by the fourth driving portion, a fifth driving portion and a sixth driving portion connected to the fourth link, and a fifth link and a sixth link connected to the fifth driving portion and the sixth driving portion, respectively.
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 sixth joint Z5, the sixth driving part and the sixth connecting rod form a sixth joint Z6, and the motion amount of each connecting rod forms the stroke corresponding to each 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 linear translation joints, and the fourth joint Z4 is a rotation joint. Specifically, the first driving portion drives the first connecting rod to move along a first direction, the second driving portion and the third driving portion 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 portion drives the fourth connecting rod to rotate around a first axis perpendicular to the first direction and the second direction, and the fifth driving portion and the sixth driving portion 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 vertical to the ground, and the second direction is parallel to the ground.
More specifically, the fifth joint Z5 and the sixth joint Z6 are arranged in parallel in the first direction and the sixth joint Z6 is located above the fifth joint Z5 with a distance L therebetween.
As shown in fig. 3, the end of the fifth link 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 hinged point is B. In the initial position, the distance between the ends of the fifth and sixth links is m, and the length of the connecting member is H1The distance from the hinge point A to the hinge point B is H2The 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 tail end 21 is always in a horizontal state and performs translational motion; 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 robot arm tip 21 tilts down or tilts up. Specifically, in the direction approaching the robot arm tip 21, when the movement amount of the sixth link is larger than the movement amount of the fifth link, the robot arm tip 21 is pitched downward; at the edge far from the mechanical armIn the direction of the tip 21, when the moving amount of the sixth link is smaller than the moving amount of the fifth link, the robot arm tip 21 is tilted upward.
The surgical robot realizes 5-degree-of-freedom 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 integral structure, low manufacturing cost and small operation required space.
In order to realize precise operation and ensure that the focus is in the effective working space of the surgical robot, the invention also provides a method for calculating the effective working space of the surgical robot, as shown in fig. 4 and 5, which comprises the following steps:
(1) calculating the transformation relation between the image coordinate system and the robot base coordinate system T;
the transformation relation of the tail end 21 of the mechanical arm relative to the base coordinate system T of the robot is solved according to the posture parameters of each joint of the mechanical arm 2 and is marked as Cbase_tool(ii) a The transformation relationship of the mechanical arm end 21 relative to the end tracer 22 mounted thereon can be obtained by a three-dimensional measuring instrument and is marked as Ctool_et(ii) a The robot base coordinate system T takes the initial position of the first joint Z1 as an origin, and the X axis, the Y axis and the Z axis of the robot base coordinate system T are respectively parallel to the rotation axis of the fourth joint Z4, the motion direction of the second joint Z2 and the motion direction of the first joint Z1;
the end tracer 22 mounted on the 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 Cots_et(ii) a The registration tracer at 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 and recorded as Cots_regist(ii) a The registered tracer can be identified in the CT image, so that the transformation relation between the image coordinate system and the registered tracer can be established and is marked as Cregist_img；
Therefore, the transformation relation C between the image coordinate system and the robot base coordinate system T can be calculatedbase_img：
Cbase_img＝Cbase_tool*Ctool_et*(Cots_et)inv*Cots_regist*Cregist_img
Wherein inv is a matrix inversion operation;
through the above coordinate system transformation, the transformation relation C of the image coordinate system relative to the robot base coordinate system T can be establishedbase_img；
(2) The transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot;
(21) defining each joint coordinate system, wherein the first joint coordinate system T1The robot base coordinate system T is used as a reference, and the moving distance of the first joint is used as a transformation relation for transformation to obtain the first joint; second joint coordinate system T2Is a first joint coordinate system T1Taking the moving distance of the second joint as a reference, and obtaining the movement distance through transformation by taking the moving distance of the second joint as a transformation relation; third joint coordinate system T3Is a second joint coordinate system T2Taking the moving distance of the third joint as a reference, and obtaining the movement distance through transformation by taking the moving distance of the third joint as a transformation relation; fourth joint coordinate system T4Is a third joint coordinate system T3Taking the rotation angle of the fourth joint as a reference, and obtaining the rotation angle through transformation by taking the rotation angle of the fourth joint as a transformation relation; fifth joint coordinate system T5And a sixth joint coordinate system T6Are all in a fourth joint coordinate system T4Taking the moving distance of the fifth joint and the moving distance of the sixth joint as a reference, and obtaining the movement distance through transformation by taking the moving distances of the fifth joint and the sixth joint as transformation relations;
specifically, the robot base coordinate is defined as:
then:
wherein, T7Is a coordinate system of the tail end of the mechanical arm, theta is a pitch angle of the tail end of the mechanical arm, alpha is a flip angle of the tail end of the mechanical arm, and dz1Is the stroke of the first joint, dy2Is the stroke of the second joint; y is2Zero difference in y-direction of origin of coordinate system of second joint relative to origin of coordinate system of first joint, z2Zero difference value, x, in the z direction of the origin of the coordinate system of the second joint relative to the origin of the coordinate system of the first joint5Is the zero difference value of the coordinate system origin of the fifth joint relative to the x direction of the coordinate system origin of the fourth joint, dx5Is the stroke of the fifth joint, z5Zero difference value in z direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, x6Is the zero difference value of the origin of the coordinate system of the sixth joint relative to the origin of the coordinate system of the fourth joint in the x direction, dx6Is the stroke of the sixth joint, z6Zero difference value in z direction of origin of coordinate system of sixth joint relative to origin of coordinate system of fourth joint, x7Zero difference in x-direction of origin of coordinate system of the seventh joint with respect to origin of coordinate system of the sixth joint, z7Is the zero difference in the z-direction of the origin of the coordinate system of the seventh joint relative to the origin of the coordinate system of the sixth joint.
The moving distance of the sixth joint Z6 in the direction away from the tail end 21 of the mechanical arm is greater than the moving distance of the fifth joint Z5 in the direction away from the tail end 21 of the mechanical arm, so that the tail end 21 of the mechanical arm can rotate upwards, that is, the tail end 21 of the mechanical arm is in a pitching state; the moving distance of the sixth joint Z6 in the direction close to the end 21 of the mechanical arm is greater than the moving distance of the fifth joint Z5 in the direction close to the end 21 of the mechanical arm, so that the downward rotation of the end 21 of the mechanical arm can be realized, that is, the end 21 of the mechanical arm is in a depression state;
calculating 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 robot base standard system, wherein the kinematic parameters of each joint of the mechanical arm are shown in FIG. 5;
(3) solving the motion range of the mechanical arm;
(31) solving the target pose of the tail end of the mechanical arm:
the transformation relation C between the image coordinate system and the robot base coordinate system is solved according to the step (1)base_imgTransforming the planned channel planned in the image to the robot base coordinate system, and calculating the starting point p of the planned channelt1And end point pt2Defining a target point p for the end 21 of the robot arm at a position under the robot base coordinate system TtTo take a starting point pt1As a starting point and along an end point pt2To the starting point pt1The point of 150mm is extended in the direction of the target pose of the tail end 21 of the robot mechanical arm is obtained through calculation, and a direction vector cd (x, y, z) when the tail end 21 of the mechanical arm is in the target pose is obtained through solving, namely the direction vector of the planning channel under the robot base coordinate system;
(32) solving a pitch angle and a roll-over angle corresponding to the tail end of the mechanical arm in a target pose:
the tail end 21 of the mechanical arm only has two-direction rotation angles, namely a pitch angle generated by asynchronous movement of a fifth joint Z5 and a sixth joint Z6 and a turnover angle rotating around the rotation axis of a fourth joint Z4, the rotation axes of the pitch angle and the turnover angle are mutually vertical, and the turnover angle alpha and the pitch angle theta of the tail end of the mechanical arm in working are solved by combining direction vectors cd (x, y, Z) of a planning channel under a robot base mark; specifically, the target pose of the tail end of the mechanical arm can be obtained through calculation according to the planning channel, 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.
θ is 90 ° -arccos (cd.x), where arccos is an inverse cosine function, and cd.x represents a scalar quantity of a direction vector cd (x, y, z) of the planned path under the robot base index in the x-axis direction;
let dx be the unit vector (1,0,0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) and vector dx of the planned channel under the robot base index; u.y denotes a scalar of u in the y-axis direction, u.z denotes a scalar of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α is — arccos (u.y), and if u.z is 0 or larger, the flip angle α is arccos (u.y).
(33) Solving the maximum limit movement position of the tail end of the mechanical arm:
knowing the pitch angle θ, the difference between the horizontal distance m and the vertical distance L between the fifth joint Z5 and the sixth joint Z6 in the initial position, and the distance H between the hinge points AB2The length of the connecting piece is H1After the fifth joint and the sixth joint move asynchronously, the distance between the tail end of the fifth joint Z5 and the tail end of the sixth joint Z6 is s, and the movement difference d of the fifth joint Z5 and the sixth joint Z6 is obtained according to the distance; the calculation is divided into the following two cases according to the pitch limit position of the robot arm tip 21:
i) the hinge point of the fifth joint Z5 and the tail end 21 of the mechanical arm is point a, the hinge point of the sixth joint Z6 and the tail end 21 of the mechanical arm moves from point B to point B ', the tail end 21 of the mechanical arm rotates upwards, that is, the tail end 21 of the mechanical arm is in a pitching state, as shown in fig. 3, the pitch angle θ is the included angle between AB and AB'; at this time, an acute angle θ formed by a horizontal direction and a line connecting the end of the sixth joint Z6 and the end of the fifth joint Z5 after the movement is set to1And then:
simplifying to obtain:
therefore, the joint parameter i of the minimum limit position of the tail end 21 of the mechanical arm when the tail end 21 of the mechanical arm is in the pitching state can be calculatedmin{0, 0,0, α,0, d1} and the joint parameter i in the maximum limit positionmax. 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 d1，imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the fifth joint Z5, maximum motion stroke of the fifth joint Z5 + d)1) Otherwise imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the sixth joint Z6-d)1The maximum movement stroke of the sixth joint Z6), and α is the flip angle. The minimum limit position and the maximum limit position of the tail end 21 of the mechanical arm in the pitching state can be obtained by combining the transformation relation between the coordinate systems of all joints in the step (2) and the base coordinate system of the robot;
ii) the sixth joint Z6 moves towards the end 21 of the robot arm to rotate the end 21 of the robot arm downwards, i.e. the end 21 of the robot arm is in a bent state, at this time, the hinge point of the fifth joint Z5 and the end 21 of the robot arm is point a, the hinge point of the sixth joint Z6 and the end 21 of the robot arm moves from point B to point B ", as shown in fig. 4, at this time,an acute angle θ formed by a line connecting the end of the sixth joint Z6 and the end of the fifth joint Z5 after the movement and the vertical direction2And then:
simplifying to obtain:
d2＝m-L×tanθ2
accordingly, a joint parameter j of the minimum limit position of the robot arm end 21 when the robot arm end 21 is in the depression state is obtainedmin{0，0，0，α，d 20 and maximum limit position of the joint parameter jmaxIf 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 d2，jmaxThen (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)2Sixth joint Z6 maximum motion stroke), otherwise jmaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the fifth joint Z5, maximum motion stroke-d of the fifth joint Z5)2) And alpha is a flip angle. The minimum limit position and the maximum limit position of the mechanical arm tail end 21 in the depression state can be obtained by combining the transformation relation between the coordinate systems of all joints in the step (2) and the robot base coordinate system;
the space covered by the movement of the tail end of the mechanical arm from the maximum limit position of the bending state to the maximum limit position of the bending state is the maximum movement range of the mechanical arm, namely the effective working space.
After the lesion channel is planned by the image, if the lesion is not in the effective working space of the mechanical arm, the CT bed or the patient can be moved, so that the lesion is moved to the effective working space of the mechanical arm.
Correspondingly, the invention also provides a control method based on the mechanical arm effective working space calculation method, which comprises the following steps:
calculating the central point p of the effective working space of the mechanical arm by adopting the method for calculating the effective working space of the mechanical armc；
Planning a channel by a doctor in the image and according to a transformation relation C between an image coordinate system and a robot base coordinate system Tbase_imgTransforming the planned channel to the robot base coordinate system, and calculating to obtain the starting point p of the planned channelt1And end point pt2Defining the target point as p at the position under the robot base coordinate system TtWhich is based on a starting point pt1As a starting point and along an end point pt2To the starting point pt1A point elongated by 150mm in the direction of (a);
calculated target point ptThe central point p of the effective working space of the mechanical armcThe difference value under the robot base coordinate system is the moving distance of the CT bed;
and (4) moving the CT bed to the position by the doctor according to the calculated moving distance of the CT bed, and executing the puncture operation by the robot.
Although the preferred embodiments of the present invention have been described in detail, the present invention is not limited to the details of the foregoing embodiments, and various equivalent changes (such as number, shape, position, etc.) may be made to the technical solution of the present invention within the technical spirit of the present invention, and these equivalent changes are all within the protection scope of the present invention.
Claims (10)
1. A method for calculating the effective working space of a mechanical arm is characterized by comprising the following steps: the tail end of the mechanical arm realizes pitching through two moving joints arranged in parallel, overturning is realized through an overturning joint, 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 base coordinate system of the robot;
acquiring a transformation relation between an image coordinate system and a robot base coordinate system, and accordingly acquiring a direction vector of a planning channel in the image coordinate system under the robot base coordinate system, and accordingly acquiring a pitch angle and a roll-over angle of the tail end of the mechanical arm;
and calculating to obtain the maximum motion range of the tail end of the mechanical arm by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot and the design parameters of the mechanical arm.
2. The robot arm effective workspace calculation method of 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), and the pitch angle theta is calculated to be 90-arccos (cd.x), wherein arccos is an inverse cosine function, and cd.x represents the scalar quantity 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,0) in the x-axis direction, u be the normalized vector of the cross product of the direction vector cd (x, y, z) and vector dx of the planned channel under the robot base index; u.y denotes a scalar of u in the y-axis direction, u.z denotes a scalar of u in the z-axis direction, and if u.z is smaller than 0, the flip angle α is — arccos (u.y), and if u.z is 0 or larger, the flip angle α is arccos (u.y).
3. The robot arm effective workspace calculation method of claim 2, wherein: the two parallel moving joints are respectively 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, a 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 located at initial positions, a connecting line between the hinge point A and the hinge point B is vertical to the moving directions of the fifth joint and the sixth joint.
4. The robot arm effective workspace calculation method of claim 3, wherein: the maximum range of motion of the end of the robotic arm is calculated as follows:
knowing the pitch angle theta, knowing that the distance between the tail ends of the two parallel joints is m and the vertical distance is L when the two parallel joints are at the initial positions, and after the two parallel joints move asynchronously, the distance between the tail ends is s, H1Is the length of the connecting member, H2The distance from the hinge point A to the hinge point B;
when the tail end of the mechanical arm is in a pitching state, an acute angle formed by a connecting line of the tail ends of the two parallel joints and the moving directions of the two joints is theta1Then the movement difference d of two parallel joints is obtained1Comprises the following steps:
calculating to obtain the minimum limit position and the maximum limit position of the tail end of the mechanical arm when the tail end of the mechanical arm is in a pitching state by combining the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot;
when the tail end of the mechanical arm is in a bent state, an acute angle formed by a connecting line of the tail ends of the two parallel joints and the moving directions of the two joints is theta2Then the movement difference d of two parallel joints is obtained2Comprises the following steps:
d2＝m-L×tanθ2
and calculating to obtain 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.
5. The robot arm effective workspace calculation method of claim 4, wherein: the space covered by the movement of the tail end of the mechanical arm from the maximum limit position of the bending state to the maximum limit position of the bending state is the maximum movement range of the mechanical arm.
6. The robot arm effective workspace calculation method of claim 4, wherein: 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 both the first direction and the moving direction of the fifth joint;
when the tail end of the mechanical arm is in the pitch-up state, the joint parameter corresponding to the minimum limit position of the tail end of the mechanical arm is imin{0，0，0，α，d10 and the maximum limit position is imaxIf 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 d1，imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the fifth joint Z5, maximum motion stroke of the fifth joint Z5 + d)1) Otherwise imaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2, maximum motion stroke of the third joint Z3, alpha, maximum motion stroke of the sixth joint Z6-d)1The maximum movement stroke of the sixth joint Z6), and alpha is a flip angle;
joint parameter j corresponding to minimum limit position of mechanical arm tail end when mechanical arm tail end is in depression statemin{0，0，0，α，d20} and the maximum limit position is jmaxIf 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 d2，jmaxThen (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)2Sixth joint Z6 maximum motion stroke), otherwise jmaxThen (maximum motion stroke of the first joint Z1, maximum motion stroke of the second joint Z2 and maximum motion stroke of the third joint Z3)Maximum motion travel, α, maximum motion travel for fifth joint Z5, maximum motion travel for fifth joint Z5-d2) And alpha is a flip angle.
7. The robot arm effective workspace calculation method of claim 6, wherein: the transformation relation between the coordinate system of each joint of the mechanical arm and the base coordinate system of the robot is calculated as follows:
defining the base coordinate of the robot as follows:
then:
wherein dz is1Is the stroke of the first joint, dy2Is the stroke of the second joint; y is2Zero difference in y-direction of origin of coordinate system of second joint relative to origin of coordinate system of first joint, z2Zero difference value, x, in the z direction of the origin of the coordinate system of the second joint relative to the origin of the coordinate system of the first joint5Is the zero difference value of the coordinate system origin of the fifth joint relative to the x direction of the coordinate system origin of the fourth joint, dx5Is the stroke of the fifth joint, z5Zero difference value in z direction of the coordinate system origin of the fifth joint relative to the coordinate system origin of the fourth joint, x6Is the zero difference value of the origin of the coordinate system of the sixth joint relative to the origin of the coordinate system of the fourth joint in the x direction, dx6Is the stroke of the sixth joint, z6Zero difference value in z direction of origin of coordinate system of sixth joint relative to origin of coordinate system of fourth joint, x7Zero difference in x-direction of origin of coordinate system of the seventh joint with respect to origin of coordinate system of the sixth joint, z7Zero difference value of the origin of the coordinate system of the seventh joint relative to the origin of the coordinate system of the sixth joint in the z direction; t is1Is a first joint coordinate system, T2Is a second joint coordinate system, T3As a third joint coordinate system, T4Is a fourth joint coordinate system, T5Is a fifth joint coordinate system, T6Is a sixth joint coordinate system, T7Is a coordinate system at the tail end of the mechanical arm, theta is a pitch angle at the tail end of the mechanical arm, and alpha is a turnover angle at the tail end of the mechanical arm.
8. The robot arm effective workspace calculation method of claim 1, wherein: the method for obtaining the transformation relation between the image coordinate system and the robot base coordinate system specifically comprises the following steps:
solving according to the attitude parameters of each joint of the mechanical arm to obtain the transformation relation of the tail end of the mechanical arm relative to the robot base coordinate system, which is marked as Cbase_tool；
According to the arrangement of the mechanical armThe transformation relation between the tail end tracer and the tail end of the mechanical arm is obtained by the mounting parameters of the mounted tail end tracer and is recorded as Ctool_et；
The transformation relation between the registered tracer and the optical navigation equipment of the tail end tracer and the affected part of the patient is obtained according to the identification of the optical navigation equipment and is respectively marked as Cots_etAnd Cots_regist；
Obtaining the transformation relation between the image coordinate system and the registration tracer according to the pose of the registration tracer in the CT image, and recording as Cregist_img；
Calculating to obtain a transformation relation C between the image coordinate system and the robot base coordinate systembase_img：
Cbase_img＝Cbase_tool*Ctool_et*(Cots_et)inv*Cots_regist*Cregist_img
Wherein inv is a matrix inversion operation.
9. A control method of a surgical robot, characterized in that: the method comprises the following steps:
calculating a central point of an effective working space of a mechanical arm according to the effective working space calculation method of the mechanical arm of any one of claims 1 to 8;
calculating the positions of a starting point and an end point of a planned 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 using the starting point as a starting point to extend a set length to the direction from the end point to the starting point as a target point at the tail end of the mechanical arm;
calculating the difference value of the target point and the central point in the x, y and z axis directions of the robot base coordinate system to obtain the distance of the CT bed required to move;
and the doctor moves the CT bed according to the calculated distance, and executes a planning result after the CT bed is moved in place.
10. The control method of a surgical robot according to claim 9, characterized in that: the set length is 150 mm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210136042.2A CN114391958A (en) | 2022-02-15 | 2022-02-15 | Method for calculating effective working space of mechanical arm and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210136042.2A CN114391958A (en) | 2022-02-15 | 2022-02-15 | Method for calculating effective working space of mechanical arm and control method thereof |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114391958A true CN114391958A (en) | 2022-04-26 |
Family
ID=81234336
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210136042.2A Pending CN114391958A (en) | 2022-02-15 | 2022-02-15 | Method for calculating effective working space of mechanical arm and control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114391958A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114872053A (en) * | 2022-06-21 | 2022-08-09 | 实时侠智能控制技术有限公司 | Planning method and device for effective working space of end tool and storage medium |
Citations (5)
| 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 |
| CN110802578A (en) * | 2019-11-13 | 2020-02-18 | 吉林大学 | Maximum safe operation range detection method for upper limb of wearable robot |
| 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/CN114391958A/en active Pending
Patent Citations (5)
| 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 |
| 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 |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114872053A (en) * | 2022-06-21 | 2022-08-09 | 实时侠智能控制技术有限公司 | Planning method and device for effective working space of end tool and storage medium |
| CN114872053B (en) * | 2022-06-21 | 2023-10-13 | 实时侠智能控制技术有限公司 | Planning method, device and storage medium for effective working space of end tool |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| 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 | |
| CN112971993A (en) | Surgical robot system for positioning operation and control method thereof | |
| CN113229938A (en) | Surgical robot for positioning operation | |
| CN113133832B (en) | Calibration method and system for double-arm robot puncture system | |
| US20050256371A1 (en) | Method and system for using a variable direction of view endoscope with a robotic endoscope holder | |
| CN114343847B (en) | Hand-eye calibration method of surgical robot based on optical positioning system | |
| WO2022141153A1 (en) | Ultrasonic positioning puncture system and storage medium | |
| CN112754616B (en) | Ultrasonic positioning puncture system and storage medium | |
| CN114391958A (en) | Method for calculating effective working space of mechanical arm and control method thereof | |
| Li et al. | A skull-mounted robot with a compact and lightweight parallel mechanism for positioning in minimally invasive neurosurgery | |
| CN113180828A (en) | Operation robot constrained motion control method based on rotation theory | |
| CN109009438A (en) | Flexible noninvasive positioning device and its operation pathway is planned in art application and system | |
| Vandini et al. | Vision-based motion control of a flexible robot for surgical applications | |
| CN113974835A (en) | Operation robot motion control method based on telecentric motionless point constraint | |
| CN113081277A (en) | Deployable instrument arm | |
| WO2023046074A1 (en) | Autonomous learning method for mechanical arm control method | |
| CN116616873A (en) | Preoperative positioning method, preoperative positioning system and readable storage medium for surgical robot | |
| CN116035705A (en) | Integral kinematic space positioning method for surgical robot | |
| CN114767274A (en) | Three-dimensional positioning micro surgical robot | |
| CN113742992B (en) | Master-slave control method based on deep learning and application | |
| Wang et al. | Development of a novel 4-DOF flexible endoscopic robot using cable-driven multisegment continuum mechanisms | |
| US20220401168A1 (en) | Slave Device and Control Method Therefor, and Eye Surgery Device and Control Method Therefor | |
| CN113580141B (en) | Pose solving method for 6-axis mechanical arm |
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 |