CN117503357A - Joint motion control method and system of surgical robot - Google Patents
Joint motion control method and system of surgical robot Download PDFInfo
- Publication number
- CN117503357A CN117503357A CN202311531251.8A CN202311531251A CN117503357A CN 117503357 A CN117503357 A CN 117503357A CN 202311531251 A CN202311531251 A CN 202311531251A CN 117503357 A CN117503357 A CN 117503357A
- Authority
- CN
- China
- Prior art keywords
- surgical robot
- coordinate system
- joint
- end effector
- real
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 239000012636 effector Substances 0.000 claims abstract description 110
- 239000011159 matrix material Substances 0.000 claims abstract description 56
- 230000009466 transformation Effects 0.000 claims abstract description 51
- 238000012544 monitoring process Methods 0.000 claims abstract description 11
- 238000004364 calculation method Methods 0.000 claims description 37
- 238000005265 energy consumption Methods 0.000 claims description 34
- 230000008569 process Effects 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 9
- 230000008878 coupling Effects 0.000 abstract description 2
- 238000010168 coupling process Methods 0.000 abstract description 2
- 238000005859 coupling reaction Methods 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 238000011156 evaluation Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000001356 surgical procedure Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000002682 general surgery Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000003801 milling Methods 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 210000000115 thoracic cavity Anatomy 0.000 description 1
- 230000002485 urinary effect Effects 0.000 description 1
- 230000000007 visual effect Effects 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
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Surgery (AREA)
- Physics & Mathematics (AREA)
- Heart & Thoracic Surgery (AREA)
- General Physics & Mathematics (AREA)
- Robotics (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Biomedical Technology (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computing Systems (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Manipulator (AREA)
Abstract
The invention provides a joint motion control method and a system of a surgical robot, wherein the method comprises the following steps: monitoring data information in a basic coordinate system where each joint of the surgical robot is located and a Cartesian coordinate system of the surgical robot in real time; constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint; computing a homogeneous transformation matrix T of an end effector of a surgical robot relative to a base 0 n The method comprises the steps of carrying out a first treatment on the surface of the Calculating real-time coordinates (x n+1 ,y n+1 ,z n+1 ) The method comprises the steps of carrying out a first treatment on the surface of the And constructing an energy efficiency optimal model of the surgical robot, and calculating an optimal space real-time rotation angle for controlling the end effector to travel to a target point (X, Y, Z) in a basic coordinate system. Carry out the invention and lead toThe real-time dynamic characteristics of a plurality of joints of the surgical robot are described, and nonlinear, coupling and torsion factors in a robot system are fully considered, so that the torsion angle of an end effector of the surgical robot and the travel to a target point are controlled more accurately and finely.
Description
Technical Field
The invention belongs to the technical field of robots, and particularly relates to a joint motion control method and system of a surgical robot.
Background
Since the 21 st century, with the development of science and technology and the reduction of the use cost of surgical robots, the surgical robots have been widely used in general surgery, thoracic surgery, urinary surgery, obstetrics and gynecology and other surgical operations, and the characteristics of good stability, high accuracy and the like have also gained acceptance from more and more surgeons.
The application of the robot technology in medical operation provides a doctor with more accurate and flexible surgical instrument, more visual image feedback and man-machine interaction design which accords with human engineering more, greatly reduces the difficulty of the doctor in completing minimally invasive operation and effectively promotes the application of the endoscopic minimally invasive operation. With the development of robotics, laparoscopic minimally invasive surgery has become one of the important fields of research for surgical robots.
In the world of robotics, three different types are basically distinguished: automatic, semi-automatic, and passive. At present, the number of joint robots is numerous. The passive robot is mainly applied to basic surgery, and thus will not be described. The semiautomatic robot solves part of defects of the previous generation of automatic robots by virtue of unique human-computer interaction; in recent years, with the upgrade of algorithms and the combination of virtual technology, operators can effectively avoid unnecessary errors, and the accurate marketing and milling and accurate navigation of robots are fully utilized to complete the operation. However, in the prior art, the energy consumption is high in the process of advancing the surgical robot end effector to the target point of the focus where the task is finally required to be performed, the accuracy is poor, the surgical efficiency is low due to the mutual collision of the articular points in the advancing process, the advancing track is increased in the whole surgical process of each articular point, and the energy consumption of the whole surgical robot for completing the task is increased in order to avoid the unnecessary path of collision.
Disclosure of Invention
The present invention addresses the above-described shortcomings by providing a method and system for controlling the articulation of a surgical robot. According to the invention, by designing the optimization controller and constructing the energy efficiency optimal model of the surgical robot, the function optimal solution is obtained, and the purpose that the end effector of the surgical robot moves to the target point with the lowest energy consumption of the most space real-time corner automatic control is realized, so that the control precision and efficiency of the robot are improved.
The invention provides the following technical scheme: a method of controlling the articulation of a surgical robot, comprising the steps of:
s1, monitoring data information in a basic coordinate system where each joint of the surgical robot is located and a Cartesian coordinate system of the surgical robot in real time;
s2, constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint
S3, the homogeneous transformation matrix between the ith joint and the (i-1) th joint constructed according to the step S2Calculating a homogeneous transformation matrix of the surgical robot end effector relative to the base>
S4, calculating real-time coordinates (x) of the surgical robot end effector in a basic coordinate system according to the calculation result of the step S3 n+1 ，y n+1 ，z n+1 ) Wherein x is n+1 Real-time abscissa, y in base coordinate system for the surgical robotic end effector n+1 Real-time ordinate, z, in the base coordinate system for the surgical robotic end effector n+1 Real-time vertical coordinates in a base coordinate system for the surgical robot end effector;
s5, constructing an energy efficiency optimal model of the surgical robot according to the calculation result of the step S4 and the data in the Cartesian coordinate system of each joint obtained by real-time monitoring in the step S1, and calculating an optimal space real-time corner of a target point (X, Y, Z) for controlling the end effector to travel to the basic coordinate system, wherein X is the horizontal coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, Y is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, and Z is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system.
Further, the step S2 is implemented by constructing a homogeneous transformation matrix between the (i+1) th joint and the (i) th jointThe following are provided:
wherein alpha is i For the real-time horizontal distance between the ith joint and the ith-1 joint in the basic coordinate system, d i For the real-time vertical distance theta between the ith joint and the (i-1) th joint in the basic coordinate system i For the real-time torsion angle of the ith joint in the basic coordinate system, alpha i The real-time rotation angle of the ith mechanical arm in the basic coordinate system for connecting the ith joint with the (i-1) th joint; i=1, 2,. -%, n; thus, the surgical robot has n joints in total.
Further, the step S3 calculates a homogeneous transformation matrix of the surgical robot end effector with respect to the baseThe calculation formula of (2) is as follows:
further, the step S4 is performed according to the real-time coordinate matrix M of the surgical robot base 0 And the S3 step results, constructing a real-time coordinate matrix M of the surgical robot end effector in a basic coordinate system n+1 ：
Wherein M is n+1 ＝[x n+1 y n+1 z n+1 ] T ，M 0 ＝[x 0 y 0 z 0 ] T ，x 0 Is the x-axis coordinate and y-axis coordinate of the base of the surgical robot in a basic coordinate system 0 Z, the y-axis coordinate of the base of the surgical robot in the base coordinate system 0 Is the z-axis coordinate of the base of the surgical robot within the base coordinate system.
Further, the optimal model of the surgical robot energy efficiency constructed in the step S5 is as follows:
wherein m is i Is the mass of the mth mechanical arm, wherein U is the total energy consumption of the surgical robot end effector to travel to a target point (X, Y, Z) in a base coordinate system, U A For the energy consumption of the n joints of the surgical robot during the travel of the surgical robot end effector to the target points (X, Y, Z) in the basic coordinate system, U B Energy consumption, sigma, from nth joint to end effector during travel of said surgical robot end effector to target point (X, Y, Z) in basic coordinate system 1 For the first energy consumption coefficient, sigma 2 Is a second energy consumption coefficient; phi (phi) n+1 Real-time rotation angle delta of the end effector around the x-axis of the self Cartesian coordinate system n+1 For real-time rotation of the end effector about its own Cartesian coordinate system y-axis, gamma n+1 Real-time rotation angle of the end effector around the z-axis of the self Cartesian coordinate system; according to the optimal model of the surgical robot energy efficiency, the end effector is obtained to control the end effector to rotate in real time in an optimal spaceAnd->Travel to a target point (X, Y, Z) within the base coordinate system.
Further, n joints of the surgical robot consume U during travel of the surgical robot end effector to a target point (X, Y, Z) within a base coordinate system A Computing means of (a)The formula is as follows:
wherein I is i Is the center moment of inertia of the ith mechanical arm between the ith joint and the i-1 th joint.
Further, the energy consumption U from the nth joint to the end effector during the process of the end effector of the surgical robot advancing to the target point (X, Y, Z) in the basic coordinate system B The calculation formula of (2) is as follows:
wherein m is n Is the mass of the nth mechanical arm between the nth node and the end effector, I n Is the center moment of inertia of the nth mechanical arm between the nth node and the end effector.
Further, the first energy consumption coefficient sigma 1 =0.4, the second energy consumption coefficient σ 2 ＝0.6。
Further, when n is 6, -180 DEG is less than or equal to theta 1 ≤180°，-90°≤θ 2 ≤90°，-90°≤θ 3 ≤90°，-135°≤θ 4 ≤135°，-120°≤θ 5 ≤120°，-150°≤θ 2 Less than or equal to 150 degrees; wherein θ 1 、θ 2 、θ 3 、θ 4 、θ 5 And theta 6 The real-time torsion angles of the 1 st joint, the 2 nd joint, the 3 rd joint, the 4 th joint, the 5 th joint and the 6 th joint in the basic coordinate system are respectively limited. I.e. the 1 st to 6 th joints are twisted in real time during the operation of the surgical robot within their respective defined ranges.
The invention also provides an articulation control system of the surgical robot adopting the method, which comprises a data acquisition module, an inter-joint homogeneous transformation matrix construction module, a terminal homogeneous transformation matrix calculation module, a terminal real-time position calculation module and a travel control module;
the data acquisition module monitors the basic coordinate system where each joint of the surgical robot is located and the data information in the self Cartesian coordinate system in real time;
the inter-joint homogeneous transformation matrix construction module is used for constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint
The terminal homogeneous transformation matrix calculation module is used for constructing a homogeneous transformation matrix between the ith joint and the ith-1 joint according to the inter-joint homogeneous transformation matrix construction moduleCalculating a homogeneous transformation matrix of the surgical robot end effector relative to the base>
The end real-time position calculation module is used for calculating real-time coordinates (x) of the surgical robot end effector in a basic coordinate system according to the calculation result of the end homogeneous transformation matrix calculation module n+1 ，y n+1 ，z n+1 ) Wherein x is n+1 Real-time abscissa, y in base coordinate system for the surgical robotic end effector n+1 Real-time ordinate, z, in the base coordinate system for the surgical robotic end effector n+1 Real-time vertical coordinates in a base coordinate system for the surgical robot end effector;
the advancing control module is used for constructing an energy efficiency optimal model of the surgical robot according to the calculation module result of the real-time position calculation of the tail end and the data in the Cartesian coordinate system obtained by the real-time monitoring of the data acquisition module, calculating and controlling the tail end executor to advance to a target point (X, Y, Z) in a basic coordinate system at an optimal space real-time corner, wherein X is the horizontal coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system, Y is the vertical coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system, and Z is the vertical coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system.
The beneficial effects of the invention are as follows:
1. in order to solve the problems in the prior art, an automated control method of an end effector of a surgical robot is proposed. Using homogeneous transformation matrices between the ith joint and the (i-1) th jointThe real-time dynamic characteristics of a plurality of joints of the surgical robot are described, and nonlinear, coupling and torsion factors in a robot system are fully considered, so that the torsion angle of an end effector of the surgical robot is controlled and the end effector travels to a target point more accurately and finely. And designing an optimization controller, constructing an energy efficiency optimal model of the surgical robot, obtaining a function optimal solution, and enabling an end effector of the surgical robot to travel to a target point with the lowest energy consumption of the most spatial real-time corner automatic control so as to improve the control precision and efficiency of the robot.
Drawings
The invention will be described in more detail hereinafter on the basis of embodiments and with reference to the accompanying drawings. Wherein:
FIG. 1 is a schematic flow diagram of an articulation control method for a surgical robot provided by the present invention;
FIG. 2 is a schematic diagram of the results of an articulation control system for a surgical robot provided by the present invention;
fig. 3 is a schematic view of an electronic device employing the method for controlling the articulation of a surgical robot according to the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Fig. 1 is a schematic view showing an articulation control method of a surgical robot according to the present invention, including the steps of:
s1, monitoring data information in a basic coordinate system where each joint of the surgical robot is located and a Cartesian coordinate system of the surgical robot in real time;
s2, constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint
S3, constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint according to the step S2Calculating the homogeneous transformation matrix of the surgical robot end effector relative to the base>
S4, according to the calculation result of the step S3, calculating real-time coordinates (x n+1 ,y n+1 ,z n+1 ) Wherein x is n+1 Real-time abscissa, y for surgical robot end effector in base coordinate system n+1 Real-time ordinate, z, in the base coordinate system for the surgical robot end effector n+1 Real-time vertical coordinates of the surgical robot end effector in a basic coordinate system;
s5, constructing an energy efficiency optimal model of the surgical robot according to the calculation result of the step S4 and the data in the Cartesian coordinate system of each joint obtained by real-time monitoring in the step S1, and calculating an optimal space real-time corner of a target point (X, Y, Z) for controlling the end effector to travel to the basic coordinate system, wherein X is the horizontal coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, Y is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, and Z is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system.
Further, in order to clarify how the (i+1) th joint is changed based on the real-time data of the (i) th joint, it is necessary to clarify the homogeneous transformation matrix between the (i+1) th joint and the (i) th jointExpressed by real-time data of which ith joint, therefore, as another preferred embodiment of the invention, the (i+1) th joint constructed in step S2 and the homogeneous transformation matrix between the ith joints->The following are provided:
wherein alpha is i For the real-time horizontal distance between the ith joint and the ith-1 joint in the basic coordinate system, d i For the real-time vertical distance theta between the ith joint and the (i-1) th joint in the basic coordinate system i For the real-time torsion angle of the ith joint in the basic coordinate system, alpha i The real-time rotation angle of the ith mechanical arm in the basic coordinate system for connecting the ith joint with the (i-1) th joint; i=1, 2,. -%, n; thus, the surgical robot has n joints in total.
Further, the secondary transformation matrix of each joint is multiplied to obtain a homogeneous transformation matrix of the surgical robot end effector relative to the base calculated in step S3The calculation formula of (2) is as follows:
i.e. < ->Represents the homogeneous transformation matrix multiplied by the homogeneous transformation matrix of the 1 st joint until the homogeneous transformation matrix of the nth joint, thus obtaining the homogeneous transformation matrix +.>
Further, according to the calculated homogeneous transformation matrix of the surgical robot end effector relative to the baseAnd the real-time coordinate position of the 0 th joint (i.e. the base) in the basic coordinate system, the real-time coordinate position of the end effector can be obtained, so as another preferred embodiment of the present invention, step S4, according to the real-time coordinate matrix M of the surgical robot base 0 S3, as a result of the step S, constructing a real-time coordinate matrix M of the surgical robot end effector in a basic coordinate system n+1 ：
In the surgical robot working process, the base of the surgical robot is used as the 0 th joint, namely the initial joint, and cannot move,
wherein M is n+1 ＝[x n+1 y n+1 z n+1 ] T ，M 0 ＝[x 0 y 0 z 0 ] T ，x 0 Is the x-axis coordinate and y-axis coordinate of the base of the surgical robot in a basic coordinate system 0 Z, the y-axis coordinate of the base of the surgical robot in the base coordinate system 0 Is the z-axis coordinate of the base of the surgical robot within the base coordinate system.
As another preferred embodiment of the present invention, in order to reduce the energy consumption of the end effector of the surgical robot to travel to the target position, and further, to control the end effector of the surgical robot to travel to the target point (X, Y, Z) with this as a target, an optimal real-time torsion angle that yields the lowest energy consumption for the end effector of the surgical robot to travel to the target point can be obtained, and therefore, as another preferred embodiment of the present invention, the optimal model of the surgical robot energy efficiency constructed in step S5 is as follows:
wherein m is i Is the mass of the mth mechanical arm, wherein U is the total energy consumption of the surgical robot end effector to travel to the target point (X, Y, Z) in the base coordinate system, U A For the energy consumption of n joints of the surgical robot during the travel of the end effector of the surgical robot to the target points (X, Y, Z) in the basic coordinate system, U B Energy consumption, sigma, from nth joint to end effector during travel of surgical robot end effector to target point (X, Y, Z) in basic coordinate system 1 For the first energy consumption coefficient, sigma 2 Is a second energy consumption coefficient; phi (phi) n+1 Real-time rotation angle delta of end effector around X-axis of self Cartesian coordinate system n+1 Real-time rotation angle gamma of end effector around its own Cartesian coordinate system y-axis n+1 Real-time rotation angle of the end effector around the z-axis of the self Cartesian coordinate system; according to the optimal model of the surgical robot energy efficiency, the end effector is obtained to control the end effector to rotate in real time in an optimal spaceAnd->Travel to a target point (X, Y, Z) within the base coordinate system.
Further, n joints of the surgical robot consume U during travel of the surgical robot end effector to a target point (X, Y, Z) in the base coordinate system A The calculation formula of (2) is as follows:
wherein I is i Is the ith switchCenter moment of inertia of the ith mechanical arm between the joint and the i-1 th joint.
Wherein,and->The first derivative of the real-time coordinates of the ith joint with respect to time, respectively, is ++because distance/time is speed>And->The real-time travelling speeds of the ith joint in the basic coordinate system on the x axis, the y axis and the z axis are respectively calculated, the sum of squares of the travelling speeds in each dimension is calculated as the square of the real-time travelling speed of the ith joint in the basic coordinate system, the square of the mass multiplied by the speed is calculated as the real-time linear motion kinetic energy of the ith joint, and I i Multiplying the square of the real-time travelling speed of the ith joint by the ith mechanical arm between the ith joint and the ith-1 joint, namely the real-time driving kinetic energy for driving the ith joint to travel; the sum of the two is the energy consumption required to drive the ith joint in the process of driving the end effector of the surgical robot to travel to the target point (X, Y, Z), and the sum of the energy consumption from the 1 st joint to the nth joint is the energy consumption U of the n joints in the process of driving the end effector of the surgical robot to travel to the target point (X, Y, Z) in a basic coordinate system A 。
Further preferably, the total energy consumption of the nth joint to the end effector required to be driven during travel of the end effector to the target point (X, Y, Z) may be calculated according to the above calculation theory, and thus the energy consumption U of the nth joint to the end effector during travel of the surgical robot end effector to the target point (X, Y, Z) in the base coordinate system B The calculation formula of (2) is as follows:
wherein m is n Is the mass of the nth mechanical arm between the nth node and the end effector, I n Is the center moment of inertia of the nth mechanical arm between the nth node and the end effector.
Further preferably, the first energy consumption coefficient sigma 1 =0.4, second energy consumption coefficient σ 2 ＝0.6。
Further preferably, in order to avoid collisions caused by the travel of a plurality of joints during travel, it is necessary to twist the angle θ in real time within the base coordinate system i When n is 6, -180 DEG is less than or equal to theta 1 ≤180°，-90°≤θ 2 ≤90°，-90°≤θ 3 ≤90°，-135°≤θ 4 ≤135°，-120°≤θ 5 ≤120°，-150°≤θ 2 Less than or equal to 150 degrees; wherein θ 1 、θ 2 、θ 3 、θ 4 、θ 5 And theta 6 The real-time torsion angles of the 1 st joint, the 2 nd joint, the 3 rd joint, the 4 th joint, the 5 th joint and the 6 th joint in the basic coordinate system are respectively limited. I.e. the 1 st to 6 th joints are twisted in real time during the operation of the surgical robot within their respective defined ranges.
As shown in fig. 2, the invention also provides an articulation control system of the surgical robot adopting the method, which comprises a data acquisition module, an inter-joint homogeneous transformation matrix construction module, an end homogeneous transformation matrix calculation module, an end real-time position calculation module and a travel control module;
the data acquisition module is used for monitoring data information in a basic coordinate system where each joint of the surgical robot is located and a Cartesian coordinate system of the surgical robot in real time;
an inter-joint homogeneous transformation matrix construction module for constructing homogeneous transformation matrix between the ith joint and the (i-1) th joint
Terminal homogeneous transformation matrix calculation moduleHomogeneous transformation matrix between ith joint and ith-1 th joint constructed according to inter-joint homogeneous transformation matrix construction moduleCalculating the homogeneous transformation matrix of the surgical robot end effector relative to the base>
The terminal real-time position calculation module is used for calculating real-time coordinates (x n+1 ，y n+1 ，z n+1 ) Wherein x is n+1 Real-time abscissa, y for surgical robot end effector in base coordinate system n+1 Real-time ordinate, z, in the base coordinate system for the surgical robot end effector n+1 Real-time vertical coordinates of the surgical robot end effector in a basic coordinate system;
the advancing control module is used for constructing an energy efficiency optimal model of the surgical robot according to the calculation module result of the end real-time position calculation and the data in the Cartesian coordinate system of each joint obtained by the real-time monitoring of the data acquisition module, calculating and controlling the end effector to advance to a target point (X, Y, Z) in the basic coordinate system at an optimal space real-time corner, wherein X is the horizontal coordinate of the target point in the basic coordinate system of the end effector of the surgical robot, Y is the vertical coordinate of the target point in the basic coordinate system of the end effector of the surgical robot, and Z is the vertical coordinate of the target point in the basic coordinate system of the end effector of the surgical robot.
The present invention also provides an electronic device employing the above-described articulation control method of a surgical robot, referring to fig. 3, which illustrates a schematic structural diagram of an electronic device 100 suitable for use in implementing embodiments of the present disclosure. The electronic devices in the embodiments of the present disclosure may include, but are not limited to, mobile terminals such as mobile phones, notebook computers, digital broadcast receivers, PDAs (personal digital assistants), PADs (tablet computers), PMPs (portable multimedia players), in-vehicle terminals (e.g., in-vehicle navigation terminals), and the like, and stationary terminals such as digital TVs, desktop computers, and the like. The electronic device shown in fig. 3 is merely an example and should not be construed to limit the functionality and scope of use of the disclosed embodiments.
As shown in fig. 3, the electronic device 100 may include a processing means (e.g., a central processing unit, a graphics processor, etc.) 101 that may perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) 102 or a program loaded from a storage means 108 into a Random Access Memory (RAM) 103. In the RAM 103, various programs and data necessary for the operation of the electronic apparatus 100 are also stored. The processing device 101, ROM 102, and RAM 103 are connected to each other by a bus 104. An input/output (I/O) interface 105 is also connected to bus 104.
In general, the following devices may be connected to the I/O interface 105: input devices 106 including, for example, a touch screen, touchpad, keyboard, mouse, image sensor, microphone, accelerometer, gyroscope, etc.; an output device 107 including, for example, a Liquid Crystal Display (LCD), a speaker, a vibrator, and the like; storage devices 108 including, for example, magnetic tape, hard disk, etc.; and a communication device 109. The communication means 109 may allow the electronic device 100 to communicate wirelessly or by wire with other devices to exchange data. While fig. 3 shows the electronic device 100 with various means, it is to be understood that not all of the illustrated means are required to be implemented or provided. More or fewer devices may be implemented or provided instead.
In particular, according to embodiments of the present disclosure, the processes described above with reference to flowcharts may be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program embodied on a computer readable medium, the computer program comprising program code for performing the method shown in the flowcharts. In such an embodiment, the computer program may be downloaded and installed from a network via the communication means 109, or from the storage means 108, or from the ROM 102. The above-described functions defined in the methods of the embodiments of the present disclosure are performed when the computer program is executed by the processing device 101.
It should be noted that the computer readable medium described in the present disclosure may be a computer readable signal medium or a computer readable storage medium, or any combination of the two. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples of the computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this disclosure, a computer-readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In the present disclosure, however, the computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, with the computer-readable program code embodied therein. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: electrical wires, fiber optic cables, RF (radio frequency), and the like, or any suitable combination of the foregoing.
The computer readable medium may be contained in the electronic device; or may exist alone without being incorporated into the electronic device.
The computer readable medium carries one or more programs which, when executed by the electronic device, cause the electronic device to: acquiring at least two internet protocol addresses; sending a node evaluation request comprising at least two internet protocol addresses to node evaluation equipment, wherein the node evaluation equipment selects an internet protocol address from the at least two internet protocol addresses and returns the internet protocol address; receiving an Internet protocol address returned by node evaluation equipment; wherein the acquired internet protocol address indicates an edge node in the content distribution network.
Alternatively, the computer-readable medium carries one or more programs that, when executed by the electronic device, cause the electronic device to: receiving a node evaluation request comprising at least two internet protocol addresses; selecting an internet protocol address from at least two internet protocol addresses; returning the selected internet protocol address; wherein the received internet protocol address indicates an edge node in the content distribution network.
Computer program code for carrying out operations of the present disclosure may be written in one or more programming languages, including an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
The flowcharts and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted 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-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
The units involved in the embodiments of the present disclosure may be implemented by means of software, or may be implemented by means of hardware. The name of the unit does not in any way constitute a limitation of the unit itself, for example the first acquisition unit may also be described as "unit acquiring at least two internet protocol addresses".
It should be understood that the various steps recited in the method embodiments of the present disclosure may be performed in a different order and/or performed in parallel. Furthermore, method embodiments may include additional steps and/or omit performing the illustrated steps. The scope of the present disclosure is not limited in this respect.
The term "including" and variations thereof as used herein are intended to be open-ended, i.e., including, but not limited to. The term "based on" is based at least in part on. The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments. Related definitions of other terms will be given in the description below.
It should be noted that the terms "first," "second," and the like in this disclosure are merely used to distinguish between different devices, modules, or units and are not used to define an order or interdependence of functions performed by the devices, modules, or units.
It should be noted that references to "one", "a plurality" and "a plurality" in this disclosure are intended to be illustrative rather than limiting, and those of ordinary skill in the art will appreciate that "one or more" is intended to be understood as "one or more" unless the context clearly indicates otherwise.
The foregoing description is only of the preferred embodiments of the present disclosure and description of the principles of the technology being employed. It will be appreciated by persons skilled in the art that the scope of the disclosure referred to in this disclosure is not limited to the specific combinations of features described above, but also covers other embodiments which may be formed by any combination of features described above or equivalents thereof without departing from the spirit of the disclosure. Such as those described above, are mutually substituted with the technical features having similar functions disclosed in the present disclosure (but not limited thereto).
Claims (10)
1. A method for controlling the articulation of a surgical robot, comprising the steps of:
s1, monitoring data information in a basic coordinate system where each joint of the surgical robot is located and a Cartesian coordinate system of the surgical robot in real time;
s2, constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint
S3, the homogeneous transformation matrix between the ith joint and the (i-1) th joint constructed according to the step S2Calculating a homogeneous transformation matrix of the surgical robot end effector relative to the base>
S4, calculating real-time coordinates (x) of the surgical robot end effector in a basic coordinate system according to the calculation result of the step S3 n+1 ,y n+1 ,z n+1 ) Wherein x is n+1 Real-time abscissa, y in base coordinate system for the surgical robotic end effector n+1 Real-time ordinate, z, in the base coordinate system for the surgical robotic end effector n+1 Real-time vertical coordinates in a base coordinate system for the surgical robot end effector;
s5, constructing an energy efficiency optimal model of the surgical robot according to the calculation result of the step S4 and the data in the Cartesian coordinate system of each joint obtained by real-time monitoring in the step S1, and calculating an optimal space real-time corner of a target point (X, Y, Z) for controlling the end effector to travel to the basic coordinate system, wherein X is the horizontal coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, Y is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system, and Z is the vertical coordinate of the target point of the end effector of the surgical robot in the basic coordinate system.
2. The method according to claim 1, wherein the step S2 is performed to construct a homogeneous transformation matrix between the (i+1) th joint and the (i) th jointThe following are provided:
wherein alpha is i For the real-time horizontal distance between the ith joint and the ith-1 joint in the basic coordinate system, d i For the real-time vertical distance theta between the ith joint and the (i-1) th joint in the basic coordinate system i For the real-time torsion angle of the ith joint in the basic coordinate system, alpha i The real-time rotation angle of the ith mechanical arm in the basic coordinate system for connecting the ith joint with the (i-1) th joint; i=1, 2, …, n; thus, the surgical robot has n joints in total.
3. The method of claim 1, wherein the step S3 calculates a homogeneous transformation matrix of the surgical robot end effector with respect to the baseThe calculation formula of (2) is as follows:
4. the method according to claim 3, wherein the step S4 is performed based on the real-time coordinate matrix M of the surgical robot base 0 And the S3 step results, constructing a real-time coordinate matrix M of the surgical robot end effector in a basic coordinate system n+1 ：
In the surgical robot working process, the base of the surgical robot is used as the 0 th joint, namely the initial joint, and cannot move,
wherein M is n+1 ＝[x n+1 y n+1 z n+1 ] T ，M 0 ＝[x 0 y 0 z 0 ] T ，x 0 Is the x-axis coordinate and y-axis coordinate of the base of the surgical robot in a basic coordinate system 0 Z, the y-axis coordinate of the base of the surgical robot in the base coordinate system 0 Is the z-axis coordinate of the base of the surgical robot within the base coordinate system.
5. The method for controlling the articulation of the surgical robot according to claim 1, wherein the optimal model of the surgical robot energy efficiency constructed in the step S5 is as follows:
wherein m is i Is the mass of the mth mechanical arm, wherein U is the total energy consumption of the surgical robot end effector to travel to a target point (X, Y, Z) in a base coordinate system, U A For the energy consumption of the n joints of the surgical robot during the travel of the surgical robot end effector to the target points (X, Y, Z) in the basic coordinate system, U B Energy consumption, sigma, from nth joint to end effector during travel of said surgical robot end effector to target point (X, Y, Z) in basic coordinate system 1 For the first energy consumption coefficient, sigma 2 Is a second energy consumption coefficient; phi (phi) n+1 Real-time rotation angle delta of the end effector around the x-axis of the self Cartesian coordinate system n+1 For real-time rotation of the end effector about its own Cartesian coordinate system y-axis, gamma n+1 Real-time rotation angle of the end effector around the z-axis of the self Cartesian coordinate system; according to the optimal model of the surgical robot energy efficiency, the end effector is obtained to control the end effector to rotate in real time in an optimal spaceAnd->Travel to a target point (X, Y, Z) within the base coordinate system.
6. The method of claim 5, wherein n joints of the surgical robot consume energy U during travel of the surgical robot end effector to a target point (X, Y, Z) in a base coordinate system A The calculation formula of (2) is as follows:
wherein I is i Is the center moment of inertia of the ith mechanical arm between the ith joint and the i-1 th joint.
7. The method of claim 5, wherein the energy consumption U from the nth joint to the end effector during travel of the surgical robot end effector to the target point (X, Y, Z) in the base coordinate system B The calculation formula of (2) is as follows:
wherein m is n Is the mass of the nth mechanical arm between the nth node and the end effector, I n Is the center moment of inertia of the nth mechanical arm between the nth node and the end effector.
8. The method of controlling the articulation of a surgical robot of claim 5, wherein the first energy consumption coefficient σ 1 =0.4, the second energy consumption coefficient σ 2 ＝0.6。
9. The method for controlling the articulation of a surgical robot according to claim 2, wherein, -180 ° - θ when n is 6 1 ≤180°，-90°≤θ 2 ≤90°，-90°≤θ 3 ≤90°，-135°≤θ 4 ≤135°，-120°≤θ 5 ≤120°，-150°≤θ 2 Less than or equal to 150 degrees; wherein θ 1 、θ 2 、θ 3 、θ 4 、θ 5 And theta 6 The real-time torsion angles of the 1 st joint, the 2 nd joint, the 3 rd joint, the 4 th joint, the 5 th joint and the 6 th joint in the basic coordinate system are respectively limited.
10. An articulation control system for a surgical robot employing the method of any one of claims 1-9, comprising a data acquisition module, an inter-joint homogeneous transformation matrix construction module, an end homogeneous transformation matrix calculation module, an end real-time position calculation module, and a travel control module;
the data acquisition module monitors the basic coordinate system where each joint of the surgical robot is located and the data information in the self Cartesian coordinate system in real time;
the inter-joint homogeneous transformation matrix construction module is used for constructing a homogeneous transformation matrix between the ith joint and the (i-1) th joint
The terminal homogeneous transformation matrix calculation module is used for constructing a homogeneous transformation matrix between the ith joint and the ith-1 joint according to the inter-joint homogeneous transformation matrix construction moduleCalculating a homogeneous transformation matrix of the surgical robot end effector relative to the base>
The end real-time position calculation module is used for calculating real-time coordinates (x) of the surgical robot end effector in a basic coordinate system according to the calculation result of the end homogeneous transformation matrix calculation module n+1 ,y n+1 ,z n+1 ) Wherein x is n+1 Real-time abscissa, y in base coordinate system for the surgical robotic end effector n+1 Real-time ordinate, z, in the base coordinate system for the surgical robotic end effector n+1 Real-time vertical coordinates in a base coordinate system for the surgical robot end effector;
the advancing control module is used for constructing an energy efficiency optimal model of the surgical robot according to the calculation module result of the real-time position calculation of the tail end and the data in the Cartesian coordinate system obtained by the real-time monitoring of the data acquisition module, calculating and controlling the tail end executor to advance to a target point (X, Y, Z) in a basic coordinate system at an optimal space real-time corner, wherein X is the horizontal coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system, Y is the vertical coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system, and Z is the vertical coordinate of the target point of the tail end executor of the surgical robot in the basic coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311531251.8A CN117503357A (en) | 2023-11-16 | 2023-11-16 | Joint motion control method and system of surgical robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311531251.8A CN117503357A (en) | 2023-11-16 | 2023-11-16 | Joint motion control method and system of surgical robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117503357A true CN117503357A (en) | 2024-02-06 |
Family
ID=89750932
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311531251.8A Pending CN117503357A (en) | 2023-11-16 | 2023-11-16 | Joint motion control method and system of surgical robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117503357A (en) |
-
2023
- 2023-11-16 CN CN202311531251.8A patent/CN117503357A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2021128787A1 (en) | Positioning method and apparatus | |
| US8966400B2 (en) | User movement interpretation in computer generated reality | |
| CN112318506A (en) | Automatic calibration method, device, equipment, mechanical arm and medium for mechanical arm | |
| CN113119111A (en) | Mechanical arm and track planning method and device thereof | |
| WO2022205844A1 (en) | Robot forward kinematics solution method and apparatus, readable storage medium, and robot | |
| CN116079697B (en) | Monocular vision servo method, device, equipment and medium based on image | |
| CN112536792B (en) | Robot arm inverse solution method based on spherical geometric analysis method | |
| CN111890364B (en) | Robot motion control method, device, electronic device and storage medium | |
| CN116460859B (en) | SCARA robot motion compensation method, SCARA robot motion compensation device, SCARA robot motion compensation equipment and SCARA robot motion compensation storage medium | |
| CN117503357A (en) | Joint motion control method and system of surgical robot | |
| CN113129366B (en) | Monocular SLAM initialization method and device and electronic equipment | |
| CN113043277B (en) | Multi-joint mechanism trajectory planning method and device, electronic equipment and storage medium | |
| CN112025704B (en) | Real-time distributed robot control method and system based on memory type database | |
| CN117084788A (en) | Method and device for determining target gesture of mechanical arm and storage medium | |
| CN112270066B (en) | Optimization method for calculating rigid coupling coefficient of satellite and computer equipment | |
| WO2023246576A1 (en) | Robotic arm adjustment method, apparatus, electronic device and storage medium | |
| Zhao et al. | Model and simulation of the mitsubishi rv-m1 robot using matlab | |
| CN116673941B (en) | Mechanical arm auxiliary-based operation control method and device | |
| WO2023179264A1 (en) | Air input method, device and system | |
| CN116000925B (en) | Mechanical arm tracking method, device, equipment and storage medium | |
| CN118061206B (en) | Robot TCP calibration device, method, computer equipment and storage medium | |
| CN116372942B (en) | Robot joint compensation method and device, electronic equipment and readable storage medium | |
| CN117944031A (en) | Control method of flexible mechanical arm, equipment and medium | |
| CN114516052B (en) | Dynamics control method and system for parallel real-time high-performance multi-axis mechanical arm | |
| CN114147703B (en) | Robot obstacle avoidance method and device, computer readable storage medium and robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |