JPH06304893A - Calibration system for positioning mechanism - Google Patents

Abstract

PURPOSE:To easily perform calibration of a parameter related to coordinate transformation in a positioning mechanism. CONSTITUTION:A calibration jig 20 is set up in an arbitrary position in the periphery of a robot 10, to actuate the robot 10 by operating a teaching operation panel 19 by an operator, and a tool 12 in the point end approaches relating to each fixed jig 21 to 24 of the calibration jig 20, to teach a touch up point. This approach is performed in a direction toward a center position of the fixed jig with the point end spherically formed. Successively, automatic touch up by impedance control is performed, and by using each axis value or the like then obtained, the spherical center is calculated to further obtain a calibration data 6B. Thus by only touching up the fixed jigs 21 to 24 by the tool 12, thereafter calculation is automatically performed, to obtain the calibration data DELTAbeta. Accordingly, no particular skill for performing calibration is required for the operator.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a calibration system of a positioning mechanism for calibrating parameters relating to coordinate conversion in the positioning mechanism.

[0002]

2. Description of the Related Art An example of a positioning mechanism is an industrial robot. The hand position / posture of the robot is calculated by sequentially calculating the position of the coordinate system fixed for each link from the movement amount of the actuator. The parameter indicating the relationship between the origin position of the actuator and the coordinate system usually includes an error that occurs at the time of mounting with respect to design data. Therefore, parameters are calibrated in order to accurately control the hand position / posture.

As a method of calibrating this parameter, there is, for example, the following method. That is, a convex jig is attached to the tip of the robot, a concave jig and a dial gauge are attached to the base side of the robot, and the robot is placed in a predetermined position and posture so that the convex jig fits into the concave jig. Let it take. Assuming that the positioned position is known, each axis value of the robot can be calculated. Therefore, assuming that the reading of the movement amount sensor attached to each axis at that time is equal to the calculated value, the offset of each axis value is obtained.

[0004]

However, in the above-described conventional calibration method, it is necessary to manually move the robot to a specific position, which requires skill.

Of the parameters related to coordinate conversion,
Only the origin position of the actuator (servo motor) could be known. The present invention has been made in view of such a point, and an object thereof is to provide a calibration method of a positioning mechanism that can easily perform calibration with respect to any parameter related to coordinate conversion.

[0006]

According to the present invention, in order to solve the above problems, in a calibration method of a positioning mechanism for calibrating parameters relating to coordinate conversion in the positioning mechanism, a tip provided at the tip of the positioning mechanism. For calibration with a tool having a convex curved surface shape, a force sensor for detecting the force acting on the tool, and three or more fixing jigs having a spherical tip shape and known relative positions The jig and the tool are approached and brought into contact with each fixing jig of the calibration jig, and the force is controlled based on the detection result of the force sensor to position the fixing member with respect to the fixing member.
A calibration unit for a positioning mechanism, comprising: a control unit that calculates an error of a parameter relating to coordinate conversion of the positioning mechanism as calibration data by using each joint angle in each posture of the tool when the positioning is performed. Method is provided.

[0007]

Function: A force sensor and a convex curved tool whose tip shape is known are attached to the tip of the positioning mechanism. By force-controlling the positioning mechanism, the tool is positioned with respect to each of the three or more spherical fixing jigs whose relative positions are known so that the tool takes several kinds of postures. Using each axis value at that time, calibration data of a parameter relating to coordinate conversion of the positioning mechanism is obtained.

[0008]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 1 is a diagram showing an overall configuration of a calibration system of a positioning mechanism of the present invention. Here, a robot is used as the positioning mechanism. In the figure, a robot 10 is a 6-axis articulated robot, and a force sensor 11 and a tool 12 are attached to its tip. The tip of the tool 12 is formed into a convex curved surface.

A calibration jig 20 is placed around the robot 10. Calibration jig 20
Is provided with four fixing jigs 21, 22, 23 and 24. Each of these fixing jigs 21 to 24 has a spherical tip, and the center position and relative positional relationship of each sphere are known.

The robot controller 100 includes a processor 1
01 mainly controls the operation of the robot 10. The processor 101 controls the entire robot controller 100 according to a control program stored in the memory 102. A program for executing the present invention is also stored in the memory 102, and the processor 10
1 executes the calibration according to the present invention according to the program. The details will be described later.

The memory 102 is composed of a ROM or a RAM and stores the above control program, a program for executing the present invention, and various data. Among the various data, there are data relating to the tip shape of the tool 12 described above, design data relating to the calibration jig 20, center position data and shape data of the fixing jigs 21 to 24, relative positional relationship data, and the like. included. The processor 101 and the memory 102 are coupled to each other via a bus 108.

The bus 108 further includes a D / A converter 10
4, the A / D converter 106, the input / output circuit 107, and the serial port 109 are connected. D / A converter 104
Is the robot 10 in response to a command from the processor 101.
A drive signal for driving the actuator (servo motor) of each axis is output via the amplifier 103. The A / D converter 106 receives a rotation angle detection signal from an encoder provided on each axis of the robot 10 via the amplifier 105. The input / output circuit 107 also receives the detection signal of the force sensor 11 via the sensor controller 110.
Further, the serial port 109 receives a command signal when the operator operates the teaching operation panel (TP) 19 connected to the serial port 109. Robot 10
Receives the command signal and performs a predetermined operation.

Next, the procedure for executing the calibration method of the positioning mechanism of the present invention will be described. First,
The robot 10 is simply mastered. That is,
At the time of start, since it is not known which angle each axis of the robot 10 has, the operator operates the teaching operation panel 19 so that the robot 10 takes a posture in which the angle of each axis is approximately 0 °. Find the approximate offset amount of. Rough motion control of the robot 10 is performed by correcting the angle of each axis with the offset amount.

Next, the calibration jig 20 is installed at an arbitrary position around the robot 10. The operator operates the teaching operation panel 19 to operate the robot 10 to move the tool 12 at the tip to the calibration jig 20.
The respective fixing jigs 21 to 24 are approached. This approach is performed in the direction toward the center position of the fixing jig having a spherical tip.

Further, one fixing jig may be approached from several directions, and the total number of approaches (touch-up points) m (3 × m) is a parameter to be calibrated. It is decided to be equal to or more than the number. For example, if there are 5 parameters to be calibrated on each axis, there will be 30 on all 6 axes, so at least 30 equations are required to determine the calibration data for these parameters. On the other hand, since three expressions are satisfied by one approach, the number of approaches required in this case m
Is at least 10 times. Details of the parameters to be calibrated and the method of determining the calibration data will be described later.

The calibration jig 20 is provided with four fixing jigs 21 to 24 here, but at least three fixing jigs are required. This is to specify the spatial position of the calibration jig 20 by calculation. That is, the spatial position of the calibration jig 20 is obtained by calculating based on each axis value when the tool 12 is approached and positioned with respect to the fixing jig 21 or the like, and the spatial position is the tool 12 Can be specified only after approaching at least three fixing jigs 21 or the like. The plane is determined by the at least three fixing jigs, and the spatial position is also determined by the plane. Here, four fixing jigs 21 to
Although the tool 12 approaches 24, the spatial position of the calibration jig 20 can be specified with higher accuracy than when the tool 12 approaches only three fixing jigs.

Next, the parameters to be calibrated and the coordinate system set for each axis will be described. The element between the adjacent axes of the robot 10 is called a link, the coordinate system is fixed to the link, and the position and orientation of the robot 10 are expressed by sequentially converting from the coordinate system to the coordinate system. Here, the DH notation is used as one of the methods for describing the relationship between link coordinate systems. The link parameters when using the DH notation are as shown in FIG. 2, and four parameters θ _i , L _i , a _i, and α _i are set. That is, the rotation angle θ _{i of} the rotation axis i, the link L _i driven by the rotation axis i, and the link length a between the rotation axis i and the rotation axis i + 1.
_i and the rotation angle α _i around the X _i axis from the Z _i-1 axis to the Z _i axis. Parameters to be calibrated (parameters regarding coordinate conversion) are obtained by adding the offset value φ _i of θ _i generated when the servo motor is attached to these four parameters θ _i , L _i , a _i and α _i. There are a total of 5 parameters, and these will be collectively referred to as link parameters.

[0018] In this case, the transformation matrix ^i-1 indicating the position and orientation of the link coordinate system sigma _i viewed from the link coordinate system sigma _i-1
A _i is represented by the following equation (1).

[0019]

[Equation 1]

Here, a base coordinate system Σ _B is provided on the base of the robot 10 having n joints, and a tool 1 at the tip of the robot 10 is used.
Consider the tool coordinate system Σ _{T in 2} and consider the transformation from the base coordinate system Σ _B to the tool coordinate system Σ _T. In the case of the robot 10, n = 6. Base coordinate system Σ _B = Σ _0, when considered as the tool coordinate system _{Σ T = Σ n, Σ B} → Σ homogeneous transformation matrix ^B T _T indicating the coordinate transformation _T, the above equation (1) of the ^i-1 A _i
Can be expressed by the following equation (2).

[0021]

[Equation 2]

Here, it is assumed that the tool 12 sequentially approaches the four balls (fixing jigs) 21 to 24.
It is assumed that this approach is automatically performed according to the operation procedure that the operator has previously taught to the robot 10 using the teaching operation panel 19, and the j-th approach is performed on the k-th sphere. After the tool 12 approaches and touches up (contacts) the k-th sphere, impedance control is used based on the detection value of the force sensor 11 to stabilize the force acting on the sphere to a constant value.

The approach direction of the tool 12 is a direction toward the center of the sphere, and if the approach direction is the X-axis direction of the sensor coordinate system (force sensor coordinate system) Σ _S , the following equations (3), ( 4) and (5) are established.

[0024]

[Equation 3]

[0025]

[Equation 4]

[0026]

[Equation 5]

However, K _f : 6 × 6 force gain f _d : Target external force in work coordinate system Σ _W K _d : Set spring constant matrix Parameters other than equations (3), (4) and (5), set inertia matrix The M _d and the set viscosity coefficient matrix D _d are experimentally determined depending on the mechanism, the work (fixing jig), the tool, etc. so that the position, force, etc. can be stably controlled. The position target is the current position when the contact is started, and the acceleration and velocity targets may be zero. At this time, when the reaction force f that the tool 12 actually receives from the sphere and the torque τ that acts on the tool 12 are viewed from the sensor coordinate system Σ _S ,

[0028]

[Equation 6]

[0029]

[Equation 7]

Then, where ^S f _x = f _xd ,
It is in a statically stable equilibrium state. Actually, as an index for stabilization judgment,

[0031]

[Equation 8]

Is introduced, and | e _f |
It is considered to be stabilized when e _f | <e _t is satisfied.
When the tool 12 and the ball is in point contact, the contact point ^S r _c as viewed from the sensor coordinate system sigma _S, the following equation between the ^S _f, ^{S τ} (9) is satisfied.

[0033]

[Equation 9]

The tip shape of the tool 12 is a known convex curved surface, and an equation representing the tip shape is expressed on the sensor coordinate system Σ _S by

[0035]

[Equation 10]

Then, the following equation (11) in which ^S r _c of the equation (9) is replaced by ^S x

[0037]

[Equation 11]

Is a straight line equation,
By solving equations (11) simultaneously,^Sr _cIs required. This
When, the center position of the sphere^Sr is given by the following expression (12).

[0039]

[Equation 12]

However, r is the radius of the sphere. Thus, the detection value of the force sensor when the touch-up tool 12 to a sphere, the center position ^S r _c of the sphere with the data such as the known tip shape of the tool 12 is obtained.

Here, the work coordinate system fixed to the calibration jig 20 is Σ _W, and the homogeneous transformation matrix showing the transformation from the work coordinate system Σ _W to the base coordinate system Σ _B is ^WT.
_B, and homogeneous transformation matrix indicating a transformation from the tool coordinate system sigma _T to the sensor coordinate system sigma _S and ^T T _S. It is assumed that the j-th touch-up is performed on the k-th sphere. By the impedance control, each axis value of the robot 10 in a state where static constant force contact is performed is θ _i , and the sphere center position viewed from the sensor coordinate system Σ _S is the point contact as shown in the above formula (12). If it is possible to calculate ^S r _j from the force, the k-th sphere center position ^w P processed as known from the workpiece coordinate system Σ _W
k can be calculated as in the following equation (13).

[0042]

[Equation 13]

The right side of the equation (13) can be calculated by the vector Ψ that represents the translational movement amount representing the ^W T _B and the posture, and the link parameters a, d, α, and φ. Set these parameters to 1
One vector,

[0044]

[Equation 14]

Then, the equation (13) can be rewritten as the following equation (15).

[0046]

[Equation 15]

[0047] (15) is satisfied when beta is true value, because the true value is unknown, taking its initial value to its beta section of the primary and developed with ₀ as beta _0,

[0048]

[Equation 16]

It becomes The formula (16) holds true for the touch-up point j and the sphere k, but if the formula (16) is obtained for all the touch-up points and the sphere and they are combined into one, the following formula (17) is obtained.

[0050]

[Equation 17]

Here, the number of touch-up points is m, the number of parameters to be calibrated is N, and the difference Δβ from the initial value β ₀ of the vector β is calculated as follows. This difference Δβ becomes the calibration data of the parameter. Note that, as described above, N is 30 for 6 axes if the number of parameters to be calibrated on one axis is 5, for example. (I) When 3m = N, A is regular, so

[0052]

[Equation 18]

It becomes (Ii) When 3m> N, Δβ is calculated by the method of least squares. In particular,

[0054]

[Formula 19]

It becomes The cutoff value t of Δβ is set in advance, and when | Δβ | ≦ t, the parameter estimation ends. When | Δβ |> t, β ₀ + Δβ is set as a new β ₀ , and the calculation of formula (18) or formula (19) is repeated.

The parameters θ _i , L _i , a _i and α _i
In some cases, it may be desirable to target only a part of Φ _i and Φ _i , not all. Also, the parameters may not be independent of the features of the positioning mechanism. In this case, the parameters to be reduced and the parameters that are not independent may be removed from the equation (14), and the following may be similarly calculated.

For example, the parameter a_i, D_i, Α_iVs
If it is an elephant, the value of each axis can be calculated from the tip position of the robot 10.
Since the required calculation becomes complicated, consider the required accuracy, etc.
These parameters a_i, D_i, Α_iExclude from the target
Good. Also, the parameter φ _iOut of φ₀Is independent of Ψ
It is clear that there is no. Considering these, (1
Equation (4) can be simplified as the following equation (20),
The calculation speed can be increased accordingly.

[0058]

[Equation 20]

However, n: degree of freedom FIG. 3 is a diagram showing a schematic processing procedure of the present invention. S in the figure
The number following is a step number. [S1] Simple mastering of the robot 10 is performed. [S2] The calibration jig 20 is installed. [S3] The teaching operation panel 19 is operated to operate the robot 10 to teach the approach point. [S4] Automatic touch-up by impedance control and calculation of the sphere center are performed. The details will be described with reference to FIG. [S5] The calibration data Δβ is obtained. The details will be described with reference to FIG.

FIG. 4 shows an automatic tally of the processing procedure of the present invention.
It is a figure which shows the processing procedure of a cheek and a sphere center calculation. In the figure
The number following S represents a step number. [S11] The touch-up number j is set. [S12] Move to the j-th touch-up point. [S13] Touch up by impedance control
U [S14] The contact between the tool 12 and the ball is determined. Sanawa
Then, the tool 12 obtained from the detection value of the force sensor 11 is a sphere.
The reaction force f received from is a predetermined judgment value f_tGreater than or not
If it is larger, it is judged that it is in contact and the next step
Go to step S15, otherwise approach further
To return to step S13. [S15] The contact is stabilized by impedance control.
Let [S16] It is determined whether the contact is stable. Sanawa
Stability judgment index | e_f| Is a predetermined time T
Judgment value e_tDetermine whether or not the condition is smaller
Separately, it is determined that the contact is stable if it remains small.
Separately, proceed to the next step S17, and if not, further
In order to stabilize by impedance control, step S1
Return to 5. [S17] Reaction force that the tool 12 receives from the ball^Sf and two
Torque acting on Le 12^STake in τ. [S18] Equation (7), Equation (8), Equation (9), and (10)
From the formula to the center of the sphere^Sr _jIs calculated and stored. [S19] Move to the next touch-up point j + 1. [S20] Touch-up point j is a predetermined number of touch-up points
It is determined whether or not it is larger than m. If all
It ’s finished as it is, and it ’s not.
If so, the process returns to step S12.

FIG. 5 is a diagram showing a calibration data Δβ calculation processing procedure in the processing procedure of the present invention.
The numbers following S in the figure represent step numbers. [S31] A and b in equation (17) are calculated. [S32] It is determined whether 3m is larger than N. If so, go to step S35; otherwise, go to step S3
Go to 3 respectively. [S33] It is determined whether 3m is equal to N. If they are equal, go to step S36; otherwise, go to step S34.
To each. [S34] An error is displayed. [S35] Δβ is calculated using the equation (19). [S36] Δβ is calculated using the equation (18). [S37] (β ₀ + Δβ) is set as a new initial value. [S38] It is determined whether | Δβ | is equal to or less than the cutoff value t. When | Δβ | ≦ t, the parameter estimation is terminated, and Δβ at that time is used as the calibration data. If | Δβ |> t, the process returns to step S31.

As described above, in the present embodiment, the difference Δβ from the initial value β ₀ of the vector β caused by the error of each of the five parameters is set to the spherical fixing jigs 21 to 24 for the tool 12. Each axis value obtained by touching up, the detection value of the force sensor 11, data of the center position of the work coordinate system Σ _W of the sphere that is known in advance, and the like are used, and the parameter Δ is calibrated by the difference Δβ. I do.
The difference Δβ is obtained by simply touching up the tool 12 on the fixing jigs 21 to 24 and then automatically calculating the difference. Therefore, the operator does not need any special skill to perform the calibration, and can easily perform the calibration.

Although a six-axis robot is used as the positioning mechanism in the above description, a robot other than the six-axis robot may be used. Further, it may be configured as a manipulator.

Further, although each axis is configured as a rotation axis,
The present invention can be similarly applied to a linear drive shaft.

[0065]

As described above, in the present invention, the calibration data of the parameter relating to the coordinate conversion is obtained only by touching up the tool on the fixing jig. Therefore, the operator does not need any special skill to perform the calibration, and can easily perform the calibration.

[Brief description of drawings]

FIG. 1 is a diagram showing an overall configuration of a calibration system of a positioning mechanism of the present invention.

FIG. 2 is an explanatory diagram of link parameters and a link coordinate system when the DH notation is used.

FIG. 3 is a diagram showing a schematic processing procedure of the present invention.

FIG. 4 is a diagram showing a processing procedure of automatic touch-up and sphere center calculation among processing procedures of the present invention.

FIG. 5 is a diagram showing a processing procedure of calibration data Δβ calculation in the processing procedure of the present invention.

[Explanation of symbols]

 10 robot 11 force sensor 12 tool 19 teaching operation panel 20 calibration jig 21, 22, 23, 24 fixing jig 100 robot controller 101 processor 102 memory 104 D / A converter 105 A / D converter 109 serial Port 110 sensor controller

Claims (4)

[Claims] 1. A calibration method for a positioning mechanism, which calibrates parameters relating to coordinate conversion in a positioning mechanism, comprising: a tool provided at a tip of the positioning mechanism; A force sensor for detecting a force acting on the tool, a calibration jig having three or more fixing jigs each having a spherical tip shape and known relative positions, and the tool for each of the calibration jigs. Positioning is performed with respect to the fixing member by approaching and bringing into contact with the fixing jig and controlling the force based on the detection result of the force sensor, and using each joint angle in each posture of the tool when the positioning is performed. A control unit for calculating an error of a parameter relating to coordinate conversion of the positioning mechanism to obtain calibration data; Calibration method of the positioning mechanism, characterized by. 2. The control unit, the center position of each fixing jig in a work coordinate system obtained in advance, the detection result of the force sensor when the positioning is performed, and each joint in each posture when the positioning is performed. 2. The calibration system for a positioning mechanism according to claim 1, wherein calibration data of a parameter relating to the coordinate conversion is obtained from an error from a center position of each fixing jig obtained from an angle. 3. The control unit specifies the spatial position of the calibration jig by approaching at least three of the three or more fixing jigs. Positioning mechanism calibration method. 4. The parameters relating to the coordinate transformation include a rotation angle θ _{i of} a rotation axis i, a link L _i driven by the rotation axis i, a link length a _i , Z between the rotation axis i and the rotation axis i + 1.
The positioning according to claim 1, which is all or part of a rotation angle α _i around the X _i axis from the _i-1 axis to the Z _i axis and an offset value φ _i of the rotation angle θ _i. Mechanism calibration method.