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

Abstract

The present invention relates to a method of calibrating an interventional procedure robot. According to the present invention, a calibration parameter is selected by using a gravity vector calculated by calculating a gravity vector with respect to a gravity axis as a constraint, So that the accuracy of the calibration can be improved.

Description

[0001] The present invention relates to a method of calibrating an interventional robot,

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

In general, robots have been developed for industrial use and have been used as part of factory automation or to perform tasks on behalf of humans in extreme environments that humans can not tolerate. These robotic engineering fields have recently been used in the most advanced space development industry and have been developed and developed into human-friendly home robots in recent years.

Particularly in the field of medical treatment, intervention is a typical minimally invasive medical technology, which is rapidly developing as a new medical technology that can be applied to a wide range of fields including diagnosis and treatment. It is a multidimensional medical treatment for needle insertion such as biopsy and high frequency / It is required to develop a robot system capable of accurately targeting lesions of the abdomen and chest organs using image matching and induction techniques.

In this case, it is essential to secure a high level of space matching technology between the imaging device and the robot and the robot calibration technology.

Generally, the position of the end effector of a robot can be calculated by inputting rotation or translation information of each joint into a robot kinematic model. However, the position of the end effector calculated using the kinematic model is different from the actual position due to the manufacturing error of the mechanism and deflection due to its own weight. To reduce this error, it is more effective to calibrate the kinematic model in software than to modify the mechanical structure or design of the robot itself, and this correction is called calibration.

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

Referring to FIG. 1, when a calibration method used in an existing industrial robot is applied to an interventional robot, a robot is moved to a plurality of positions in an operation region of the robot, and a position of the end effector is determined using a kinematic model and a joint angle The robot kinematic model, the base coordinate system and the tool coordinate system for minimizing the error between the calculated position and the actual measured position are measured numerically using a three-dimensional position measuring device Calculation can be performed.

However, since the gravity axis of the base coordinate system affects the correction value of the two axes, which is the rotation axis, in the case of the 5-DOF scalar type interventional robot as shown in FIG. 1, when the conventional industrial robot calibration technique is applied to the interventional robot, The correction of the axis is not properly performed and the accuracy of the position control of the robot is deteriorated.

[Patent Document 1] Japanese Patent Application Laid-Open No. 10-2005-0001608 (2005.01.07)

SUMMARY OF THE INVENTION The present invention has been made in order to solve the above problems, and it is an object of the present invention to improve the accuracy of calibration by correcting the gravity axis accurately during calibration of the interventional robot.

According to an aspect of the present invention, there is provided a method of calibrating an interventional robot, comprising: (a) measuring a position of a plurality of reference points on a base plane using a three-dimensional position measuring device; Calculating a gravity vector for the gravity axis as a basis; (b) measuring the position of the end effector using a three-dimensional position measuring device in various orientations, calculating an end effector position using a kinematic model of the robot based on the joint angle and the DH parameter; (c) estimating a base coordinate system based on the position of the end effector measured using the three-dimensional position measuring device and the end effector position calculated using the kinematic model of the robot, and correcting the estimated base coordinate system using the gravity vector ; (d) selecting a calibration parameter with a gravity vector as a constraint for correcting for a gravity axis at the time of calibration; And (e) performing calibration to correct the base coordinate system, the tool coordinate system, and the kinematic model of the robot based on the selected calibration parameters.

According to the present invention, by selecting a calibration parameter with the gravity vector calculated by calculating the gravity vector with respect to the gravity axis as a constraint condition, it is possible to accurately correct the gravity axis at the time of performing the calibration of the arbitration procedure robot. Accordingly, the accuracy of the calibration can be improved.

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

Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings, so that those skilled in the art can easily carry out the technical idea of the present invention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings. In the following description, well-known functions or constructions are not described in detail since they would obscure the invention in unnecessary detail.

Before describing the present invention, the basic concept of the present invention will be described briefly to facilitate understanding of the present invention.

As described above, since the gravity axis of the base coordinate system affects the correction value of the two axes in the case of the 5-degree-of-freedom scalar type interventional robot, if the existing industrial robot calibration technique is applied to the interventional robot as it is, The accuracy of the position control of the robot is deteriorated.

For this purpose, in the present invention, by selecting a calibration parameter by using a gravity vector calculated by calculating a gravity vector with respect to a gravity axis as a constraint, it is possible to accurately correct the gravity axis at the time of performing the calibration of the arbitration procedure robot. May be more clearly understood through the embodiments described below.

FIG. 2 is a flowchart illustrating a method of calibrating an interventional procedure robot according to an embodiment of the present invention. FIGS. 3 to 7 illustrate a calibration method of an interventional procedure robot according to an embodiment of the present invention.

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

Here, it is preferable that a plurality of reference points on the base plane are at least five or more.

Next, as shown in FIG. 4, the position (p _i ) of the end effector is measured by using a three-dimensional position measuring device in various poses 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 a transformation matrix from the coordinate system to the base coordinate system of the three-dimensional position measuring apparatus, T _B1 is a transformation matrix from the base coordinate system to the one-axis coordinate system of the robot, T _{i -1,} ) The transformation matrix from the axis coordinate system to the i-axis coordinate system, and 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 the transformation matrix between two coordinate systems when there are any two successive link coordinate systems of the robot link, as shown in FIG. 5, and x, y, z The conversion matrix can be expressed using the translation parameters (a, b, d) and rotation parameters (?,?,?) Of the axis. T _MB and T _TE are generally constants, As shown in Fig.

Next, based on the position (p _i ) of the end effector measured by the three-dimensional position measuring device and the end effector position f (q _i ) calculated using the kinematic model of the robot, The coordinate system T _MB is estimated (S250).

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

And T _MB represents an estimated base coordinate system.

Next, the base coordinate system estimated using the gravity vector is corrected (S260), and a more detailed description will be given below.

)를 중력 벡터(

)에 일치시킨다.First, as shown in FIG. 6, one of axes (for example, x-axis) of the x-axis, y-axis and z-axis of the base coordinate system is set as the gravitational axis and the unit vector of the axis set as the gravitational axis in the base coordinate system

) As a gravity vector (

).

)를 중력 벡터(

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

) As a gravity vector (

), As shown in Fig.

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

)간의 회전 행렬

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

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

) Rotation matrix

Is obtained by the following equation (3).

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

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

은 중력 벡터(

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

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

Is the gravity vector for 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 (

), Respectively.

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

(T _MB ') by correcting the base coordinate system (T _MB ) based on the following equation (4).

은 중력 벡터(

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

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

Is the gravity vector (

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

), And T _MB 'represents a base coordinate system corrected using gravity vectors, respectively.

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

(S270). The calibration parameter selection matrix S is used as a constraint.

Where 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.

If the value of the 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 it is 0, the parameter of the corresponding coordinate system is fixed without being selected as the calibration parameter. 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 y parameter (β _B ) It means that it is not selected and fixed.

The selection of the calibration parameters 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, and the x, y, and z axis translation parameters are selected as the calibration parameters. Then, set to x, y, 1 a rotational parameter (for example, α _B) of the z-axis rotational parameter, α _B, β _B, θ _B gravity axis (e.g., x-axis) in and select a calibration parameter.

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

) Is set as a constraint, and rotation parameters (for example,? _B ,? _B ) of the remaining axes are set to 0, and fixed without selecting as calibration parameters.

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

) Is set as a constraint, and the rotation parameters of the axes other than the gravity axis are fixed is obtained by calibrating the axes other than the gravity axes (for example, x-axis) z axis), the gravity axis is rotated together. Therefore, the rotation parameter of the axis other than the gravity axis is fixed to 0 so that the gravity axis is not rotated.

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

) As a constraint, and selects a calibration parameter.

Next, considering the characteristics of medical robots in which safety is important in the case of one axis coordinate system or n axis coordinate system, considering the fact that the cause of the error of the joint angle is larger than the guarantee of the analytical inverse kinematic solution and the other manufacturing errors, And only the rotation parameter θ of the z-axis is selected as a calibration parameter. However, the correction values of the 1-axis and 5-axis (usually n-axis) coordinate systems are not selected as calibration parameters because they are affected by the base coordinate system and the tool coordinate system, respectively.

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

When the calibration parameter is selected by the above process, the position (p _i ) of the end effector measured using the three-dimensional position measuring device according to the following equation (6) and the end effector position calculated using the kinematic model of the robot and the error of f (q _i ) is minimized, a base coordinate system, a tool coordinate system And a calibration to correct the kinematic model of the robot is performed (S280).

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

As described above, according to the present invention, by selecting the calibration parameters with the gravity vector calculated by calculating the gravity vector as the constraint condition, it is possible to accurately correct the gravity axis at the time of calibration of the interventional robot. Accordingly, the accuracy of the calibration can be improved.

In the meantime, although the method of calibrating the interventional robot is described in the present embodiment, the present invention can be applied to the calibration of the robot whose gravity axis depends on the base coordinate system.

The preferred embodiments of the present invention have been described above. It is to be understood, however, that the invention is not limited to the disclosed embodiments, but, on the contrary, is intended to cover various modifications and alternative arrangements included within the spirit and scope of the appended claims. Of course.

Claims (8)

(a) measuring a position 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 position of the measured reference point;
(b) measuring the position of the end effector using a three-dimensional position measuring device in various orientations, calculating an end effector position using a kinematic model of the robot based on the joint angle and the DH parameter;
(c) estimating a base coordinate system based on the position of the end effector measured using the three-dimensional position measuring device and the end effector position calculated using the kinematic model of the robot, and correcting the estimated base coordinate system using the gravity vector ;
(d) selecting a calibration parameter with a gravity vector as a constraint for correcting for a gravity axis at the time of calibration; And
(e) performing a calibration to correct 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 the step (c)
Wherein the base coordinate system is estimated by the following expression (1).
<Formula 1>

Where T _MB is the base coordinate system, p _i is the position of the i-th 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 robot's 1-axis coordinate system, T _{i -1, i} is the transformation matrix from the (i-1) axis coordinate system to the i axis coordinate system of the robot, T _TE is the transformation _coefficient from the tool coordinate system to the end effector Respectively.
In claim 1,
In the step (c)
When the estimated base coordinate system is corrected using the gravity vector,
A first step of setting one of an x-axis, a y-axis and a z-axis of the base coordinate system as a gravity axis;
A second step of matching the unit vector of the axis set as the gravity axis with the gravity vector,
A third step of obtaining a rotation matrix between a gravity vector and a unit vector of an axis set by 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 rotation matrix between a gravity vector and a unit vector of an axis set by a gravitational axis is obtained by the following Equation (2).
<Formula 2>

From here,

Is the gravity vector for the gravitational axis,

Is a unit vector of the axis set as the gravity axis,

Is the gravity vector (

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

), Respectively.
In claim 3,
In the fourth step,
And the base coordinate system estimated by the following equation (3) is corrected on the basis of the rotation matrix.
<Formula 3>

Here, T _MB is the base coordinate system,

Is the gravity vector (

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

), And T _MB 'represents the base coordinate system corrected using the gravity vector, respectively.
In claim 1,
In the step (d)
And a calibration parameter selection matrix is selected using a calibration parameter selection matrix having a gravity vector as a constraint condition according to the following equation (4).
<Formula 4>

Where a, b, and d represent the translation parameters of the x, y, and z axes, and alpha, beta, and θ represent the rotation parameters of the x, y, and z axes.
In claim 6,
In the step (d)
When selecting the calibration parameters,
In the case of the base coordinate system, the rotation parameter of the x, y, z axis is selected as the calibration parameter, and the rotation parameter of the gravity axis is selected as the calibration parameter so that the gravity axis is not rotated, Wherein the rotation parameter is fixed without being selected as a calibration parameter.
In claim 1,
In the step (e)
Based on the following Equation 5, the base coordinate system, the tool coordinate system And a calibration for correcting the kinematic model of the robot is performed.
&Lt; EMI ID =

Here, T _MB is a transformation matrix from the 3D position measuring apparatus to the robot base coordinate system, T _TE is a transformation matrix from the tool coordinate system to the end effector, p _i is an i th end effector the location, f (q _i) represents each of the i-th end effector position calculated from the kinematic model of the robot.

Images