CN118000904A - Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot - Google Patents
Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot Download PDFInfo
- Publication number
- CN118000904A CN118000904A CN202311807234.2A CN202311807234A CN118000904A CN 118000904 A CN118000904 A CN 118000904A CN 202311807234 A CN202311807234 A CN 202311807234A CN 118000904 A CN118000904 A CN 118000904A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- tool load
- mass
- motion
- center
- 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
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/70—Manipulators specially adapted for use in surgery
- A61B34/77—Manipulators with motion or force scaling
-
- 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
Landscapes
- Health & Medical Sciences (AREA)
- Surgery (AREA)
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Biomedical Technology (AREA)
- Robotics (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Manipulator (AREA)
Abstract
本申请提供了手术机器人末端自适应工具负载标定方法、装置及设备，所述方法包括：基于远端运动中心对机械臂运动轨迹进行约束；获取雅可比矩阵；控制安装有工具负载的机械臂按照约束的运动轨迹进行匀速运动，并对匀速运动状态下的机械臂进行数据采样；确定机械臂末端六维力；构建所述工具负载的质量最小二乘等式与质心最小二乘等式；求解所述工具负载的质量；求解所述工具负载的质心。本申请中，通过约束机械臂进行匀速运动，并基于运动过程中的关节额外力矩求解工具负载的质心与质量，从而实现对手术机器人末端工具负载的标定。
The present application provides a method, device and equipment for calibrating the adaptive tool load at the end of a surgical robot, the method comprising: constraining the motion trajectory of the robot arm based on the distal motion center; obtaining the Jacobian matrix; controlling the robot arm equipped with the tool load to move at a uniform speed according to the constrained motion trajectory, and sampling data of the robot arm in a uniform motion state; determining the six-dimensional force at the end of the robot arm; constructing the least squares equation for the mass of the tool load and the least squares equation for the center of mass; solving the mass of the tool load; solving the center of mass of the tool load. In the present application, the calibration of the tool load at the end of the surgical robot is achieved by constraining the robot arm to move at a uniform speed, and solving the center of mass and mass of the tool load based on the additional torque of the joints during the motion process.
Description
Technical Field
The application relates to the technical field of data processing, in particular to a method, a device and equipment for calibrating a self-adaptive tool load at the tail end of a surgical robot.
Background
The surgical robot system is a comprehensive system integrating a plurality of modern high-tech means, has wide application and has a great deal of clinical and surgical application. Bone joint surgery such as hip joint replacement surgery, knee joint replacement surgery, spine surgery and the like can be completed through the surgical robot system, so that pain and dysfunction caused by severe damage or diseases of joints can be treated, hip pain can be remarkably relieved, and the life quality of patients can be improved.
The self-adaptive tool load calibration of the tail end of the surgical robot plays a vital role in ensuring the surgical accuracy and the cooperative safety of the human and the machine, but an effective scheme for calibrating the self-adaptive tool load of the tail end of the surgical robot is still lacking at present.
Disclosure of Invention
The application solves the problem that a scheme for calibrating the self-adaptive tool load of the tail end of the surgical robot is lacking.
To solve the above problems, a first aspect of the present application provides a method for calibrating a load of an adaptive tool at a distal end of a surgical robot, including:
Constraining the motion trail of the mechanical arm based on the far-end motion center;
Acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
controlling a mechanical arm provided with a tool load to perform uniform motion according to a constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain a joint angle and joint additional moment of the mechanical arm;
determining six-dimensional force at the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
constructing a mass least squares equation and a centroid least squares equation for the tool load;
solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
A second aspect of the present application provides a surgical robot tip adaptive tool load calibration apparatus comprising:
the track constraint module is used for constraining the motion track of the mechanical arm based on the far-end motion center;
the matrix acquisition module is used for acquiring a jacobian matrix based on the kinematic parameters of the mechanical arm;
The mechanical arm movement module is used for controlling the mechanical arm provided with the tool load to move at a constant speed according to the constrained movement track, sampling data of the mechanical arm in a constant speed movement state and acquiring the joint angle and the joint additional moment of the mechanical arm;
The tail end calculation module is used for determining six-dimensional force of the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
an equation construction module for constructing a mass least squares equation and a centroid least squares equation for the tool load;
the mass solving module is used for solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And the mass center solving module is used for solving the mass center of the tool load according to the mass center least square equation and the six-dimensional force of the tail end of the mechanical arm.
A third aspect of the present application provides an electronic device comprising: a memory and a processor;
The memory is used for storing programs;
The processor, coupled to the memory, is configured to execute the program for:
Constraining the motion trail of the mechanical arm based on the far-end motion center;
Acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
controlling a mechanical arm provided with a tool load to perform uniform motion according to a constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain a joint angle and joint additional moment of the mechanical arm;
determining six-dimensional force at the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
constructing a mass least squares equation and a centroid least squares equation for the tool load;
solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
A fourth aspect of the present application provides a computer readable storage medium having stored thereon a computer program for execution by a processor to implement the surgical robot tip adaptive tool load calibration method described above.
According to the application, the mechanical arm is restrained to perform uniform motion, and the mass center and the mass of the tool load are solved based on the joint additional moment in the motion process, so that the calibration of the tool load at the tail end of the surgical robot is realized.
According to the application, through planning the angular velocity, the mechanical arm is constrained to perform uniform motion based on the far-end constraint center, so that the mechanical arm uniformly covers a more comprehensive gesture during motion, and acceleration interference caused by non-uniform velocity is eliminated, so that the load is accurately calibrated.
Drawings
FIG. 1 is a flow chart of a method for calibrating a surgical robot end adaptive tool load according to an embodiment of the present application;
FIG. 2 is a block diagram of a surgical robot end adaptive tool load calibration apparatus according to an embodiment of the present application;
fig. 3 is a block diagram of an electronic device according to an embodiment of the present application.
Detailed Description
In order that the above objects, features and advantages of the application will be readily understood, a more particular description of the application will be rendered by reference to specific embodiments thereof which are illustrated in the appended drawings. While exemplary embodiments of the present application are shown in the drawings, it should be understood that the present application may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the application to those skilled in the art.
It is noted that unless otherwise indicated, technical or scientific terms used herein should be given the ordinary meaning as understood by one of ordinary skill in the art to which this application belongs.
The surgical robot system is a comprehensive system integrating a plurality of modern high-tech means, has wide application and has a great deal of clinical and surgical application. Bone joint surgery such as hip joint replacement surgery, knee joint replacement surgery, spine surgery and the like can be completed through the surgical robot system, so that pain and dysfunction caused by severe damage or diseases of joints can be treated, hip pain can be remarkably relieved, and the life quality of patients can be improved.
The end self-adaptive tool load calibration of the surgical robot has a vital effect on ensuring the surgical accuracy, the cooperation safety of the human and the machine, accurate end tool load parameters are beneficial to improving passive man-machine interaction such as the knee joint replacement robot, and the operator operates the mechanical arm to control the end osteotomy tool to finish the accuracy of knee joint osteotomy operation. However, the existing method mainly adopts manual estimation, and an effective scheme for calibrating the self-adaptive tool load of the tail end of the surgical robot is still lacking.
Aiming at the problems, the application provides a new scheme for calibrating the tail end self-adaptive tool load of the surgical robot, which can determine the mass center and the mass of the tail end tool load by restraining the movement track of the mechanical arm and solve the problem that the existing scheme for calibrating the tail end self-adaptive tool load of the surgical robot is lacking.
The embodiment of the application provides a method for calibrating an end-of-surgical-robot self-adaptive tool load, which is specifically provided by figures 1-3, and can be implemented by an end-of-surgical-robot self-adaptive tool load calibrating device which can be integrated in electronic equipment such as a computer, a server, a computer, a server cluster, a data center and the like. Referring to fig. 1, a flow chart of a method for calibrating a surgical robot end adaptive tool load according to one embodiment of the application is shown; the method for calibrating the self-adaptive tool load at the tail end of the surgical robot comprises the following steps:
S100, restraining a motion track of the mechanical arm based on a far-end motion center;
In the application, the motion track of the mechanical arm is restrained based on the far-end motion center, namely the motion of the mechanical arm is restrained to move around a far-end fixed point, namely the far-end motion center (RCM).
In one embodiment, the constraint formula of the distal center of motion is:
Wherein Wx and Wy are angular velocities around x and y axes of the far-end movement center, w is a planned angular velocity, and t is time.
Further, under the condition that the far-end movement center of the mechanical arm movement is set, the mechanical arm is restrained to perform uniform movement through the far-end movement center, so that the interference suffered by the mechanical arm is eliminated through uniform movement, and the accuracy of end load calibration is ensured.
According to the application, through the planning of the angular velocity, the mechanical arm is constrained to perform uniform motion based on the far-end constraint center, so that the mechanical arm uniformly covers a more comprehensive gesture during motion, and acceleration interference caused by non-uniform velocity is eliminated, so that the load is accurately calibrated.
It should be noted that, the constraint formula for the distal movement center is to set the coordinates of the distal movement center under the flange of the mechanical arm as (0, z), so as to constrain the distal movement center to be in the z-axis and rotate around the z-axis (i.e. the rotation plane is perpendicular to the z-axis) at a uniform speed.
In one embodiment, the constraint formula of the motion trail of the mechanical arm is:
Wherein e is the terminal flange coordinate system, v e is the linear velocity of the terminal flange, w e is the angular velocity of the terminal flange, rcm is the coordinate system of the distal center of motion, v rcm is the linear velocity of the distal center of motion, w rcm is the angular velocity of the distal center of motion, The rotation matrix is the terminal flange coordinate system and the remote movement center coordinate system, and P rcm is the antisymmetric matrix of the coordinates of the remote movement center under the terminal flange coordinate system.
S200, acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
Setting the joint angle of the mechanical arm as q; because the mechanical arm is provided with a plurality of joints, the i joint angle is q i, wherein i is more than 5, and the additional moment tau i at the joint is the additional joint moment read after the dead weight and inertia of the mechanical arm connecting rod are ignored at the i joint;
Therefore, a conversion matrix of the end flange under the base can be obtained according to the kinematic parameters of the mechanical arm:
Wherein the method comprises the steps of The description of the end flange of the mechanical arm under the base is that e is the end flange and b is the base of the mechanical arm. n is the number of joints of the mechanical arm, n is more than or equal to 6,For the description of joint 2 in the case of joint 1 as a base,For the description of joint 3 in the case of a2 nd joint as a base,Described for the nth joint in the case of the nth-1 joint as the base; the 1 st joint is a mechanical arm base, and the n-th joint is a terminal flange.
In the application, the jacobian matrix refers to the partial differential relation between the pose (position and pose) of the end-effector of the robot and the position values of all joints of the robot.
In one embodiment, the jacobian is obtained based on kinematic parameters of the robotic arm including a link length of the robotic arm and a deflection angle of the link.
When the mechanical arm is analyzed, the mechanical arm is formed by connecting a plurality of rods (joints are arranged at the positions where the two rods are connected), the rods are called connecting rods in the mechanical arm, each connecting rod can rotate around a certain axis on the basis of a connecting rod (a last connecting rod) (the connecting rod is driven to rotate by the rotation of a motor), and the axis is only forbidden relatively to the connecting rod, so that once the connecting rods move, the connecting rods can only perform direct space conversion relation with a last-stage connecting rod and a next-stage connecting rod, namely, when the rotation angle of the first connecting rod is determined, the specific position of the first connecting rod can be calculated according to the space coordinates of a base, and then the position of the final mechanical arm, namely, the tail end position of the mechanical arm can be obtained by first-stage backward calculation.
In the present application, a jacobian matrix, which can be considered as a function of the joint angle of each joint, can be constructed from known kinematic parameters of the robotic arm (link length, deflection angle of adjacent links).
In the present application, the jacobian matrix J (θ) can also be based on a conversion matrix(Wherein, the length of the connecting rod and the deflection angle of the connecting rod between the two adjacent joints are mutually corresponding) and the specific calculation mode can refer to the corresponding calculation process in the prior art, and the application is not repeated.
S300, controlling the mechanical arm provided with the tool load to perform uniform motion according to the constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain the joint angle and the joint additional moment of the mechanical arm;
in the application, under the condition of knowing the jacobian matrix and the constrained mechanical arm motion trail, the mechanical arm motion trail can be converted into the joint angular velocity by utilizing the jacobian matrix, namely:
wherein J (theta) is a jacobian matrix of differential motion of the mechanical arm, v is a terminal flange speed, comprising a linear speed and an angular speed, The joint angular velocity is also the control input data of the mechanical arm.
In the present application, the actual joint angle change/the derivative of the actual joint angle with time is the joint angular velocity.
Therefore, based on the acquired control input data of the mechanical arm, the mechanical arm can be input, and the mechanical arm is controlled to perform uniform motion according to the constrained motion track.
In the application, the data of the mechanical arm in the motion state can be sampled by selecting proper remote motion center coordinates and angular velocity and taking a preset deltat as a sampling interval; other sampling modes are also possible, and the specific sampling mode can be adjusted according to actual conditions.
The data acquisition of the mechanical arm can be completed through a sensor, the sensor can be a six-dimensional moment sensor arranged at the base and the joint position of the mechanical arm, and can simultaneously measure three axial forces Fx, fy and Fz and three axial moments Mx, my and Mz (then corresponding joint angles and joint additional moments are calculated); other sensors or other measuring means that can directly measure or indirectly measure the joint angle and the joint additional moment are also possible.
It should be noted that the additional moment of the joints of the mechanical arm is the additional moment of all the joints on the mechanical arm; the additional moment of the joint can be read directly from the sensor based on the sensor characteristics, or can be calculated from the joint moment-calculated from the kinetic equation.
According to the application, the mechanical arm is controlled to perform uniform motion according to the constrained track, and in the motion state, the angular speed and the linear speed of the flange are constant values, so that the interference of acceleration on the gravity calculation is eliminated.
In the application, in the process of controlling the mechanical arm to perform uniform motion according to the constrained track, the mechanical arm can be controlled to move a plurality of different joint angle poses to complete uniform motion.
S400, determining six-dimensional force at the tail end of the mechanical arm according to the Jacobian matrix and the joint additional moment;
In one embodiment, the calculation formula of the six-dimensional force at the tail end of the mechanical arm is as follows:
F＝J(θ)*τ
Wherein F is six-dimensional force at the tail end of the mechanical arm, J (theta) is a jacobian matrix of differential motion of the mechanical arm, and tau is an additional moment vector of the joint.
In the application, the formula is obtained according to kinematic constraint, wherein J (theta) is 6*n matrix, which is jacobian matrix for representing differential motion of the mechanical arm, and tau is joint extra moment vector epsilon n multiplied by 1.
Based on the aforementioned end six-dimensional force f= (F x,Fy,Fz,Mx,My,Mz)T and joint additional moment conversion relationship, the arm end six-dimensional force can be calculated given the jacobian matrix and the joint additional moment.
S500, constructing a mass least square equation and a centroid least square equation of the tool load;
in one embodiment, the mass least squares equation for the tool load is:
B＝(RTR)-1·RTf
f＝[Fx1 Fy1 Fz1 Fx2 Fy2 Fz2 …… Fxn Fyn Fzn]T
B＝[0 0 mg]T
wherein B is a mass coordinate matrix, F is a plurality of groups of sampling data vectors of force, R is a rotation matrix, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translation representation of a translation vector along x, y and z axes, n is the number of samples, m is the load mass, g is the gravitational acceleration, T is the transpose, For the rotation matrix of the end flange under the base,Is the rotation matrix at the first sampling.
It should be noted that F is a lower case expression, which is merely for distinguishing from the aforementioned terminal six-dimensional force F, and does not have any additional meaning.
In the application, the default gravity direction points to the positive Z direction.
In the application, the base coordinate system of the mechanical arm is set as base for short b, the terminal flange coordinate system is set as end for short e, and the default base installation is parallel to the horizon, so that the mass least square equation is obtained.
In one embodiment, the centroid least squares equation for the tool load is:
M＝[Mx1 My1 Mz1 Mx2 My2 Mz2 ...... Mxn Myn Mzn]T
Wherein A is a centroid coordinate matrix, For an antisymmetric matrix of forces, M is a six-dimensional force rotation vector, T is a transpose, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translational representation of the translation vector along the x, y, z axes, M x1、My1、Mz1、Mx2、Mz2、Mxn、Myn、Mzn is a rotational representation of the rotation vector about the x, y, z axes, and n is the number of samples.
It should be noted that the number of the substrates,The superscript representation is merely to distinguish from the aforementioned terminal six-dimensional force F and does not have any additional meaning in the rest.
In the application, the coordinates of the mass center of the sensor and the end clamp holder in the end flange coordinate system are (x, y, z),
Mx＝Fz×y-Fy×z
My＝Fx×z-Fz×x
Mz＝Fy×x-Fx×y
It is rewritten into the following format to generate the centroid least squares equation described above.
According to the application, after the mechanical arm is controlled to move to different postures, n (n is more than or equal to 3) sets of joint additional moments (each set of data is one sampling moment) can be obtained, so that the centroid least square equation is calculated based on the n sets of joint additional moments.
S600, solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
and S700, solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
In the application, when a plurality of groups of sampling data are acquired, a least square method of a matrix is carried out according to a mass least square equation and a mass center least square equation, the least square method can simply calculate unknown data, and the square sum of errors between the calculated data and actual data is minimized. The specific calculation mode is not described in detail in the present application.
According to the application, the mechanical arm is restrained to perform uniform motion, and the mass center and the mass of the tool load are solved based on the joint additional moment in the motion process, so that the calibration of the tool load at the tail end of the surgical robot is realized.
In the application, the remote motion center RCM is utilized to carry out sampling track constraint, so that the gravity is uniformly distributed on the flange when data are acquired, the coverage is wide, and the calibration is proper.
The embodiment of the application provides a surgical robot tail end self-adaptive tool load calibration device, which is used for executing the surgical robot tail end self-adaptive tool load calibration method provided by the application, and the surgical robot tail end self-adaptive tool load calibration device is described in detail below.
As shown in fig. 2, the surgical robot end adaptive tool load calibration device includes:
a track constraint module 101, configured to constrain a motion track of the mechanical arm based on a distal motion center;
a matrix acquisition module 102 for acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
The mechanical arm movement module 103 is used for controlling the mechanical arm with the tool load to move at a uniform speed according to the constrained movement track, sampling data of the mechanical arm in a uniform movement state and acquiring the joint angle and the joint additional moment of the mechanical arm;
An end calculation module 104 for determining a six-dimensional force of the end of the mechanical arm according to the jacobian matrix and the joint additional moment;
An equation construction module 105 for constructing a mass least squares equation and a centroid least squares equation for the tool load;
a mass solving module 106, configured to solve the mass of the tool load according to the mass least square equation and the six-dimensional force of the end of the mechanical arm;
a centroid solving module 107, configured to solve for a centroid of the tool load according to the centroid least square equation and the arm end six-dimensional force.
In one embodiment, the constraint formula of the distal center of motion is:
Wherein Wx and Wy are angular velocities around x and y axes of the far-end movement center, w is a planned angular velocity, and t is time.
In one embodiment, the constraint formula of the motion trail of the mechanical arm is:
Wherein e is the terminal flange coordinate system, v e is the linear velocity of the terminal flange, w e is the angular velocity of the terminal flange, rcm is the coordinate system of the distal center of motion, v rcm is the linear velocity of the distal center of motion, w rcm is the angular velocity of the distal center of motion, The rotation matrix is the terminal flange coordinate system and the remote movement center coordinate system, and P rcm is the antisymmetric matrix of the coordinates of the remote movement center under the terminal flange coordinate system.
In one embodiment, the kinematic parameters of the robotic arm include a link length of the robotic arm and a deflection angle of the link.
In one embodiment, the calculation formula of the six-dimensional force at the tail end of the mechanical arm is as follows:
F＝J(θ)*τ
Wherein F is six-dimensional force at the tail end of the mechanical arm, J (theta) is a jacobian matrix of differential motion of the mechanical arm, and tau is an additional moment vector of the joint.
In one embodiment, the mass least squares equation for the tool load is:
B＝(RTR)-1·RTf
f＝[Fx1 Fy1 Fz1 Fx2 Fy2 Fz2 …… Fxn Fyn Fzn]T
B＝[0 0 m g]T
wherein B is a mass coordinate matrix, F is a plurality of groups of sampling data vectors of force, R is a rotation matrix, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translation representation of a translation vector along x, y and z axes, n is the number of samples, m is the load mass, g is the gravitational acceleration, T is the transpose, For the rotation matrix of the end flange under the base,Is the rotation matrix at the first sampling.
In one embodiment, the centroid least squares equation for the tool load is:
M＝[Mx1 My1 Mz1 Mx2 My2 Mz2 ...... Mxn Myn Mzn]T
Wherein A is a centroid coordinate matrix, For an antisymmetric matrix of forces, M is a six-dimensional force rotation vector, T is a transpose, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translational representation of the translation vector along the x, y, z axes, M x1、My1、Mz1、Mx2、Mz2、Mxn、Myn、Mzn is a rotational representation of the rotation vector about the x, y, z axes, and n is the number of samples.
The surgical robot end self-adaptive tool load calibration device provided by the embodiment of the application has a corresponding relation with the surgical robot end self-adaptive tool load calibration method provided by the embodiment of the application, so that specific content in the device has a corresponding relation with the surgical robot end self-adaptive tool load calibration method, and specific content can refer to records in the surgical robot end self-adaptive tool load calibration method, and the description of the specific content is omitted in the application.
The surgical robot end self-adaptive tool load calibration device and the surgical robot end self-adaptive tool load calibration method provided by the embodiment of the application have the same beneficial effects as the method adopted, operated or realized by the stored application program because of the same inventive concept.
The internal functions and structures of the surgical robot end adaptive tool load calibration apparatus are described above, and as shown in fig. 3, in practice, the surgical robot end adaptive tool load calibration apparatus may be implemented as an electronic device, including: memory 301 and processor 303.
The memory 301 may be configured to store a program.
In addition, the memory 301 may also be configured to store other various data to support operations on the electronic device. Examples of such data include instructions for any application or method operating on the electronic device, contact data, phonebook data, messages, pictures, videos, and the like.
The memory 301 may be implemented by any type or combination of volatile or nonvolatile memory devices such as Static Random Access Memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic or optical disk.
A processor 303 coupled to the memory 301 for executing programs in the memory 301 for:
Constraining the motion trail of the mechanical arm based on the far-end motion center;
Acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
controlling a mechanical arm provided with a tool load to perform uniform motion according to a constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain a joint angle and joint additional moment of the mechanical arm;
determining six-dimensional force at the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
constructing a mass least squares equation and a centroid least squares equation for the tool load;
solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
In one embodiment, the constraint formula of the distal center of motion is:
Wherein Wx and Wy are angular velocities around x and y axes of the far-end movement center, w is a planned angular velocity, and t is time.
In one embodiment, the constraint formula of the motion trail of the mechanical arm is:
Wherein e is the terminal flange coordinate system, v e is the linear velocity of the terminal flange, w e is the angular velocity of the terminal flange, rcm is the coordinate system of the distal center of motion, v rcm is the linear velocity of the distal center of motion, w rcm is the angular velocity of the distal center of motion, The rotation matrix is the terminal flange coordinate system and the remote movement center coordinate system, and P rcm is the antisymmetric matrix of the coordinates of the remote movement center under the terminal flange coordinate system.
In one embodiment, the kinematic parameters of the robotic arm include a link length of the robotic arm and a deflection angle of the link.
In one embodiment, the calculation formula of the six-dimensional force at the tail end of the mechanical arm is as follows:
F＝J(θ)*τ
Wherein F is six-dimensional force at the tail end of the mechanical arm, J (theta) is a jacobian matrix of differential motion of the mechanical arm, and tau is an additional moment vector of the joint.
In one embodiment, the mass least squares equation for the tool load is:
B＝(RTR)-1·RTf
f＝[Fx1 Fy1 Fz1 Fx2 Fy2 Fz2 …… Fxn Fyn Fzn]T
B＝[0 0 mg]T
wherein B is a mass coordinate matrix, F is a plurality of groups of sampling data vectors of force, R is a rotation matrix, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translation representation of a translation vector along x, y and z axes, n is the number of samples, m is the load mass, g is the gravitational acceleration, T is the transpose, For the rotation matrix of the end flange under the base,Is the rotation matrix at the first sampling.
In one embodiment, the centroid least squares equation for the tool load is:
M＝[Mx1 My1 Mz1 Mx2 My2 Mz2 ...... Mxn Myn Mzn]T
Wherein A is a centroid coordinate matrix, For an antisymmetric matrix of forces, M is a six-dimensional force rotation vector, T is a transpose, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translational representation of the translation vector along the x, y, z axes, M x1、My1、Mz1、Mx2、Mz2、Mxn、Myn、Mzn is a rotational representation of the rotation vector about the x, y, z axes, and n is the number of samples.
In the present application, the processor is further specifically configured to execute all the processes and steps of the method for calibrating the load of the self-adaptive tool at the tail end of the surgical robot, and the specific content can refer to the record in the method for calibrating the load of the self-adaptive tool at the tail end of the surgical robot, which is not described in detail in the present application.
In the present application, only some components are schematically shown in fig. 3, which does not mean that the electronic device only includes the components shown in fig. 3.
The electronic equipment provided by the embodiment of the application has the same beneficial effects as the method adopted, operated or realized by the application program stored by the electronic equipment and the method for calibrating the self-adaptive tool load of the tail end of the surgical robot provided by the embodiment of the application are the same in inventive concept.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described 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 flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or 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, embedded processor, 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, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
In one typical configuration, a computing device includes one or more processors (CPUs), input/output interfaces, network interfaces, and memory.
The memory may include volatile memory, random Access Memory (RAM), and/or nonvolatile memory, such as Read Only Memory (ROM) or Flash memory (Flash RAM), among others, in a computer readable medium. Memory is an example of computer-readable media.
The present application also provides a computer readable storage medium corresponding to the surgical robot end adaptive tool load calibration method provided in the foregoing embodiments, on which a computer program (i.e., a program product) is stored, which when executed by a processor, performs the surgical robot end adaptive tool load calibration method provided in any of the foregoing embodiments.
Computer readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of storage media for a computer include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium, which can be used to store information that can be accessed by a computing device. Computer-readable media, as defined herein, does not include transitory computer-readable media (Transitory Media), such as modulated data signals and carrier waves.
The computer readable storage medium provided by the above embodiment of the present application has the same beneficial effects as the method adopted, operated or implemented by the application program stored in the computer readable storage medium, because the computer readable storage medium and the method for calibrating the surgical robot end adaptive tool load provided by the embodiment of the present application are the same inventive concept.
In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the application may be practiced without these specific details. In some instances, well-known structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
It should also be noted that 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 one … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises the element.
The foregoing is merely exemplary of the present application and is not intended to limit the present application. Various modifications and variations of the present application will be apparent to those skilled in the art. Any modification, equivalent replacement, improvement, etc. which come within the spirit and principles of the application are to be included in the scope of the claims of the present application.
Claims (10)
1. A method for calibrating a load of an adaptive tool at the tail end of a surgical robot, comprising the steps of:
Constraining the motion trail of the mechanical arm based on the far-end motion center;
Acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
controlling a mechanical arm provided with a tool load to perform uniform motion according to a constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain a joint angle and joint additional moment of the mechanical arm;
determining six-dimensional force at the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
constructing a mass least squares equation and a centroid least squares equation for the tool load;
solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
2. The surgical robot tip adaptive tool load calibration method of claim 1, wherein the constraint formula for the distal center of motion is:
Wherein Wx and Wy are angular velocities around x and y axes of the far-end movement center, w is a planned angular velocity, and t is time.
3. The surgical robot end adaptive tool load calibration method according to claim 1, wherein the constraint formula of the motion trail of the mechanical arm is:
Wherein e is the terminal flange coordinate system, v e is the linear velocity of the terminal flange, w e is the angular velocity of the terminal flange, rcm is the coordinate system of the distal center of motion, v rcm is the linear velocity of the distal center of motion, w rcm is the angular velocity of the distal center of motion, The rotation matrix is the terminal flange coordinate system and the remote movement center coordinate system, and P rcm is the antisymmetric matrix of the coordinates of the remote movement center under the terminal flange coordinate system.
4. A method of calibrating a surgical robot tip-adaptive tool load according to any of claims 1-3, wherein the jacobian is obtained based on kinematic parameters of the robotic arm, the kinematic parameters including a length of a link of the robotic arm and a deflection angle of the link.
5. A method for calibrating a surgical robot end adaptive tool load according to any one of claims 1-3, wherein the calculation formula of the six-dimensional force of the arm end is:
F＝J(θ)*τ
Wherein F is six-dimensional force at the tail end of the mechanical arm, J (theta) is a jacobian matrix of differential motion of the mechanical arm, and tau is an additional moment vector of the joint.
6. A surgical robot end-of-range adaptive tool load calibration method according to any one of claims 1-3, wherein the tool load's mass least squares equation is:
B＝(RTR)-1·RTf
f＝[Fx1 Fy1 Fz1 Fx2 Fy2 Fz2 … … Fxn Fyn Fzn]T
B＝[0 0 mg]T
wherein B is a mass coordinate matrix, F is a plurality of groups of sampling data vectors of force, R is a rotation matrix, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translation representation of a translation vector along x, y and z axes, n is the number of samples, m is the load mass, g is the gravitational acceleration, T is the transpose, For the rotation matrix of the end flange under the base,Is the rotation matrix at the first sampling.
7. A surgical robot tip adaptive tool load calibration method according to any one of claims 1 to 3, wherein the centroid least squares equation for the tool load is:
M＝[Mx1 My1 Mz1 Mx2 My2 Mz2 … … Mxn Myn Mzn]T
Wherein A is a centroid coordinate matrix, For an antisymmetric matrix of forces, M is a six-dimensional force rotation vector, T is a transpose, F x1、Fy1、Fz1、Fx2、Fz2、Fxn、Fyn、Fzn is a translational representation of the translation vector along the x, y, z axes, M x1、My1、Mz1、Mx2、Mz2、Mxn、Myn、Mzn is a rotational representation of the rotation vector about the x, y, z axes, and n is the number of samples.
8. A surgical robot tip-adaptive tool load calibration device, comprising:
the track constraint module is used for constraining the motion track of the mechanical arm based on the far-end motion center;
the matrix acquisition module is used for acquiring a jacobian matrix based on the kinematic parameters of the mechanical arm;
The mechanical arm movement module is used for controlling the mechanical arm provided with the tool load to move at a constant speed according to the constrained movement track, sampling data of the mechanical arm in a constant speed movement state and acquiring the joint angle and the joint additional moment of the mechanical arm;
The tail end calculation module is used for determining six-dimensional force of the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
an equation construction module for constructing a mass least squares equation and a centroid least squares equation for the tool load;
the mass solving module is used for solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And the mass center solving module is used for solving the mass center of the tool load according to the mass center least square equation and the six-dimensional force of the tail end of the mechanical arm.
9. An electronic device, comprising: a memory and a processor;
The memory is used for storing programs;
The processor, coupled to the memory, is configured to execute the program for:
Constraining the motion trail of the mechanical arm based on the far-end motion center;
Acquiring a jacobian matrix based on kinematic parameters of the mechanical arm;
controlling a mechanical arm provided with a tool load to perform uniform motion according to a constrained motion track, and sampling data of the mechanical arm in a uniform motion state to obtain a joint angle and joint additional moment of the mechanical arm;
determining six-dimensional force at the tail end of the mechanical arm according to the jacobian matrix and the joint additional moment;
constructing a mass least squares equation and a centroid least squares equation for the tool load;
solving the mass of the tool load according to the mass least square equation and the six-dimensional force of the tail end of the mechanical arm;
And solving the centroid of the tool load according to the centroid least square equation and the six-dimensional force of the tail end of the mechanical arm.
10. A computer readable storage medium having stored thereon a computer program, wherein the program is executed by a processor to implement the surgical robot tip adaptive tool load calibration method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311807234.2A CN118000904B (en) | 2023-12-26 | 2023-12-26 | Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311807234.2A CN118000904B (en) | 2023-12-26 | 2023-12-26 | Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118000904A true CN118000904A (en) | 2024-05-10 |
| CN118000904B CN118000904B (en) | 2024-08-16 |
Family
ID=90947705
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311807234.2A Active CN118000904B (en) | 2023-12-26 | 2023-12-26 | Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118000904B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014110682A1 (en) * | 2013-01-18 | 2014-07-24 | Robotiq Inc. | Force/torque sensor, apparatus and method for robot teaching and operation |
| WO2016057967A2 (en) * | 2014-10-09 | 2016-04-14 | Harris Dynamics | Articulating torsional coupling |
| WO2017219639A1 (en) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | Motion trail planning method and device for robotic arm, and robot |
| CN113601509A (en) * | 2021-08-16 | 2021-11-05 | 安徽元古纪智能科技有限公司 | Multi-degree-of-freedom mechanical arm flexible control method and system |
| US20220193902A1 (en) * | 2020-12-23 | 2022-06-23 | Ubtech Robotics Corp Ltd. | Total centroid state estimation method, humanoid robot and computer readable storage medium using the same |
| CN116695342A (en) * | 2023-05-26 | 2023-09-05 | 浙江大学 | Robot-assisted sewing feed track planning method based on curvature segmentation |
| US20230373089A1 (en) * | 2021-02-05 | 2023-11-23 | Ubtech Robotics Corp Ltd | Method for controlling robot, robot and computer-readable storage medium |
-
2023
- 2023-12-26 CN CN202311807234.2A patent/CN118000904B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014110682A1 (en) * | 2013-01-18 | 2014-07-24 | Robotiq Inc. | Force/torque sensor, apparatus and method for robot teaching and operation |
| WO2016057967A2 (en) * | 2014-10-09 | 2016-04-14 | Harris Dynamics | Articulating torsional coupling |
| WO2017219639A1 (en) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | Motion trail planning method and device for robotic arm, and robot |
| US20220193902A1 (en) * | 2020-12-23 | 2022-06-23 | Ubtech Robotics Corp Ltd. | Total centroid state estimation method, humanoid robot and computer readable storage medium using the same |
| US20230373089A1 (en) * | 2021-02-05 | 2023-11-23 | Ubtech Robotics Corp Ltd | Method for controlling robot, robot and computer-readable storage medium |
| CN113601509A (en) * | 2021-08-16 | 2021-11-05 | 安徽元古纪智能科技有限公司 | Multi-degree-of-freedom mechanical arm flexible control method and system |
| CN116695342A (en) * | 2023-05-26 | 2023-09-05 | 浙江大学 | Robot-assisted sewing feed track planning method based on curvature segmentation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118000904B (en) | 2024-08-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111618859B (en) | Method for feeding back mechanical arm high-precision force under static or low-speed working condition | |
| Murotsu et al. | Parameter identification of unknown object handled by free-flying space robot | |
| CN104842352B (en) | Robot system using visual feedback | |
| Wilson et al. | Relative end-effector control using cartesian position based visual servoing | |
| JP5618066B2 (en) | Force control robot calibration apparatus and method | |
| JP7058929B2 (en) | Drive devices, robot devices, control methods, article manufacturing methods, control programs, and recording media | |
| EP3112096A1 (en) | Robot arm apparatus, calibration method, and program | |
| CN111496791A (en) | Overall dynamics parameter identification method based on series robot | |
| JP2007030054A (en) | Robot equipment | |
| CN108445764B (en) | Active compliance control strategy of Stewart platform | |
| CN115533916B (en) | A method, system, device and medium for identifying load mass at the end of a robotic arm | |
| CN118322205B (en) | A finite-time control method for visual servoing based on velocity observer | |
| WO2025073242A9 (en) | Method and apparatus for controlling robotic arm, and device, storage medium and program product | |
| Feng et al. | Measurement of mobile manipulator chassis pose change caused by suspension deformation and end-effector accuracy improvement based on multi-sensor fusion | |
| JP2013013950A (en) | Robot, robot control device, robot control method and robot control program | |
| JP6896824B2 (en) | Robot devices, robot systems, control methods for robot devices, manufacturing methods for articles using robot devices, information processing devices, information processing methods, control programs and recording media | |
| WO2024216564A1 (en) | Method for calibrating articulated robot, computer device and readable storage medium | |
| JP3698770B2 (en) | Load weight estimation method | |
| CN118000904A (en) | Method, device and equipment for calibrating load of adaptive tool at the end of surgical robot | |
| JPH02205489A (en) | Control method for impedance of manipulator | |
| JP3104839B2 (en) | Robot diagnosis method | |
| CN119423903A (en) | Control method and control device of planar bone cutting device and planar bone cutting device | |
| Chen et al. | Pose estimation of robotic end-effectors under low speed motion using EKF with inertial and SE (3) measurements | |
| CN106826826B (en) | A kind of robot control method and device | |
| Zhang et al. | Lightweight long-reach 5-dof robot arm for farm application |
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 |