KR102269776B1 - Calibration Method of Robot for Interventional treatment - Google Patents

Abstract

The present invention relates to a method for calibrating an interventional robot, and according to the present invention, by selecting a calibration parameter by calculating a gravity vector with respect to the gravity axis and using the calculated gravity vector as a constraint, the gravity axis when performing calibration of the interventional robot It is possible to accurately perform the correction for , and accordingly, there is an effect of improving the accuracy of the calibration.

Description

Calibration Method of Robot for Interventional treatment

The present invention relates to a method of calibrating an interventional robot, and more particularly, to a method of improving the accuracy of calibration by accurately correcting a gravity axis when an interventional robot is calibrated.

In general, robots have been developed for industrial use and used as part of factory automation, or to perform tasks on behalf of humans in extreme environments that humans cannot tolerate. In recent years, this field of robotics has been developed while being used in the cutting-edge space development industry, leading to the development of human-friendly home robots in recent years.

In particular, interventional surgery in the medical field is a representative minimally invasive medical technology and is rapidly developing as a new medical technology that can be applied in a wide range of fields including diagnosis and treatment. The development of a robot system that can accurately target lesions in the abdominal and thoracic organs using image registration and guidance technology is required.

For these interventional robots, safer and more accurate robot control based on surgical plan information is of utmost importance, and for this, it is essential to secure high-level spatial matching technology between imaging device and robot and robot calibration technology.

In general, the position of the end effector of the robot can be calculated by inputting rotational or translational information of each joint into the robot kinematics model. However, the position of the end effector calculated using the kinematic model is different from the actual position due to manufacturing errors of the instrument and deflection due to its own weight. In order to reduce these errors, it is more effective to calibrate the kinematic model through software than to change the mechanical structure or design of the robot itself, and this calibration is called calibration.

1 is a view for explaining a problem when the conventional calibration method is applied to an interventional robot.

Referring to FIG. 1, when the calibration method used in the existing industrial robot is applied to the interventional robot, the position of the end effector is determined using the kinematic model and the joint angle while moving the robot to a plurality of positions within the operating area of the robot. At the same time, after measuring the actual position of the end effector using a three-dimensional position measuring device, the robot kinematics model and the base coordinate system and the tool coordinate system are numerically calculated to minimize the error between the calculated position and the actual measured position. Calibration can be performed by calculating.

However, in the case of the 5-DOF scara-type interventional robot as shown in FIG. 1, since the gravity axis of the base coordinate system affects the correction value of the two axes, which are the rotational axes, if the calibration technology of the existing industrial robot is applied to the interventional robot as it is, the gravity There is a problem in that the accuracy of the position control of the robot decreases because the axis is not properly compensated.

Patent Publication No. 10-2005-0001608 (Jan. 7, 2005)

The present invention has been devised to solve the problems of the prior art as described above, and an object of the present invention is to improve the accuracy of calibration by accurately correcting the gravity axis when performing calibration of an interventional robot.

In order to achieve the above object, the calibration method of the interventional robot according to an embodiment of the present invention includes (a) measuring the positions of a plurality of reference points on a base plane using a three-dimensional position measuring device and measuring the positions of the measured reference points. calculating a gravity vector about the gravity axis as a basis; (b) measuring the position of the end effector using a three-dimensional position measuring device in various postures, and calculating the position of the end effector using a kinematic model of the robot based on the joint angle and the DH parameter; (c) Estimate the base coordinate system based on the position of the end effector measured using the three-dimensional position measuring device and the position of the end effector calculated using the kinematic model of the robot, and correct the estimated base coordinate system using the gravity vector to do; (d) selecting a calibration parameter using a gravity vector as a constraint to correct the gravity axis when performing calibration; and (e) performing calibration for correcting the base coordinate system, the tool coordinate system, and the kinematic model of the robot based on the selected calibration parameter.

According to the present invention, by calculating the gravity vector with respect to the gravity axis and selecting the calibration parameter using the calculated gravity vector as a constraint condition, it is possible to accurately correct the gravity axis when performing the calibration of the interventional robot. Accordingly, there is an effect that the accuracy of calibration can be improved.

1 is a view for explaining a problem when the conventional calibration method is applied to an interventional robot.
2 is a flowchart illustrating a calibration method of an interventional robot according to an embodiment of the present invention.
3 to 7 are diagrams for explaining a calibration method of an interventional robot according to an embodiment of the present invention.

Hereinafter, a preferred embodiment of the present invention will be described with reference to the accompanying drawings in order to describe in detail enough that a person of ordinary skill in the art to which the present invention pertains can easily implement the technical idea of the present invention.

In describing the preferred embodiment of the present invention, if it is determined that the detailed description of the related known technology may unnecessarily obscure the gist of the present invention, the detailed description will be omitted or briefly described.

Prior to describing the present invention, the basic concept of the present invention will be briefly described to help the understanding of the present invention as follows.

As described above, in the case of the 5-DOF scara type interventional robot, the gravity axis of the base coordinate system affects the correction value of the two axes. Therefore, if the calibration technology of the existing industrial robot is applied to the interventional robot as it is, the correction of the gravity axis Since this is not done properly, there is a problem in that the accuracy of the position control of the robot decreases.

To this end, the present invention calculates a gravity vector with respect to the gravity axis and selects a calibration parameter using the calculated gravity vector as a constraint condition so that the correction of the gravity axis is accurately performed when the interventional robot is calibrated. The characteristics of will be more clearly understood through the embodiments described below.

2 is a flowchart illustrating a method of calibrating an interventional robot according to an embodiment of the present invention, and FIGS. 3 to 7 are diagrams for explaining a method of calibrating an interventional robot according to an embodiment of the present invention.

First, as shown in FIG. 3, the positions of a plurality of reference points on the base plane are measured using a three-dimensional position measuring device (S210), and a gravity vector for the gravity axis is calculated based on the positions of the measured reference points. Calculate (S220).

Here, the plurality of reference points on the base plane is preferably at least 5 or more.

_{Next, as shown in FIG. 4 , the position p i} of the end effector is measured using a three-dimensional position measuring device in various postures while changing the posture of the robot ( S230 ).

_{At this time, the end effector position f(q i} ) is calculated by the following Equation 1 using the kinematic model of the robot based on the joint angle (q _i ) and the DH parameter input from the encoder (S240).

Here, T _MB is the transformation matrix from the coordinate system of the three-dimensional position measuring device to the base coordinate system, T _B1 is the transformation matrix from the base coordinate system to the 1-axis coordinate system of the robot, and T _{i -1,i} is the (i-1) of the robot. The transformation matrix from the )-axis coordinate system to the i-axis coordinate system, T _TE represents the transformation matrix from the tool coordinate system to the end effector, respectively.

These transformation matrices can be represented by DH parameters, which are parameters representing transformation matrices between two coordinate systems when there are two arbitrary continuous link coordinate systems among robot links as shown in FIG. 5, x, y, z A transformation matrix can be expressed using the axis translational parameters (a, b, d) and rotational parameters (α, β, θ). T _MB and T _TE are generally constants, but in the present invention, they are a different transformation matrix because they are a subject of calibration. It can be expressed as a DH parameter like

Next, based on the position of the end effector (p _{i ) measured using the three-dimensional position measuring device and the position of the end effector ( q i} ) calculated using the kinematic model of the robot, the base by the following Equation 2 A coordinate system (T _MB ) is estimated (S250).

이며, T_MB는 추정된 베이스 좌표계를 나타낸다. Here, p _i represents the position of the i-th end-effector measured using a three-dimensional position measuring device, and f(q _i ) represents the i-th end-effector position calculated using the kinematic model of the robot,

, and T _MB represents the estimated base coordinate system.

Then, the estimated base coordinate system is corrected using the gravity vector (S260), and this will be described in more detail as follows.

)를 중력 벡터(

)에 일치시킨다.First, as shown in Fig. 6, any one of the x-axis, y-axis, and z-axis of the base coordinate system is set as the gravity axis, and the unit vector of the axis set as the gravity axis in the base coordinate system (

) to the gravity vector (

) to match.

)를 중력 벡터(

)에 일치시키는 경우를 가정하여 설명한다.In this embodiment, the x-axis of the base coordinate system is set as the gravity axis, and the unit vector (

) to the gravity vector (

) is assumed to match.

)와 중력축으로 설정된 축의 단위 벡터(

)간의 회전 행렬

을 다음의 수학식 3에 의하여 구한다.Next, the gravity vector (

) and the unit vector of the axis set as the gravity axis (

) rotation matrix between

is obtained by the following equation (3).

는 중력축에 대한 중력 벡터,

는 베이스 좌표계에서 중력축으로 설정된 축의 단위 벡터,

은 중력 벡터(

)와 중력축으로 설정된 축의 단위 벡터(

)간의 회전 행렬을 각각 나타낸다.From here,

is the gravitational vector about the gravitational axis,

is the unit vector of the axis set as the gravity axis in the base coordinate system,

is the gravity vector (

) and the unit vector of the axis set as the gravity axis (

) represents the rotation matrix between

을 기초로 다음의 수학식 4에 의하여 베이스 좌표계(T_MB)를 보정하여 보정된 베이스 좌표계(T_MB')를 구한다.Next, the obtained rotation matrix

Based on Equation 4 below, the base coordinate system T _MB is corrected to obtain the corrected base coordinate system T _MB '.

은 중력 벡터(

)와 중력축으로 설정된 축의 단위 벡터(

)간의 회전 행렬, T_MB'은 중력 벡터를 이용하여 보정된 베이스 좌표계를 각각 나타낸다.where T _MB is the base coordinate system,

is the gravity vector (

) and the unit vector of the axis set as the gravity axis (

), the rotation matrix, T _MB ', represents the base coordinate system corrected using the gravity vector, respectively.

)를 구속조건으로 하는 캘리브레이션 파라미터 선택 행렬(S)을 이용하여 캘리브레이션 파라미터를 선택한다(S270).Then, the gravity vector (

) is used as a constraint to select a calibration parameter using the calibration parameter selection matrix S (S270).

Here, S is a calibration parameter selection matrix, a, b, and d represent translation parameters of x, y, and z axes, and α, β, and θ represent rotation parameters of x, y, and z axes.

If the value of an element in the calibration parameter selection matrix S is 1, it means that the parameter of the corresponding coordinate system is selected as the calibration parameter, and if 0, it means that the parameter of the corresponding coordinate system is not selected as the calibration parameter and is fixed. For example, b _{B = 1 means that the b parameter (b B} ), which is the y-axis translation parameter of the base coordinate system, is selected as the calibration parameter, and β _{B = 0 means that the β parameter (β B} ), the y-axis rotation parameter of the base coordinate system, is the calibration parameter. It means that it is not selected and is fixed.

Selecting a calibration parameter using the calibration parameter selection matrix S will be described in more detail as follows.

First, in the case of the base coordinate system, a _B , b _B , and d _B are set to 1 to select the translation parameters of the x, y, and z axes as the calibration parameters. Then, among the rotation parameters of the x, y, and z axes, α _B , β _B , and θ _{B , the rotation parameter (eg, α B} ) of the gravity axis (eg, the x-axis) is set to 1 and selected as a calibration parameter.

)를 구속조건으로 하여 나머지 다른 축의 회전 파라미터(예를 들어 β_B, θ_B )는 0으로 설정하여 캘리브레이션 파라미터로 선택하지 않고 고정시킨다.At this time, the gravity vector (

) as a constraint, the rotation parameters of the other axes (eg, β _B , θ _B ) are set to 0 and are fixed without being selected as calibration parameters.

)를 구속조건으로 하여 중력축이 아닌 다른 축의 회전 파라미터를 고정시키는 이유는, 도 7에 도시된 바와 같이 캘리브레이션에 의해 중력축(예를 들어 x축)이 아닌 다른 축(예를 들어 y축 또는 z축)을 중심으로 회전이 발생하게 되면 중력축도 함께 회전되기 때문에, 중력축이 아닌 다른 축의 회전 파라미터를 0으로 고정시켜 중력축이 회전되지 않도록 하는 것이다.Here, the gravity vector (

) as a constraint, the reason for fixing the rotation parameter of an axis other than the gravity axis is, as shown in FIG. 7, an axis other than the gravity axis (eg x-axis) by calibration When rotation occurs about the z-axis), since the gravity axis is also rotated, the rotation parameter of the axis other than the gravity axis is fixed to 0 to prevent the gravity axis from rotating.

)를 구속조건으로 하여 캘리브레이션 파라미터를 선택하는 것이다.That is, in order to accurately correct the gravity axis when performing calibration later, the gravity vector (

) as a constraint to select a calibration parameter.

Next, in the case of the uniaxial coordinate system or the n-axis coordinate system, the translational parameter d of the z-axis is considered to be the largest error dimension of the joint angle than the guarantee of analytical inverse kinematic solutions and other manufacturing errors due to the characteristics of the medical robot where safety is important. It is based on selecting only θ, which is a rotation parameter of the z-axis, as a calibration parameter. However, the correction values of the 1-axis coordinate system and the 5-axis coordinate system (generally n-axis) are not selected as calibration parameters because they are affected by the base coordinate system and the tool coordinate system, respectively.

And, in the case of the tool coordinate system, only a, b, and d parameters that are translation parameters of the x, y, and z axes are selected as calibration parameters.

When the calibration parameter is selected by the above process, the position of the end effector (p _i ) measured using the three-dimensional position measuring device and the position of the end effector calculated using the kinematic model of the robot according to the following Equation (6) Base coordinate system, tool coordinate system so that the error of f(q _{i ) is minimized} And calibration for correcting the kinematic model of the robot is performed (S280).

Here, T _MB is the transformation matrix from the 3D position measuring device to the robot base coordinate system, T _TE is the transformation matrix from the tool coordinate system to the end effector, and p _i is the i-th end effector measured using the 3D position measuring device. The position of f(q _i ) represents the i-th end effector position calculated using the kinematic model of the robot, respectively.

As described above, according to the present invention, by selecting a calibration parameter by calculating a gravity vector and using the calculated gravity vector as a constraint condition, it is possible to accurately correct the gravity axis when performing the calibration of the interventional robot. Accordingly, there is an effect that the accuracy of calibration can be improved.

On the other hand, although the present embodiment has been described with respect to the calibration method of the interventional robot, the present invention can also be utilized for the calibration of the robot whose gravity axis is dependent on the base coordinate system.

Up to now, the present invention has been described focusing on preferred embodiments thereof. However, the embodiments of the present invention are provided to more completely explain the present invention to those of ordinary skill in the art, and the scope of the present invention is not limited to the above embodiments, and various other Of course, it is possible to change the shape.

Claims (8)

(a) measuring the positions of a plurality of reference points on a base plane using a three-dimensional position measuring device, and calculating a gravity vector with respect to the gravity axis based on the measured positions of the reference points;
(b) The position of the end effector is measured using a three-dimensional position measuring device in various postures, and the position of the end effector is measured using the kinematic model of the robot based on the joint angle and DH parameter (Denavit-Hartenberg parameter) input from the encoder. calculating ;
(c) Estimate the base coordinate system based on the position of the end effector measured using the three-dimensional position measuring device and the position of the end effector calculated using the kinematic model of the robot, and correct the estimated base coordinate system using the gravity vector to do;
(d) selecting a calibration parameter using a gravity vector as a constraint to correct the gravity axis when performing calibration; and
(e) Calibration method of an interventional robot, characterized in that it comprises the step of performing a calibration for correcting the base coordinate system, the tool coordinate system, and the kinematic model of the robot based on the selected calibration parameter.
In claim 1,
In step (c),
A calibration method of an interventional robot, characterized in that the base coordinate system is estimated by the following <Equation 1>.
<Equation 1>

Here, T _MB is the base coordinate system, p _i is the position of the ith end effector measured using a three-dimensional position measuring device, f(q _i ) is the position of the ith end effector calculated using the kinematic model of the robot, T _B1 is the transformation matrix from the base coordinate system to the 1-axis coordinate system of the robot, T _{i -1,i} is the transformation matrix from the (i-1)-axis coordinate system of the robot to the i-axis coordinate system, T _TE is the transformation matrix from the tool coordinate system to the end effector Each of the transformation matrices of
In claim 1,
In step (c),
When correcting the estimated base coordinate system using the gravity vector,
A first step of setting any one of the x-axis, y-axis, and z-axis of the base coordinate system as the gravity axis;
A second step of matching the unit vector of the axis set as the gravity axis to the gravity vector;
A third step of obtaining a rotation matrix between the gravity vector and the unit vector of the axis set as the gravity axis;
and a fourth step of correcting the estimated base coordinate system based on the rotation matrix.
In claim 3,
In the third step,
A calibration method of an interventional robot, characterized in that it obtains a rotation matrix between the gravity vector and the unit vector of the axis set as the gravity axis by the following <Equation 2>.
<Equation 2>

From here,

is the gravitational vector about the gravitational axis,

is the unit vector of the axis set as the gravitational axis,

is the gravity vector (

) and the unit vector of the axis set as the gravity axis (

) represents the rotation matrix between
In claim 3,
In the fourth step,
Calibration method of an interventional robot, characterized in that correcting the base coordinate system estimated by the following <Equation 3> based on the rotation matrix.
<Equation 3>

where T _MB is the base coordinate system,

is the gravity vector (

) and the unit vector of the axis set as the gravity axis (

), the rotation matrix, T _MB ', represents the base coordinate system calibrated using the gravitational vector.
In claim 1,
In step (d),
A calibration method of an interventional robot, characterized in that the calibration parameter is selected using a calibration parameter selection matrix using a gravity vector as a constraint according to the following <Equation 4>.
<Equation 4>

Here, S is a calibration parameter selection matrix, a, b, and d represent the translation parameters of the x, y, and z axes, and α, β, and θ represent the rotation parameters of the x, y, and z axes.
In claim 6,
In step (d),
When selecting calibration parameters,
In the case of the base coordinate system, the translation parameter of the x, y, and z axes is selected as the calibration parameter, and the rotation parameter of the gravity axis is selected as the calibration parameter. A calibration method of an interventional robot, characterized in that the rotation parameter is fixed without being selected as a calibration parameter.
In claim 1,
In step (e),
Base coordinate system and tool coordinate system by the following <Equation 5> And Calibration method of the interventional robot, characterized in that performing a calibration for correcting the kinematics model of the robot.
<Equation 5>

Here, T _MB is the transformation matrix from the 3D position measuring device to the robot base coordinate system, T _TE is the transformation matrix from the tool coordinate system to the end effector, and p _i is the i-th end effector measured using the 3D position measuring device. The position of f(q _i ) represents the i-th end-effector position calculated using the kinematic model of the robot, respectively.

Images