CN116889471A - Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm - Google Patents
Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm Download PDFInfo
- Publication number
- CN116889471A CN116889471A CN202310857108.1A CN202310857108A CN116889471A CN 116889471 A CN116889471 A CN 116889471A CN 202310857108 A CN202310857108 A CN 202310857108A CN 116889471 A CN116889471 A CN 116889471A
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- joint angle
- pose
- calculating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000000007 visual effect Effects 0.000 claims abstract description 59
- 239000003550 marker Substances 0.000 claims abstract description 56
- 238000011156 evaluation Methods 0.000 claims abstract description 29
- 238000012163 sequencing technique Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 15
- 230000003902 lesion Effects 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 9
- 230000001174 ascending effect Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000008929 regeneration Effects 0.000 claims description 3
- 238000011069 regeneration method Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000009471 action Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000001356 surgical procedure Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000001575 pathological effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer 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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
- G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
-
- 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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2046—Tracking techniques
- A61B2034/2055—Optical tracking systems
-
- 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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2068—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis using pointers, e.g. pointers having reference marks for determining coordinates of body points
-
- 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/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2072—Reference field transducer attached to an instrument or patient
-
- 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
- A61B2034/305—Details of wrist mechanisms at distal ends of robotic arms
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30204—Marker
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Surgery (AREA)
- Life Sciences & Earth Sciences (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Robotics (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Geometry (AREA)
- Manipulator (AREA)
Abstract
The application provides a method, a device, equipment and a computer readable storage medium for selecting and solving the optimal joint angle of a navigation operation mechanical arm. The method for selecting and solving the optimal joint angle of the navigation operation mechanical arm comprises the following steps: rotating the tool around a straight line for one circle to generate a Cartesian pose set at the tail end of the mechanical arm; calculating the pose of the visual marker under the sensor under the pose of each mechanical arm; judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition; generating a mechanical arm Cartesian pose set meeting the recognition constraint conditions; solving the inverse kinematics joint angle of the mechanical arm for each Cartesian pose; based on a preset evaluation index, sequencing each group of joint angles, and endowing corresponding evaluation scores according to the ranks; and (5) sorting in descending order according to the total score, and determining the optimal joint angle with the highest score. According to the embodiment of the application, the optimal joint angle can be determined more accurately.
Description
Technical Field
The application belongs to the field of visible light navigation surgery robot kinematics, and particularly relates to a method, a device and equipment for selecting and solving an optimal joint angle of a navigation surgery mechanical arm and a computer readable storage medium.
Background
Currently, in the related art, by acquiring target joint values of each joint of the arm and each joint of the trolley arm of the surgical trolley, the current posture of the vision-guided surgical robot is combined, the optimal configuration of the arm is determined, and the movement of each joint of the arm from the current joint value to the optimal configuration is controlled, but the constraint of the end tool space and the constraint of the vision sensor are ignored, so that the determined optimal joint angle is inaccurate.
Therefore, how to determine the optimal joint angle more accurately is a technical problem that needs to be solved by those skilled in the art.
Disclosure of Invention
The embodiment of the application provides a method, a device, equipment and a computer readable storage medium for selecting and solving the optimal joint angle of a navigation operation mechanical arm, which can more accurately determine the optimal joint angle.
In a first aspect, an embodiment of the present application provides a method for selecting an optimal joint angle of a navigation surgical mechanical arm, including:
rotating the tool around a straight line for one circle to generate a Cartesian pose set at the tail end of the mechanical arm;
calculating the pose of the visual marker under the sensor under the pose of each mechanical arm;
judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition;
generating a mechanical arm Cartesian pose set meeting the recognition constraint conditions;
solving the inverse kinematics joint angle of the mechanical arm for each Cartesian pose;
based on a preset evaluation index, sequencing each group of joint angles, and endowing corresponding evaluation scores according to the ranks;
and (5) sorting in descending order according to the total score, and determining the optimal joint angle with the highest score.
Optionally, sorting the joint angles of each group based on a preset evaluation index, and assigning a corresponding evaluation score according to the ranking, including:
calculating singular values of each set of joint angles;
calculating the mobility variance between each group of joint angles and the current joint angle;
calculating the shortest distance between the focus visual marker and the mechanical arm connecting rod and between the focus visual marker and the end tool;
and sorting the joint angles of each group according to a singular value descending order, a mobility variance descending order, a shortest distance ascending order of the tail end track and the focus area and a visual field angle descending order, and endowing corresponding evaluation scores according to the ranks.
Optionally, calculating singular values for each set of joint angles includes:
acquiring a terminal Cartesian space velocity vector and a joint angular velocity vector;
based on the terminal Cartesian space velocity vector and the joint angular velocity vector, calculating a mechanical arm Jacobian matrix of the current joint angle;
carrying out SVD (singular value decomposition) on the mechanical arm Jacobian matrix of the current joint angle to obtain a phase characteristic value matrix;
based on the period eigenvalue matrix, respectively determining the maximum eigenvalue and the minimum eigenvalue of the Jacobi matrix;
and determining the singular value of the joint angle by the ratio of the maximum characteristic value to the minimum characteristic value.
Optionally, calculating the mobility variance of each set of joint angles from the current joint angle includes:
acquiring the joint angle of the current joint group, the joint angle of the target joint group and the total joint number;
and calculating the mobility variance of each group of joint angles and the current joint angle based on the joint angle of the current joint group, the joint angle of the target joint group and the total joint number.
Optionally, calculating the shortest distance between the lesion visual marker and the robotic arm linkage and end tool comprises:
calculating a first distance between the lesion visual marker and the end tool;
calculating a second distance between the focus visual marker and the mechanical arm connecting rod;
and determining the minimum value between the first distance and the second distance as the shortest distance between the focus visual marker and the mechanical arm connecting rod and the end tool.
Optionally, calculating a first distance between the lesion visual marker and the end tool comprises:
determining the sphere center and the radius of the joint sphere;
determining the sphere center and the radius of the minimum externally connected sphere of the focus visual marker;
the first distance between the lesion visual marker and the end tool is calculated based on the center and radius of the spherical body of the joint and the center and radius of the smallest outer sphere of the lesion visual marker.
Optionally, determining the center and radius of the joint sphere includes:
acquiring the length and the deflection angle of a connecting rod of an ith joint;
based on the length and deflection angle of the connecting rod of the ith joint, calculating a conversion matrix of the ith-1 th joint under the ith joint coordinate system;
determining a transformation matrix of the ith joint coordinate system under the base based on the transformation matrix of the ith-1 joint under the ith joint coordinate system;
determining the coordinate of the center of the ith joint relative to the base based on a transformation matrix of the coordinate system of the ith joint under the base;
and converting the coordinate system into a mechanical arm base coordinate system by using a visual sensor, and determining the sphere center and the radius of the joint sphere.
In a second aspect, an embodiment of the present application provides a device for selecting and resolving an optimal joint angle of a navigation surgical mechanical arm, where the device includes:
the pose set generating module is used for rotating around the straight line where the tool is positioned for one circle to generate a Cartesian pose set at the tail end of the mechanical arm;
the pose calculating module is used for calculating the pose of the visual marker under the sensor under the pose of each mechanical arm;
the constraint condition judgment module is used for judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition;
the pose set regeneration module is used for generating a mechanical arm Cartesian pose set meeting the recognition constraint condition;
the joint angle calculation module is used for calculating the inverse kinematics joint angle of the mechanical arm for each Cartesian pose;
the evaluation score determining module is used for sequencing each group of joint angles based on a preset evaluation index and giving corresponding evaluation scores according to the ranks;
and the optimal joint angle determining module is used for carrying out descending order sorting according to the total scores and determining the optimal joint angle with the highest score.
In a third aspect, an embodiment of the present application provides an electronic device, including: a processor and a memory storing computer program instructions;
the processor executes the computer program instructions to implement the method for selecting and resolving the optimal joint angle of the navigation surgical mechanical arm as shown in the first aspect.
In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, where computer program instructions are stored, where the computer program instructions, when executed by a processor, implement a method for selecting an optimal joint angle of a navigation surgical mechanical arm as shown in the first aspect.
The embodiment of the application provides a method, a device, equipment and a computer readable storage medium for selecting and solving the optimal joint angle of a navigation operation mechanical arm, which can more accurately determine the optimal joint angle.
The method for selecting and solving the optimal joint angle of the navigation operation mechanical arm comprises the following steps: rotating the tool around a straight line for one circle to generate a Cartesian pose set at the tail end of the mechanical arm; calculating the pose of the visual marker under the sensor under the pose of each mechanical arm; judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition; generating a mechanical arm Cartesian pose set meeting the recognition constraint conditions; solving the inverse kinematics joint angle of the mechanical arm for each Cartesian pose; based on a preset evaluation index, sequencing each group of joint angles, and endowing corresponding evaluation scores according to the ranks; and (5) sorting in descending order according to the total score, and determining the optimal joint angle with the highest score.
Drawings
In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings that are needed in the description of the embodiments or the prior art will be briefly described, and it is obvious that the drawings in the description below are some embodiments of the present application, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a method for selecting an optimal joint angle for a navigational surgical manipulator according to one embodiment of the present application;
FIG. 2 is a flowchart of a method for selecting an optimal joint angle for a navigational surgical manipulator according to one embodiment of the present application;
FIG. 3 is a schematic view of a collision between joint enclosure balls provided in accordance with one embodiment of the present application;
FIG. 4 is a schematic view of a collision between a joint enclosing ball and a connecting rod enclosing cylinder according to one embodiment of the present application;
FIG. 5 is a schematic structural view of a device for selecting an optimal joint angle of a mechanical arm for navigation surgery according to an embodiment of the present application;
fig. 6 is a schematic structural diagram of an electronic device according to an embodiment of the present application.
Detailed Description
Features and exemplary embodiments of various aspects of the present application will be described in detail below, and in order to make the objects, technical solutions and advantages of the present application more apparent, the present application will be described in further detail below with reference to the accompanying drawings and the detailed embodiments. It should be understood that the particular embodiments described herein are meant to be illustrative of the application only and not limiting. It will be apparent to one skilled in the art that the present application may be practiced without some of these specific details. The following description of the embodiments is merely intended to provide a better understanding of the application by showing examples of the application.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises the element.
Currently, in the related art, by acquiring target joint values of each joint of the arm and each joint of the trolley arm of the surgical trolley, the current posture of the vision-guided surgical robot is combined, the optimal configuration of the arm is determined, and the movement of each joint of the arm from the current joint value to the optimal configuration is controlled, but the constraint of the end tool space and the constraint of the vision sensor are ignored, so that the determined optimal joint angle is inaccurate.
In order to solve the problems in the prior art, the embodiment of the application provides a method, a device, equipment and a computer-readable storage medium for selecting and solving the optimal joint angle of a navigation operation mechanical arm. The following first describes a method for selecting and solving the optimal joint angle of the navigation operation mechanical arm provided by the embodiment of the application.
Fig. 1 is a flow chart illustrating a method for selecting and resolving an optimal joint angle of a navigation surgical mechanical arm according to an embodiment of the present application. As shown in fig. 1, the method for selecting and solving the optimal joint angle of the navigation operation mechanical arm comprises the following steps:
s101, rotating around a straight line where a tool is located for one circle to generate a Cartesian pose set of the tail end of the mechanical arm;
s102, calculating the pose of the visual marker under the sensor of each mechanical arm pose;
s103, judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition;
s104, generating a mechanical arm Cartesian pose set meeting identification constraint conditions;
s105, solving the inverse kinematics joint angle of the mechanical arm for each Cartesian pose;
s106, sorting the angles of each group of joints based on a preset evaluation index, and giving corresponding evaluation scores according to the ranks;
and S107, sorting in descending order according to the total score, and determining the optimal joint angle with the highest score.
In the positioning link of the navigation operation mechanical arm guided by visible light, when the positioning tool is a space straight line, a tool zero space exists, namely the space straight line description of the positioning tool is unchanged, and the joint angle of the mechanical arm is changed. When the joint angle sent to the mechanical arm is improper, the problems that a visual sensor loses a visual signal, a measurement error of the visual marker and the sensor with overlarge angle is increased, the mechanical arm body is positioned near a singular value and cannot move continuously, and the mechanical arm body and a tool collide with a focus visual marker during positioning are caused. The application aims to solve the problems, and provides a method for selecting and solving the optimal joint angle of the mechanical arm for the navigation operation on the basis of the problems, and the specific method is shown in figure 2.
In one embodiment, sorting each set of joint angles based on a preset evaluation index and assigning a corresponding evaluation score according to the ranking comprises:
calculating singular values of each set of joint angles;
calculating the mobility variance between each group of joint angles and the current joint angle;
calculating the shortest distance between the focus visual marker and the mechanical arm connecting rod and between the focus visual marker and the end tool;
and sorting the joint angles of each group according to a singular value descending order, a mobility variance descending order, a shortest distance ascending order of the tail end track and the focus area and a visual field angle descending order, and endowing corresponding evaluation scores according to the ranks.
In one embodiment, calculating singular values for each set of joint angles includes:
acquiring a terminal Cartesian space velocity vector and a joint angular velocity vector;
based on the terminal Cartesian space velocity vector and the joint angular velocity vector, calculating a mechanical arm Jacobian matrix of the current joint angle;
carrying out SVD (singular value decomposition) on the mechanical arm Jacobian matrix of the current joint angle to obtain a phase characteristic value matrix;
based on the period eigenvalue matrix, respectively determining the maximum eigenvalue and the minimum eigenvalue of the Jacobi matrix;
and determining the singular value of the joint angle by the ratio of the maximum characteristic value to the minimum characteristic value.
Specifically, the calculation mode of the singular value of the key value joint angle is as follows:
wherein the method comprises the steps ofFor the terminal Cartesian space velocity vector, +.>As the joint angular velocity vector, J (theta) is a mechanical arm Jacobian matrix related to the current joint angle, and the matrix is subjected to SVD (singular value decomposition) to obtain a term characteristic value matrix
J(θ)＝UΣV T (2)
Wherein sigma 1 Is the best of Jacobian matrixLarge eigenvalue, sigma n For the minimum characteristic value, the number of joints of the n mechanical arm, the singular value is calculated by adopting the ratio of the maximum characteristic value to the minimum characteristic value:the larger the value of S, the closer the flexibility of the manipulator in cartesian space is to the pathological condition.
In one embodiment, calculating the mobility variance of each set of joint angles from the current joint angle includes:
acquiring the joint angle of the current joint group, the joint angle of the target joint group and the total joint number;
and calculating the mobility variance of each group of joint angles and the current joint angle based on the joint angle of the current joint group, the joint angle of the target joint group and the total joint number.
The mechanical arm mobility variance is calculated as follows:
k is the variance of the mobility, θ 0i For the ith joint angle of the current joint group, θ i The ith joint angle is the target joint group, and n is the total joint number.
In one embodiment, calculating the shortest distance between a lesion visual marker and a robotic arm linkage and end tool comprises:
calculating a first distance between the lesion visual marker and the end tool;
calculating a second distance between the focus visual marker and the mechanical arm connecting rod;
and determining the minimum value between the first distance and the second distance as the shortest distance between the focus visual marker and the mechanical arm connecting rod and the end tool.
In one embodiment, calculating a first distance between a lesion visual marker and an end tool comprises:
determining the sphere center and the radius of the joint sphere;
determining the sphere center and the radius of the minimum externally connected sphere of the focus visual marker;
the first distance between the lesion visual marker and the end tool is calculated based on the center and radius of the spherical body of the joint and the center and radius of the smallest outer sphere of the lesion visual marker.
In one embodiment, determining the center of sphere and radius of the joint sphere includes:
acquiring the length and the deflection angle of a connecting rod of an ith joint;
based on the length and deflection angle of the connecting rod of the ith joint, calculating a conversion matrix of the ith-1 th joint under the ith joint coordinate system;
determining a transformation matrix of the ith joint coordinate system under the base based on the transformation matrix of the ith-1 joint under the ith joint coordinate system;
determining the coordinate of the center of the ith joint relative to the base based on a transformation matrix of the coordinate system of the ith joint under the base;
and converting the coordinate system into a mechanical arm base coordinate system by using a visual sensor, and determining the sphere center and the radius of the joint sphere.
Specifically, the distance between the mechanical arm connecting rod and the end tool and the focus visual marker determines whether collision can occur during mechanical arm positioning, all joints of the mechanical arm are simplified into spheres, the connecting rod is replaced by a cylinder, the tool is replaced by a cylinder, the focus visual marker is replaced by a minimum external sphere, and the probability of collision is represented by the shortest space distance between the minimum external sphere of the focus visual marker and geometric objects of the mechanical arm and the end tool. The center coordinates of all joints under the base of the mechanical arm are needed to be used in calculation, and the following formula is adopted according to the mechanical arm standard D-H kinematic modeling mode:
wherein the method comprises the steps ofSitting at the ith joint for the (i-1) th jointConversion matrix alpha under standard system i Length of the connecting rod of the ith joint, theta i Is the deflection angle of the ith joint; this gives a description of the ith joint coordinate system under the base:
thus, the coordinate O of the center of the ith joint relative to the base is obtained i Is that
The visual sensor can be used for converting the coordinate system into the mechanical arm base standard system, and the minimum circumcircle center O of the focus visual marker is set p ，r p With a sphere center O p Radius r p Can build minimum external round balls respectively by O i Establishing sphere for the sphere center and the parameter radius of the actual joint sphere to obtain the joint O i With visual markers O p Distance d of (2) i ：
d i ＝||O i O p ||-(r i +r p ) (8)
Ith connecting rod L i From the centre of sphere O i With O i+1 The connecting line is an axis and the actual dimension R i For radius construction, visual marker ball O p And connecting rod cylinder L i Distance D of (2) i The solution can be approximated by the following equation:
d for a joint angle i And d i Is the minimum value D of (2) min The probability of collision of the mechanical arm body, the end tool and the focus visual marker can be represented. The two collision modes are shown in fig. 3 and 4, and the left Bian Qiuti and right spheres in fig. 3 represent: the focus visual marker is minimum externally connected with a sphere and a mechanical arm joint sphere; in FIG. 4The sphere represents the smallest circumscribing sphere and the cylinder represents the manipulator connecting rod surrounding cylinder of the focus visual marker.
Let the set of end Cartesian pose vectors after the mechanical arm rotates around the end straight tool vector for one circle be A= { x i ,y i ,z i ,rx i ,ry i ,rz i A pose vector set meeting the sensor identification condition and the accuracy range condition is A τ ＝{x i ,y i ,z i ,rx i ,ry i ,rz i For A }, pair τ All the pose solving mechanical arm inverse kinematics in the model (1) obtain a joint angle vector set Q= { Q 0i ,q 1i ,q 2i ,……q ni For each set of joint angles in geometry Q, singular value S, variance of momentum K, collision distance D min Can obtain a construction set Q τ ＝{q 0i ,q 1i ,q 2i ,……q ni ,S i ,K i ,D mini For Q τ Per group element of S i Sorting in ascending order, scoring each group of elements according to sorting result, adding element vector, and score M S =an, n is the ranking of the ordered ranking, Q S ＝{q 0i ,q 1i ,q 2i ,……q ni ,S i ,K i ,D mini ,M Si -a }; then to Q S Each group of elements in K i Sorting in descending order, scoring each group of elements according to sorting result, adding element vector, and scoring M K =bn, n is the ordered ranking, and qk= { q 0i ,q 1i ,q 2i ,……q ni ,S i ,K i ,D mini ,M Si ,M Mi -a }; immediately after Q K Each group of elements in D mini Sorting in ascending order, scoring each group of elements according to sorting result, adding element vector, and score M D =cn, n is the ranking after ordering, Q D ＝{q 0i ,q 1i ,q 2i ,……q ni ,S i ,K i ,D mini ,M Si ,M Ki ,M Di -a }; finally, e=m according to the algebraic sum of the three scoring values S +M K +M D Can obtain Q E ＝{q 0i ,q 1i ,q 2i ,……q ni ,S i ,K i ,D mini ,M Si ,M Ki ,M Di ,E i -a }; final pair set Q E According to E i Ascending order of joint angles q in the last group of element vectors 0i ,q 1i ,q 2i ,……q ni The optimal joint angle of the mechanical arm in the positioning is obtained, wherein the scoring coefficients a, b and c meet the condition that a+b+c=1; depending on the emphasis of each localization task, three variables may be assigned different values.
The method is different from the traditional multi-axis mechanical arm track planning method aiming at single problem, and in the application, the characteristics of a visual sensor, mechanical arm movement hard constraint and zero space movement of a positioning tool are combined in the navigation operation, so that the method comprises the following steps: the optimal joint angle during positioning is selected by selecting and optimizing a vector of the target joint angle, singular values, collision coefficients, momentum variances and other parameters related to positioning of the navigation mechanical arm and evaluation scores corresponding to the parameters through multiple sequencing.
Fig. 5 is a schematic structural diagram of a device for selecting and resolving an optimal joint angle of a navigation surgical mechanical arm according to an embodiment of the present application, where the device includes:
the pose set generating module 501 is configured to rotate around a line where the tool is located for one circle, and generate a cartesian pose set of the tail end of the mechanical arm;
the pose calculating module 502 is used for calculating the pose of the visual marker under the sensor under each mechanical arm pose;
a constraint condition judging module 503, configured to judge whether the pose of the marker meets the identified geometric constraint condition and the identified accuracy constraint condition;
a pose set regeneration module 504, configured to generate a mechanical arm cartesian pose set that meets the recognition constraint condition;
the joint angle calculation module 505 is configured to calculate, for each cartesian pose, a joint angle of the inverse kinematics of the mechanical arm;
the evaluation score determining module 506 is configured to sort the joint angles of each group based on a preset evaluation index, and assign a corresponding evaluation score according to the ranking;
the optimal joint angle determining module 507 is configured to perform descending order sorting according to the total score, and determine an optimal joint angle with the highest score.
Fig. 6 shows a schematic structural diagram of an electronic device according to an embodiment of the present application.
The electronic device may include a processor 601 and a memory 602 storing computer program instructions.
In particular, the processor 601 may include a Central Processing Unit (CPU), or an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), or may be configured as one or more integrated circuits that implement embodiments of the present application.
Memory 602 may include mass storage for data or instructions. By way of example, and not limitation, memory 602 may include a Hard Disk Drive (HDD), floppy Disk Drive, flash memory, optical Disk, magneto-optical Disk, magnetic tape, or universal serial bus (Universal Serial Bus, USB) Drive, or a combination of two or more of the above. The memory 602 may include removable or non-removable (or fixed) media, where appropriate. The memory 602 may be internal or external to the electronic device, where appropriate. In particular embodiments, memory 602 may be a non-volatile solid state memory.
In one embodiment, memory 602 may be Read Only Memory (ROM). In one embodiment, the ROM may be mask-programmed ROM, programmable ROM (PROM), erasable PROM (EPROM), electrically Erasable PROM (EEPROM), electrically rewritable ROM (EAROM), or flash memory, or a combination of two or more of these.
The processor 601 reads and executes the computer program instructions stored in the memory 602 to implement any of the above-described embodiments of the method for navigating the optimal joint angle selection of the surgical manipulator.
In one example, the electronic device may also include a communication interface 603 and a bus 610. As shown in fig. 6, the processor 601, the memory 602, and the communication interface 603 are connected to each other through a bus 610 and perform communication with each other.
The communication interface 603 is mainly used for implementing communication between each module, apparatus, unit and/or device in the embodiment of the present application.
Bus 610 includes hardware, software, or both, that couple components of the electronic device to one another. By way of example, and not limitation, the buses may include an Accelerated Graphics Port (AGP) or other graphics bus, an Enhanced Industry Standard Architecture (EISA) bus, a Front Side Bus (FSB), a HyperTransport (HT) interconnect, an Industry Standard Architecture (ISA) bus, an infiniband interconnect, a Low Pin Count (LPC) bus, a memory bus, a micro channel architecture (MCa) bus, a Peripheral Component Interconnect (PCI) bus, a PCI-Express (PCI-X) bus, a Serial Advanced Technology Attachment (SATA) bus, a video electronics standards association local (VLB) bus, or other suitable bus, or a combination of two or more of the above. Bus 610 may include one or more buses, where appropriate. Although embodiments of the application have been described and illustrated with respect to a particular bus, the application contemplates any suitable bus or interconnect.
In addition, in combination with the method for selecting the optimal joint angle of the navigation operation mechanical arm in the above embodiment, the embodiment of the application can be implemented by providing a computer readable storage medium. The computer readable storage medium has stored thereon computer program instructions; the computer program instructions, when executed by the processor, implement any of the methods of navigating the optimal joint angle selection of the surgical robotic arm described in the embodiments above.
It should be understood that the application is not limited to the particular arrangements and instrumentality described above and shown in the drawings. For the sake of brevity, a detailed description of known methods is omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method processes of the present application are not limited to the specific steps described and shown, and those skilled in the art can make various changes, modifications and additions, or change the order between steps, after appreciating the spirit of the present application.
The functional blocks shown in the above-described structural block diagrams may be implemented in hardware, software, firmware, or a combination thereof. When implemented in hardware, it may be, for example, an electronic circuit, an Application Specific Integrated Circuit (ASIC), suitable firmware, a plug-in, a function card, or the like. When implemented in software, the elements of the application are the programs or code segments used to perform the required tasks. The program or code segments may be stored in a machine readable medium or transmitted over transmission media or communication links by a data signal carried in a carrier wave. A "machine-readable medium" may include any medium that can store or transfer information. Examples of machine-readable media include electronic circuitry, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, CD-ROMs, optical disks, hard disks, fiber optic media, radio Frequency (RF) links, and the like. The code segments may be downloaded via computer networks such as the internet, intranets, etc.
It should also be noted that the exemplary embodiments mentioned in this disclosure describe some methods or systems based on a series of steps or devices. However, the present application is not limited to the order of the above-described steps, that is, the steps may be performed in the order mentioned in the embodiments, or may be performed in a different order from the order in the embodiments, or several steps may be performed simultaneously.
Aspects of the present application are described above with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each block of the flowchart illustrations and/or block diagrams, and combinations of blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, enable the implementation of the functions/acts specified in the flowchart and/or block diagram block or blocks. Such a processor may be, but is not limited to being, a general purpose processor, a special purpose processor, an application specific processor, or a field programmable logic circuit. It will also be understood that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware which performs the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In the foregoing, only the specific embodiments of the present application are described, and it will be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working processes of the systems, modules and units described above may refer to the corresponding processes in the foregoing method embodiments, which are not repeated herein. It should be understood that the scope of the present application is not limited thereto, and any equivalent modifications or substitutions can be easily made by those skilled in the art within the technical scope of the present application, and they should be included in the scope of the present application.
Claims (10)
1. The method for selecting and solving the optimal joint angle of the navigation operation mechanical arm is characterized by comprising the following steps of:
rotating the tool around a straight line for one circle to generate a Cartesian pose set at the tail end of the mechanical arm;
calculating the pose of the visual marker under the sensor under the pose of each mechanical arm;
judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition;
generating a mechanical arm Cartesian pose set meeting the recognition constraint conditions;
solving the inverse kinematics joint angle of the mechanical arm for each Cartesian pose;
based on a preset evaluation index, sequencing each group of joint angles, and endowing corresponding evaluation scores according to the ranks;
and (5) sorting in descending order according to the total score, and determining the optimal joint angle with the highest score.
2. The method for selecting and solving the optimal joint angles of the navigation surgical manipulator according to claim 1, wherein the steps of sorting each group of joint angles based on a preset evaluation index and assigning corresponding evaluation scores according to the ranks comprise:
calculating singular values of each set of joint angles;
calculating the mobility variance between each group of joint angles and the current joint angle;
calculating the shortest distance between the focus visual marker and the mechanical arm connecting rod and between the focus visual marker and the end tool;
and sorting the joint angles of each group according to a singular value descending order, a mobility variance descending order, a shortest distance ascending order of the tail end track and the focus area and a visual field angle descending order, and endowing corresponding evaluation scores according to the ranks.
3. The method of claim 2, wherein calculating the singular values for each set of joint angles comprises:
acquiring a terminal Cartesian space velocity vector and a joint angular velocity vector;
based on the terminal Cartesian space velocity vector and the joint angular velocity vector, calculating a mechanical arm Jacobian matrix of the current joint angle;
carrying out SVD (singular value decomposition) on the mechanical arm Jacobian matrix of the current joint angle to obtain a phase characteristic value matrix;
based on the period eigenvalue matrix, respectively determining the maximum eigenvalue and the minimum eigenvalue of the Jacobi matrix;
and determining the singular value of the joint angle by the ratio of the maximum characteristic value to the minimum characteristic value.
4. The method of claim 2, wherein calculating the variance of the mobility of each set of joint angles from the current joint angle comprises:
acquiring the joint angle of the current joint group, the joint angle of the target joint group and the total joint number;
and calculating the mobility variance of each group of joint angles and the current joint angle based on the joint angle of the current joint group, the joint angle of the target joint group and the total joint number.
5. The method of claim 2, wherein calculating the shortest distance between the lesion visual marker and the arm link and end tool comprises:
calculating a first distance between the lesion visual marker and the end tool;
calculating a second distance between the focus visual marker and the mechanical arm connecting rod;
and determining the minimum value between the first distance and the second distance as the shortest distance between the focus visual marker and the mechanical arm connecting rod and the end tool.
6. The method of claim 5, wherein calculating a first distance between the lesion visual marker and the end tool comprises:
determining the sphere center and the radius of the joint sphere;
determining the sphere center and the radius of the minimum externally connected sphere of the focus visual marker;
the first distance between the lesion visual marker and the end tool is calculated based on the center and radius of the spherical body of the joint and the center and radius of the smallest outer sphere of the lesion visual marker.
7. The method of claim 6, wherein determining the center and radius of the joint sphere comprises:
acquiring the length and the deflection angle of a connecting rod of an ith joint;
based on the length and deflection angle of the connecting rod of the ith joint, calculating a conversion matrix of the ith-1 th joint under the ith joint coordinate system;
determining a transformation matrix of the ith joint coordinate system under the base based on the transformation matrix of the ith-1 joint under the ith joint coordinate system;
determining the coordinate of the center of the ith joint relative to the base based on a transformation matrix of the coordinate system of the ith joint under the base;
and converting the coordinate system into a mechanical arm base coordinate system by using a visual sensor, and determining the sphere center and the radius of the joint sphere.
8. A device for selecting and separating an optimal joint angle of a navigation operation mechanical arm, which is characterized by comprising:
the pose set generating module is used for rotating around the straight line where the tool is positioned for one circle to generate a Cartesian pose set at the tail end of the mechanical arm;
the pose calculating module is used for calculating the pose of the visual marker under the sensor under the pose of each mechanical arm;
the constraint condition judgment module is used for judging whether the pose of the marker meets the recognized geometric constraint condition and the recognized precision constraint condition;
the pose set regeneration module is used for generating a mechanical arm Cartesian pose set meeting the recognition constraint condition;
the joint angle calculation module is used for calculating the inverse kinematics joint angle of the mechanical arm for each Cartesian pose;
the evaluation score determining module is used for sequencing each group of joint angles based on a preset evaluation index and giving corresponding evaluation scores according to the ranks;
and the optimal joint angle determining module is used for carrying out descending order sorting according to the total scores and determining the optimal joint angle with the highest score.
9. An electronic device, the electronic device comprising: a processor and a memory storing computer program instructions;
the processor, when executing the computer program instructions, implements the method for selecting and resolving the optimal joint angle of the navigation surgical mechanical arm according to any one of claims 1-7.
10. A computer readable storage medium, wherein computer program instructions are stored on the computer readable storage medium, and when executed by a processor, the computer program instructions implement the method for selecting an optimal joint angle for a navigational surgical manipulator according to any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310857108.1A CN116889471B (en) | 2023-07-13 | 2023-07-13 | Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310857108.1A CN116889471B (en) | 2023-07-13 | 2023-07-13 | Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116889471A true CN116889471A (en) | 2023-10-17 |
| CN116889471B CN116889471B (en) | 2024-04-02 |
Family
ID=88314498
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310857108.1A Active CN116889471B (en) | 2023-07-13 | 2023-07-13 | Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116889471B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117860382A (en) * | 2024-01-02 | 2024-04-12 | 北京长木谷医疗科技股份有限公司 | Navigation surgery mechanical arm vision servo pose prediction PD control method based on LSTM |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140277741A1 (en) * | 2013-03-15 | 2014-09-18 | Samsung Electronics Co., Ltd. | Robots and methods of controlling the same |
| CN106028995A (en) * | 2014-02-21 | 2016-10-12 | 直观外科手术操作公司 | Mechanical joints, and related systems and methods |
| US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
| CN114227685A (en) * | 2021-12-28 | 2022-03-25 | 深圳市优必选科技股份有限公司 | Mechanical arm control method and device, computer readable storage medium and mechanical arm |
| WO2023274000A1 (en) * | 2021-06-29 | 2023-01-05 | 武汉联影智融医疗科技有限公司 | Robot system, and evaluation method and control method therefor |
| CN116117813A (en) * | 2023-01-28 | 2023-05-16 | 重庆大学 | Mechanical arm control method, mechanical arm, robot and storage medium |
-
2023
- 2023-07-13 CN CN202310857108.1A patent/CN116889471B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140277741A1 (en) * | 2013-03-15 | 2014-09-18 | Samsung Electronics Co., Ltd. | Robots and methods of controlling the same |
| CN106028995A (en) * | 2014-02-21 | 2016-10-12 | 直观外科手术操作公司 | Mechanical joints, and related systems and methods |
| US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
| WO2023274000A1 (en) * | 2021-06-29 | 2023-01-05 | 武汉联影智融医疗科技有限公司 | Robot system, and evaluation method and control method therefor |
| CN114227685A (en) * | 2021-12-28 | 2022-03-25 | 深圳市优必选科技股份有限公司 | Mechanical arm control method and device, computer readable storage medium and mechanical arm |
| CN116117813A (en) * | 2023-01-28 | 2023-05-16 | 重庆大学 | Mechanical arm control method, mechanical arm, robot and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 翟红飞: "基于深度学习的机械臂抓取***研究", 信息科技辑, pages 140 - 75 * |
| 董加顺: "双连杆机械臂动力学参数辨识及线性自抗扰控制策略研究", 信息科技辑, pages 140 - 279 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117860382A (en) * | 2024-01-02 | 2024-04-12 | 北京长木谷医疗科技股份有限公司 | Navigation surgery mechanical arm vision servo pose prediction PD control method based on LSTM |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116889471B (en) | 2024-04-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116889471B (en) | Method, device and equipment for selecting and solving optimal joint angle of navigation operation mechanical arm | |
| CN106557844B (en) | Path planning method for welding robot | |
| CN110696000B (en) | Obstacle avoidance method for mechanical arm heuristic sensing | |
| US8290620B2 (en) | Route creation method and route creation device | |
| US10828777B2 (en) | Method for the redundancy-optimized planning of the operation of a mobile robot | |
| JP5365379B2 (en) | Robot system and robot system calibration method | |
| KR101633357B1 (en) | Manipulator and control method thereof | |
| WO2002023121A1 (en) | Positioning in computer aided manufacturing by measuring both parts (cameras, retro reflectors) | |
| Hu et al. | Automatic calibration of hand–eye–workspace and camera using hand-mounted line laser | |
| CN112809682B (en) | Mechanical arm obstacle avoidance path planning method and system and storage medium | |
| CN111319038B (en) | Track generation system and track generation method | |
| KR102094004B1 (en) | Method for controlling a table tennis robot and a system therefor | |
| KR101797122B1 (en) | Method for Measurement And Compensation of Error on Portable 3D Coordinate Measurement Machine | |
| CN110345936B (en) | Track data processing method and processing system of motion device | |
| CN113203419B (en) | Indoor inspection robot correction positioning method based on neural network | |
| CN111123947A (en) | Robot traveling control method and device, electronic device, medium, and robot | |
| CN110893619A (en) | Industrial robot position appearance calibrating device based on laser tracker | |
| Liu et al. | High precision calibration for three-dimensional vision-guided robot system | |
| Xie et al. | Pose error estimation using a cylinder in scanner-based robotic belt grinding | |
| CN102608969B (en) | Cascade robot hand and eye registration control method based on infrared optical alignment | |
| CN114299039B (en) | Robot and collision detection device and method thereof | |
| CN113799130A (en) | Robot position and posture calibration method in man-machine cooperation assembly | |
| CN114489050A (en) | Obstacle avoidance route control method, device, equipment and storage medium for straight line driving | |
| CN117860382B (en) | Navigation surgery mechanical arm vision servo pose prediction PD control method based on LSTM | |
| KR20200109552A (en) | Obstacle avoidance pathe generation simulation system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |