JP3263175B2 - Installation error calibration method and calibration control device for industrial robot system - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an installation error calibration method and a calibration control device for an industrial robot system.

[0002]

2. Description of the Related Art Conventionally, a robot manipulator (hereinafter, referred to as a robot) that grips a work tool and controls the position and orientation of the tool (hereinafter, referred to as position and orientation).
An industrial robot system is constructed using a one-axis slider, which is a moving mechanism device that mounts a robot, a positioner, which is a work handling device that mounts a work to be worked and controls the posture of the work, and a control device that controls them. Make up,
It is known to control the position and orientation of a tool by a cooperative operation of operating both a robot and a slider by using a stored computer program method. Here, the coordinate system for each machine set in the control device is generally set such that the direction of the drive axis of the slider and the direction of the coordinate axes of the machine coordinate system of the robot match.

[0003]

However, in an actual robot system, the direction of the drive axis of the slider and the direction of the coordinate axis of the robot's mechanical coordinate system are often accurate due to an installation error when the robot is mounted on the slider. There may be cases where they do not match. In an actual robot system having the installation error, when both the robot and the slider are operated cooperatively, the relative speed of the tool with respect to the workpiece cannot be constant, and the trajectory of the working tool is determined in advance. When applied to a welding operation or the like out of the trajectory of the desired tool, there is a problem that a desired welding result cannot be obtained, for example, the quality of welding is significantly reduced.

In order to solve the above problems, a method of calibrating the coordinates of each machine (hereinafter referred to as a conventional method).
Is disclosed, for example, in JP-A-64-8408. In this method, a homogeneous transformation matrix unique to each of a robot system and a visual device that controls the operation of a robot by combining data represented by each of a plurality of coordinate systems is set, and the position and orientation of a workpiece are set by the visual device. The amount of deviation is detected, and calibration between the coordinate systems is performed between the robot and the visual device.

However, the conventional method has a problem that the use of a visual device increases the cost of the entire system. In addition, at the end of the detailed description of the invention in the specification of the direction, at the end of the description, "a peripheral device such as a robot and a positioner, a robot and other robots, and an industrial A method for calibrating between coordinate systems between a robot and a visual device can be similarly applied to a robot system or the like, "but the specific means for realizing the method is not described, and the method is not described. Is unknown. In other words, according to this conventional method,
It can be applied only to a robot alone, but cannot be applied to a robot system composed of multiple machines.

SUMMARY OF THE INVENTION An object of the present invention is to solve the above problems and to calibrate an installation error such as mounting in an industrial robot system including a tool moving device for holding a tool and a work handling device for mounting a work. It is an object of the present invention to provide a method of calibrating an installation error of an industrial robot system and a calibration control device thereof.

[0007]

[0008]

According to a first aspect of the present invention, there is provided a method for calibrating an installation error of an industrial robot system, comprising: a tool for performing a predetermined operation; and a first tool for moving the tool.
A method for calibrating an installation error of an industrial robot system comprising a tool moving device including a joint means, a work as an object to be worked, and a work handling device including a second joint means for moving the work. Controlling the first joint means so that the position of the tool is the origin of the work base coordinate system set for the work, and controlling the position of the tool before and after the control of the first joint means. A first step of measuring each distance between a rectangular parallelepiped block placed such that one vertex thereof coincides with the origin of the work base coordinate system, and a distance measured in the first step. The position error of the work base coordinate system with respect to the predetermined absolute coordinate system is calculated based on the absolute coordinate system, and the position of the work base coordinate system with reference to the absolute coordinate system is calculated based on the calculated error. And calibrating the second joint means so as to move the position of the tool from a first predetermined position to a second predetermined position. A third measuring the distance between the tip of the tool and the second joint means before and after the control;
Calculating the amount of displacement of the tool based on the distances measured in the third step and calculating the error of the position and orientation of the work base coordinate system with respect to the absolute coordinate system, And a fourth step of calibrating the position and orientation of the work base coordinate system based on the absolute coordinate system based on the calculated error.

According to a second aspect of the present invention, there is provided a method for calibrating an installation error of an industrial robot system, wherein the tool moving device moves the first joint means along a predetermined axial direction. The method further comprises: moving the first joint means from a predetermined third position to a predetermined fourth position along the predetermined axial direction. Controlling the third joint means, and using a distance measuring means provided at the tip of the tool via a jig, the respective surfaces before and after the control of the third joint means are adjusted to the absolute coordinates. A fifth step of measuring each distance to each surface of the rectangular parallelepiped block placed so as to be parallel to each axis direction of the system; and, based on each distance measured in the fifth step, On an absolute coordinate system The torsion angle of the position and orientation of the tool base coordinate system set in the tool is calculated, and the position and orientation of the tool base coordinate system set in the tool based on the absolute coordinate system is calibrated based on the calculated torsion angle. A first step of moving the first joint means from a predetermined fifth position along the predetermined axial direction under the condition that the position and orientation of the tool with respect to the workpiece are constant. Controlling the first joint means and the third joint means so as to move them to a predetermined sixth position, so that the tool before and after the control of the first joint means and the third joint means; A seventh step of measuring each distance between the tip of the object and a predetermined solid position; and, based on each distance measured in the seventh step, the third joint means with respect to the predetermined axial direction. Above toolet Calculating the torsion angle of the tool base coordinate system with respect to the predetermined axial direction of the third joint means based on the calculated torsion angle. It is characterized by.

[0010]

According to a third aspect of the present invention, there is provided an installation error calibration control device for an industrial robot system, comprising: a tool for performing a predetermined operation; and a tool moving device including first joint means for moving the tool. In an industrial robot system including a work which is a work and a work handling device including a second joint means for moving the work, the position of the tool is defined by a work base coordinate system set in the work. A first control means for controlling the first joint means so as to be an origin; a first control means provided at a tip of the first joint means, and a tip of the tool before and after the operation of the first control means; First measuring means for measuring each distance between a rectangular parallelepiped block placed so that one vertex thereof coincides with the origin of the work base coordinate system; and measuring by the first measuring means. Based on the obtained distances, an error of the position of the work base coordinate system based on the predetermined absolute coordinate system is calculated, and based on the calculated error, the position error of the work base coordinate system based on the absolute coordinate system is calculated. First calibration means for calibrating the position, second control means for controlling the second joint means so as to move the position of the tool from a predetermined first position to a predetermined second position, A second measuring means provided at a tip of the first joint means for measuring each distance between the tip of the tool and the second joint means before and after the operation of the second control means; Based on the distances measured by the second measuring means, the amount of displacement of the tool is calculated, and the error of the position and orientation of the work base coordinate system with respect to a predetermined absolute coordinate system is calculated. Absolute seat based on the error Characterized in that a second calibration means for calibrating the position and orientation of the work base coordinate system based on the system.

According to a fourth aspect of the present invention, there is provided an industrial robot system installation error calibration control device, wherein the tool moving device moves the first joint means along a predetermined axial direction. The apparatus further includes third joint means for moving, and the installation error calibration control device further includes the first joint means.
Third control means for controlling the third joint means so as to move the joint means from a predetermined third position to a predetermined fourth position along the predetermined axial direction, and a tip of the tool And a surface of a rectangular parallelepiped block placed so that each surface before and after the control of the third joint means is parallel to each axial direction of the absolute coordinate system. A third measuring means for measuring each distance, and a position and orientation of a tool base coordinate system set in the tool based on the absolute coordinate system based on each distance measured by the third measuring means. Third calibration means for calculating a torsion angle, and for calibrating the position and orientation of a tool base coordinate system set in the tool based on the absolute coordinate system based on the calculated torsion angle; Tool position Under conditions constant, the first
Controlling the first joint means and the third joint means to move the joint means from a predetermined fifth position to a predetermined sixth position along the predetermined axial direction. Control means; fourth measuring means for measuring each distance between the tip of the tool and a predetermined solid position; and third joint based on each distance measured by the fourth measuring means. Calculating a torsion angle of the tool base coordinate system with respect to the predetermined axis direction of the means; and calculating a torsion angle of the tool base coordinate system with respect to the predetermined axis direction of the third joint means based on the calculated torsion angle. And fourth calibration means for performing calibration.

[0013]

In the method according to the first aspect and the apparatus according to the third aspect, the first joint means is controlled so that the position of the tool becomes an origin of a work base coordinate system set for the work. Measuring the distance between the tip of the tool before and after the control of the first joint means and a rectangular parallelepiped block placed so that one vertex thereof coincides with the origin of the work base coordinate system. . Next, based on each of the measured distances, an error in the position of the work base coordinate system based on a predetermined absolute coordinate system is calculated, and based on the calculated error, the work based on the absolute coordinate system is calculated. Calibrate the position of the base coordinate system.

Further, the second position of the tool is moved from a predetermined first position to a predetermined second position.
And measuring each distance between the tip of the tool and the second joint means before and after the control of the second joint means. Next, based on the measured distances, the amount of displacement of the tool is calculated, and the position and orientation error of the work base coordinate system with respect to the absolute coordinate system is calculated, based on the calculated error. Calibrating the position and orientation of the work base coordinate system with respect to the absolute coordinate system.

A method according to claim 2 and a method according to claim 4.
In the device described, the third joint means is controlled to move the first joint means from a predetermined third position to a predetermined fourth position along the predetermined axial direction,
Using a distance measuring means provided via a jig at the tip of the tool, the surface is mounted so that each surface before and after the control of the third joint means is parallel to each axis direction of the absolute coordinate system. Each distance to each surface of the placed rectangular parallelepiped block is measured. Next, based on each of the measured distances, a torsion angle of a position and orientation of a tool base coordinate system set in the tool based on the absolute coordinate system is calculated, and the absolute angle is calculated based on the calculated torsion angle. Calibrate the position and orientation of the tool base coordinate system set for the tool with reference to the coordinate system. Further, under the condition that the position and orientation of the tool with respect to the workpiece are constant, the first
Controlling the first joint means and the third joint means so as to move the joint means from a predetermined fifth position to a predetermined sixth position along the predetermined axial direction, First
Each distance between the tip of the tool and a predetermined solid position before and after the control of the joint means and the third joint means is measured. Next, a torsion angle of the tool base coordinate system with respect to the predetermined axis direction of the third joint means is calculated based on the measured distances, and the third joint is calculated based on the calculated torsion angle. Calibrating the torsion angle of the tool base coordinate system with respect to the predetermined axial direction of the means.

[0016]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An installation error calibration control device for an industrial robot system according to an embodiment of the present invention will be described below in the following order with reference to the drawings. (1) Industrial robot system and its installation error (2) Configuration of industrial robot system and its installation error calibration control device (3) Processing of installation error calibration control device (3-1) Main routine (3-2) No. (1) Calibration process (3-3) Second calibration process (3-4) Third calibration process (3-5) Fourth calibration process (4) Teaching operation and reproduction operation (5) Modification

(1) Industrial Robot System and Its Installation Error FIG. 1 shows the entirety of the industrial robot system of this embodiment and its installation error calibration control device (hereinafter, referred to as a control device) 5, and FIG. FIG.

In the industrial robot system according to the present embodiment, as shown in FIG. 1, a tool moving device 11, a work handling device 12, and a control device 5 are broadly divided.
The tool moving device 11 comprises, for example, a robot 1 having six degrees of freedom which grips a tool 4 which is a welding torch and controls its position and orientation, and a slider 2 which is a moving mechanism device equipped with the robot. The device 12 includes a positioner 3 on which a work (not shown) as an object to be worked is placed. (C) The control device 5 operates the two devices 11 and 12 during a teaching operation. Teaching box 20 for the operator to input data to create a work program, and an operation manually performed by the operator to switch between teaching work and automatic driving work or to output a start signal at the time of automatic driving work. The box 21 is connected to three distance sensors SS1 to SS3 each for measuring a predetermined distance as described in detail later. The position of the coordinate system between each machine produced,
Before operating the robot system, that is, before a teaching operation or a reproducing operation, a control device for calibrating and accurately cooperating these machines is provided.

Further, in the tool moving device 11, the slider 2 on which the robot 1 is mounted is cascaded between its mechanical origin Os and the mechanical origin Om of the robot 1 as shown in FIGS. A first axis which is a direction parallel to the link L11 (the first axis is parallel to the link L11 and is denoted by L1
Let it be 1. ) And the link L11
The robot 1 moves along a second axis which is a direction parallel to the link L12 and is parallel to the link L12 (the second axis is parallel to the link L12 and is denoted by L12), and the mounted robot 1 is moved to X in the absolute coordinate system Σworld. -Move horizontally in the Y plane. Here, each link L10 to L13 of the slider 2
Are jointed, and each of the joints RJ11 and RJ12 is connected to the first axis L11.
And a moving mechanism that moves along the second axis L12.

Further, as shown in FIGS. 1 and 2, the robot 1 has six rotatable joints RJ1 to RJ6,
It has seven links L0 to L6 connecting the joints, and the mechanical origin Om of the robot 1 is connected to the first joint RJ1 via the 0th link L0, and the first joint RJ1 is connected to the first joint RJ1.
The second joint RJ2 is connected via the link L1 to the second joint RJ2, and the second joint RJ2 is connected to the third joint RJ3 via the second link L2. Further, the third joint RJ3 is connected to the fourth joint RJ4 via the third link L3, and the fourth joint RJ4 is connected to the fourth joint RJ4.
The fifth joint RJ5 is connected via a link L4, and the fifth joint RJ5 is connected via a fifth link L5 to a sixth joint RJ6. Further, the sixth joint RJ6 is connected to the tool 4 via the sixth link L6. Each joint RJ of the robot 1
As shown in FIG. 4, k (k = 1, 2,..., 6) is a motor MOk composed of a servo stepping motor whose rotation angle is controlled by the controller 3 and a motor MOk having a predetermined reduction ratio λk , And a sensor SNk for detecting the position of the rotating shaft after deceleration, and the shafts are respectively rotated.

Each joint RJ11, RJ1 of the slider 2
Similarly to the robot 1, the moving mechanisms corresponding to the second and third motors are a motor MO9 that moves along the first axis L11, a sensor SN9 that detects a position after the movement, and a motor MO10 that moves along the second axis L12. A sensor SN10 for detecting the position after the movement.

Further, the positioner 3 has its mechanical origin O
p and four links L20 to L23 cascaded and connected to each other between the control point Owork of the work, a rotatable joint RJ21 between the links L21 and L22, and a rotation between the links L22 and L23. Possible joint R
J22. As shown in FIG. 4, the joints RJ21 and RJ22 are each controlled by a control device 5 to control the rotation angle of the motor MOk as a servomotor, and the rotation of the motor MOk to a predetermined reduction ratio λk. , And a sensor SNk for detecting the position of the rotating shaft after deceleration, and the shafts are respectively rotated, where k = 7, 8.

In order to define the mechanical origin and the system origin of each mechanical element of the robot system of the present embodiment, a coordinate system is set as shown in FIGS. 2 and 7 to 9, and the name and the name of each coordinate system are set. , The coordinates of the coordinate system and the setting positions are shown below. (A) Absolute coordinate system $ world: Set to the origin Oworld of the robot system. (B) Tool base coordinate system Δs: Set to the mechanical origin Os of the slider 2. (C) Tool machine coordinate system Σm: Mechanical origin O of robot 1
Set to m. (D) Tool side mechanical interface coordinate system Δt
m: Set to the output axis origin Otm of the robot 1. (E) Tool coordinate system Σtool: tip position O of tool 4
Set to tool. (F) Work base coordinate system $ p: Set to the mechanical origin Op of the positioner 3. (G) Work side mechanical interface coordinate system Σw
m: Set to the output shaft origin Owm of the positioner 2. (H) Work coordinate system @work: Work control origin (hereinafter referred to as the work origin) is set to Owork. Each coordinate system is represented by three axes of an X axis, a Y axis, and a Z axis, and each of them is provided with an attached code.

Here, the sign of the homogeneous transformation matrix indicating the relationship between the position and orientation between the two coordinate systems is shown below. Each element of these homogeneous transformation matrices and position data and orientation data, which will be described later in detail when calculating these homogeneous transformation matrices, are RA
It is stored in M102. (A) [Zt]: position and orientation of the tool base coordinate system Σs based on the absolute coordinate system Σworld. (B) [Ts]: position and orientation of the tool machine coordinate system Σm based on the tool base coordinate system Σs. The position / posture is the position / posture of the slider 2. (C) [Tma]: Position and orientation of the tool-side mechanical interface coordinate system Σtm with respect to the tool machine coordinate system Σm. The position / posture is the position / posture of the robot 1. (D) [Et]: Position and orientation of the tool coordinate system Σtool based on the tool-side mechanical interface coordinate system Σtm. (E) [Zw]: position and orientation of the work base coordinate system ΣOp based on the absolute coordinate system Σworld. (F) [Tp]: Position and orientation of the workpiece-side mechanical interface coordinate system Σwm based on the work base coordinate system Σp. The position / posture is the position / posture of the positioner 3. (G) [Ew]: Position and orientation of the work coordinate system Σwork based on the work-side mechanical interface coordinate system Σwm.

First, a homogeneous transformation matrix [Et] indicating the position and orientation of the tool coordinate system Σtool based on the tool-side mechanical interface coordinate system Σwm is represented by three position data Ate, Bte, Cte and three posture data αt.
It can be expressed by the following equation 1 using e, βte, and γte.

[Et] = Trans (Ate, Bte, Cte) Rot (X, αte) Rot (Y, βte) Rot (Z, γte)

Here, Rot (a, b) is a rotation transformation indicating that the rotation is made by an angle b (rad) about the axis of a, and Trans (a, b, c) is a distance a in the X-axis direction. , Translation in the Y-axis direction by a distance b, and translation in the Z-axis direction by a distance c, and so on. These homogeneous transformations are described by Richard P Paul,
"Robot Manipulators; Mathematics, Programming and Control (ROBOT MANIPULATORS; MA
THEMATICS, PROGRAMMING, AND
CONTROL) ", The MIT Press, 1981. From the above equation 1, the position and orientation of the tool coordinate system Σtool based on the tool-side mechanical interface coordinate system Σtm is used. The homogeneous transformation matrix [E
t] performs a translational transformation of Trans (Ate, Bte, Cte), rotationally transforms about the X axis by αte [rad], rotationally transforms about the Y axis by βte [rad], and transforms the Z axis. This indicates that the transformation is a homogeneous transformation in which the rotation is transformed by γte [rad] to the center.

The result of the above equation (1) is obtained by calculating 4 rows ×
It can be represented in the form of a four-column homogeneous transformation matrix.

(Equation 2)

The homogeneous transformation matrix [Ew] indicating the position and orientation of the work coordinate system based on the work side mechanical interface coordinate system Σwm is represented by three pieces of position data Aw.
e, Bwe, Cwe and three attitude data αwe, βw
It can be expressed by the following Equation 3 using e and γwe.

[Ew] = Trans (Awe, Bwe, Cwe) · Rot (X, αwe) · Rot (Y, βwe) · Rot (Z, γwe)

Further, a homogeneous transformation matrix [Zt] indicating the position and orientation of the tool base coordinate system Σs based on the absolute coordinate system Σworld is obtained by three position data Atz, Btz, C
tz and the three attitude data αtz, βtz, and γtz can be expressed by the following equation 4.

[Zt] = Trans (Atz, Btz, Ctz) · Rot (X, αtz) · Rot (Y, βtz) · Rot (Z, γtz)

Further, the homogeneous transformation matrix [Zw] indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld is obtained by three position data Awz and Bw.
z, Cwz and three attitude data αwz, βwz, γw
It can be expressed by the following equation 5 using z.

[Zw] = Trans (Awz, Bwz, Cwz) · Rot (X, αwz) · Rot (Y, βwz) · Rot (Z, γwz)

Further, coordinate conversion at each link Li in a machine having a plurality of joints such as the robot 1, the slider 2 and the positioner 3 and a plurality of links connecting the joints is performed by a known Dinabit-Hartenburg (Denav).
It can be expressed as in Equation 6 using an A matrix expressed by the link parameters of it-Hartenberg (hereinafter, referred to as DH link parameters).

Ai = Rot (Z, θi) · Trans (ai, 0, di) · Rot (X, αi), where i = 0, 1, 2,...

Using this A matrix, a homogeneous transformation matrix indicating the position and orientation of the robot 1, slider 2 and positioner 3 of the robot system can be expressed by the following equations 7 to 9.

[Tma] = Ama0 · Ama1 · Ama2 · Ama3 · Ama4 · Ama5 · Ama6

[Ts] = As0 · As1 · As2 · As3

[Tp] = Ap0 ・ Ap1 ・ Ap2 ・ Ap3 Here, Ama0, As0 and Ap0 correspond to the pedestal link of each machine.

Further, a homogeneous transformation matrix [wXt] indicating the position and orientation of the tool coordinate system Σtool based on the workpiece coordinate system Σwork is defined by the following equation 10 by using the above equation 2.

(Equation 10)

At this time, the following equation 11 holds for the position and orientation of each mechanical component set in the robot system including the robot 1, the slider 2, the positioner 3, and the tool 4.

[Zt] · [Ts] · [Tma] · [Et] = [Zw] · [Tp] · [Ew] · [wXt]

According to the above equation (11), the relation between the relative position and orientation of the tool 4 and the workpiece is defined by a homogeneous transformation matrix [wXt]. By changing the homogeneous transformation matrix [wXt], a specific relationship is obtained. By changing each element of the homogeneous transformation matrix [wXt], the robot 1, the slider 2, and the positioner 3 can be operated cooperatively.

In this embodiment, the position and orientation data necessary for setting the installation relationship between the machines in the robot system of FIG. 1 are specifically set as follows. First, the position and orientation data for calculating the homogeneous transformation matrix [Zt] indicating the position and orientation of the mechanical origin Os of the slider 2 based on the origin Oworld of the absolute coordinate system Σworld is calculated as shown in the following Expressions 12 to 17. Set.

## EQU12 ## Atz = 0.0

[Mathematical formula-see original document] Btz = 0.0

## EQU14 ## Ctz = 0.0

## EQU15 ## αtz = αtz

## EQU16 ## βtz = βtz

[Mathematical formula-see original document] γtz = γtz

Next, the origin O of the absolute coordinate system Σworld
Position and orientation data for calculating a homogeneous transformation matrix [Zw] indicating the position and orientation of the mechanical origin Op of the positioner 3 based on the world is set as shown in the following Expressions 18 to 23.

Awz = awz

[Equation 19] Bwz = bwz

[Equation 20] Cwz = cwz

Equation 21 αwz = αwz

## EQU22 ## βwz = βwz

Γwz = γwz

Similarly, the homogeneous transformation matrices [Et] and [Et]
w] is calculated from the following equations 24 to 3
Set as shown in FIG.

Ate = 0.0

(Equation 25) Bte = 0.0

[Equation 26] Cte = Cte

[Equation 27] αte = 0.0

[Equation 28] βte = βte

## EQU29 ## γte = 0.0

Awe = 0.0

[Mathematical formula-see original document] Bwe = 0.0

(32) Cwe = 0.0

[Equation 33] αwe = 0.0

[Expression 34] βwe = 0.0

[Expression 35] γwe = 0.0

In order to calculate the position and orientation of the robot 1, the slider 2 and the positioner 3, the following Tables 1 to 3 are used.
DH link parameters of the robot 1, the skiider 2, and the positioner 3, respectively, are used. Here, in the robot 1, for example, the joint variable θmai indicates the rotation angle of the n-th link Li, and the common normal distance amai
Corresponds to the length of the i-th link Li, and the link distance dmai corresponds to the distance between the (i-1) -th link Li-1 and the i-th link Li, and the twist angle αmai
Are the (i-1) th link Li-1 and the nth link L
The angle between the joint axes corresponding to the angle with i, where i = 0, 1, 2, 3,..., 6. The same applies to the slider 2 and the positioner 3. Also, in Tables 1 to 3, the joint variable θmai (i =
1, 2,..., 6), the operating positions ds1, ds2, which are the distances between the links of the slider 2, and the joint variable θs of the slider 2.
3 and the joint variable θpk (k = 1, 2) of the positioner 3 are variables and stored in the RAM 102, but the other parameters are constant values stored in the ROM 101.

[0040]

[Table 1] Link Joint variables θmai Distance amai Distance dmai Torsion angle αmai Ｌ L0 0.0 0.0 dma0 0.0 L1 θma1 ama1 dma1 + π / 2 L2 θma2 ama2 0.0 0.0 L3 θma3 ama3 0.0 + π / 2 L4 θma4 0.0 dma4 + π / 2 L5 θma5 0.0 0.0 -π / 2 L6 θma6 0.0 dma6 0.0

[0041]

[Table 2] Link Joint variable θsj Distance asj Distance dsj Torsion angle αsj ────────────────────────────────── L10 + Π / 2 0.0 ds0 + π / 2 L11 + π / 2 0.0 ds1 + π / 2 L12 + π / 2 0.0 ds2 + π / 2 L13 θs3 0.0 0.0 0.0

[0042]

[Table 3] Link joint variable θpk distance apk distance dpk torsion angle αpk ────────────────────────────────── L20 0.0 0.0 0.0 0.0 L21 θp1 0.0 dp1 + π / 2 L22 θp2 0.0 0.0 -π / 2 L23 0.0 0.0 dp3 0.0

Each of the machines 1, 2, 3 and the machines 1, 2, 3
Given the data of the position and orientation between, each element of the homogeneous transformation matrix indicating the relationship of the position and orientation between each coordinate system is calculated,
It is stored in the RAM 102 (see FIG. 3) in the control device 5.

However, the data of the position and orientation between the machines 1, 2 and 3 stored in the control device 5 often does not become the same as the actual values of the position and orientation, and various data as shown in FIGS. There is an installation error of Therefore, in the present embodiment according to the present invention, an apparatus configuration for calibrating these installation errors will be described below.

In the present embodiment, the calibration process executed by the control device 5 includes the following four calibration processes.

(A) First calibration processing (step S1, see FIGS. 30 and 31): an absolute coordinate system Σworld and a tool base coordinate system Σs set at the origin Os of the slider 2
Attitude component αtz relating to the rotation of the mutual coordinate system between
This is a process for calibrating βtz and γtz, and the installation error parameter is calculated based on the absolute coordinate system Σworld and the tool base coordinate system 姿勢 s posture components αtz, βtz, and torsion angles of γtz △
An installation error ε3 including αtz, △ βtz, and △ γtz.

(B) Second calibration process (step S2, see FIG. 32): the direction of the drive first axis L11 of the slider 2;
Tool machine coordinate system Σm provided on the reference mounting plane of robot 1
This is a process for calibrating the posture component θs3 relating to the rotation of the coordinate system alternate between the two. The error parameters are the direction of the driving first axis L11 of the slider 2 and the tool mechanical coordinate system Σm provided on the installation reference plane of the robot 1. Is an installation error ε4 consisting of a torsion angle Δθs3.

(C) Third calibration process (step S3, see FIGS. 33 to 36): absolute coordinate system Σworld,
An absolute coordinate system Σw relating to the rotation of the mutual coordinate system with the work base coordinate system Σp set at the origin Op of the positioner 3
posture components γwz, βw of the old Z-axis, Y-axis and X-axis
This is a process for calibrating z and αwz, and the error parameter is
An absolute coordinate system between an absolute coordinate system {world} and a work base coordinate system {p set at the origin Op of the positioner 3}
torsional angles around the world's Z, Y and X axes
An installation error ε2 including γwz, Δβwz, and Δαwz. Here, as shown in FIG. 33, the third calibration process includes an angle γwz calibration process (step S31, see FIG. 34) and an angle βwz calibration process (step S32, FIG. 3).
See 5. ) And the angle αwz calibration process (step S33,
See FIG. ).

(D) Fourth calibration processing (step S4, see FIG. 37): With respect to the work coordinate system Σwork provided at the origin Owm of the output shaft of the positioner 3, the origin of the work coordinate system Σwork calculated by the controller 5 is calculated. This is a process for calibrating the difference between the position of the Owork and the position of the actual origin of the work. The error parameter is represented by an absolute coordinate system Σworld
Of the position components awz, bwz, and cwz from the origin Oworld of the positioner 3 to the mechanical origin Op of the positioner 3 △ aw
z, △ bwz, and △ cwz.

Accordingly, the four installation errors described above are illustrated in FIG. That is, the installation errors ε1 and ε2
Is the installation error between the absolute coordinate system Σworld and the positioner 3, and the installation error ε3 is the absolute coordinate system Σw
is the installation error between the old and the slider 2, and the installation error ε
4 is an installation error between the robot 1 and the slider 2. (2) Configuration of Industrial Robot System and Its Installation Error Calibration Control Device FIG. 3 shows the configuration of the control device 5.

As shown in FIG. 3, the control device 5 comprises (a)
Clock signal generated by clock generator 110 and frequency divider 1 for dividing the clock signal by a predetermined frequency division ratio
11 based on the synchronization signal SYNC generated by
According to the system program stored in the OM 101,
C for controlling the robot 1, the slider 2, and the positioner 3
PU100, (b) a ROM 101 for storing a system program for controlling each operation of the robot 1, the slider 2, and the positioner 3, and data necessary for executing the system program, and (c) a working area of the CPU 100. RAM 102 for storing the work program generated in the work program generation processing, (d) an interface 103 to which the teaching box 20 and the operation box 21 are connected, and (e) each of the motors MO1 to MO10. Command position DPk (k =
, 10) are stored, and the respective circuits 100 to 10 are provided.
3 and the first port of the dual port RAM 104 are connected via a bus 109. The processing for each of the operation switches of the teaching box 20 and the operation box 21 is executed by the interrupt processing of the CPU 100.

The second port of the dual port RAM 104 is connected to the latch circuit 105, and the latch circuit 105 synchronizes with the synchronization signal SYNC to output the dual port RAM1.
After simultaneously latching the data of ten motor command positions DPk for each of the motors MO1 to MO10 stored in 04, the following three operations are simultaneously executed. (A) Motor command position D related to joint control of robot 1
The data of P1 to DP6 are output to the motors MO1 to MO6 via the servo control circuit 106 and driven. (B) Motor command position D related to joint control of slider 2
The data of P9 to DP10 are output to the motors MO9 to MO10 via the servo control circuit 108 and driven. (C) The data of the motor command positions DP7 to DP8 relating to the joint control of the positioner 3 are output to the motors MO7 to MO8 via the servo control circuit 107 and driven. Here, the generation cycle of the synchronization signal SYNC corresponds to the drive cycle of the motors MO1 to MO10 in the teaching operation and the cycle of driving the motors MO1 to MO10 for each interpolation point in the reproduction operation process. The generation cycle is determined by the control device 2
, A value obtained by adding a slight margin value to the processing required for each interpolation during the teaching operation and the reproducing operation, that is, the time required for the calculation. Further, it is preferable that the generation cycle be short, but this requires processing by a CPU with a short processing time, which is expensive. In practice, it is preferably set to 10 to 70 msec.

The CPU 100 calculates the data of the motor command position DPk by dividing the joint variable calculated as described later by the reduction ratio λk, writes the data in the dual port RAM 104, and the CPU 100 calculates the current position. The values of the joint variables and the values of the respective elements of the homogeneous transformation matrix indicating the relationship between the coordinate systems described above are stored in the RAM 102.

FIG. 4 is a block diagram showing the structure of each joint RJk of the robot 1 and the positioner 3 and the servo control circuits 106 and 107. As shown in FIG. 4, the rotation shaft of the motor MOk is connected to the input shaft of the reduction gear REk, and the reduction gear REk is generated by the motor MOk by reducing the rotation speed of the motor MOk by a predetermined reduction ratio λk. The torque is increased by the reciprocal of the reduction ratio, and the reduction gear REk
Is output to the link Lk connected to the output shaft. here,
The rotation axis of the motor MOk is connected to the axis of the sensor SNk, and the output signal from the sensor SNk is input to the subtraction input terminal of the adder 120 in the servo control circuits 106 and 107. On the other hand, data of the motor command position DPk from the latch circuit 105 is input to the addition input terminal of the adder 120. Data output from the adder 120 is converted into an analog voltage signal by a D / A converter (not shown), and then applied to the drive terminal of the motor MOk via an amplifier 121 having a predetermined amplification factor. You. In the servo control feedback system configured as described above, based on the output signal from the sensor SNk, X
The rotation angle from the axis Xk-2 ″ that is parallel to the axis Xk-1 and extends from the joint axis Zk-1 and corresponds to the axis Xk-2 ′ and extends from the rotation axis of the motor MOk is the motor command position. DPk so that the link L
k is rotated from the axis Xk-2 ′ by the joint variable θk. The structure of the joint of the slider 2 and the servo control circuit 108 therefor have the same configuration except that the slider 2 is translated by two drive axes L11 and L12.

(3) Process of Installation Error Calibration Control Device (3-1) Main Routine FIG. 26 shows a flowchart of a main routine of the installation error calibration process executed by the control device 5.

In FIG. 26, first, in a step S1, a first calibration process for calibrating the installation error ε3 is executed,
Next, in step S2, a second calibration process for calibrating the installation error ε4 is executed. Further, in step S3, a third calibration process for calibrating the installation error ε2 is executed,
Next, in step S4, a fourth calibration process for calibrating the installation error ε1 is executed.

(3-2) First Calibration Process FIGS. 30 and 31 are flowcharts showing the first calibration process (step S1) of FIGS. 26 and 28.

In the first calibration process, the mechanical origin Os of the slider 2 based on the absolute coordinate system Σworld
The attitude component αtz of the tool base coordinate system Σs set in
βtz and γtz are set as calibration targets, and these parameters are stored in the RAM 102 of the control device 5. However, in the actual robot system, as shown in FIGS. 10 to 12, the torsion angle {αtz, △
βtz and △ γtz. In the first calibration process, the torsion angles △ αtz, △ βtz, △ γtz, which are differences between the position and orientation data stored in the control device 5 and the actual position and orientation data of the robot system, are detected. Is calibrated to the actual position and orientation data of the robot system.

First, as shown in FIGS. 13 and 14, each surface of the rectangular parallelepiped block BK1 has an absolute coordinate system Σworld
A rectangular parallelepiped block BK1 that has been machined so that its surface is flat is placed at a predetermined position that is parallel to each axial direction. Further, a distance sensor SS1 for measuring a distance to the upper surface of the block BK1 parallel to the Zworld axis and a distance sensor S for measuring a distance to a side surface of the block BK2 parallel to the Xworld axis.
S2 is set so that the sensor surfaces of the distance sensors SS1 and SS2 are perpendicular to each other, the sensor axis of the distance sensor SS1 is perpendicular to the upper surface of the block BK1, and the sensor axis of the distance sensor SS2 is perpendicular to the side surface of the block BK1. Then, an operator attaches the tool 4 of the robot 1 via a jig J1 that holds the distance sensors SS1 and SS2. The operator notifies the CPU 100 using the teaching box 20 that the preparation work has been completed.
The control device 5 executes the following first calibration processing assuming that the slider 5 is at the position A1.

In step S 110, the current operating position ds 1 of the slider 2 at the current position A 1, which is stored in the RAM 102 of the control device 5, is loaded into the CPU 100. Hereinafter, “fetching data” in each process means “taking data into the CPU 100”. Next, in step S112, the absolute coordinate system $ world stored in the RAM 102 of the control device 5 is set.
A homogeneous transformation matrix [Zt] indicating the position and orientation of the tool base coordinate system Σs based on d is fetched. Further, in step S116, the slider 2 shown in FIGS.
Is located at position A1, distance sensors SS1,
FIG. 3 shows the sensor detection distances lx1 and lz1 detected by SS2.
Is taken in through the interface 103 shown in FIG. Here, the sensor detection distances lx1 and lz1 are distances in the X-axis direction and the Z-axis direction of the absolute coordinate system Σworld, respectively.

Next, in step S118, the slider 2 on which the robot 1 is mounted is moved by a predetermined distance DL1 along the first axis L11, and the slider 2 is positioned and fixed at the position B1. In this process, the robot 1 on the slider 2 is not operated. Then, in step S119, as shown in FIGS. 13 and 14, when the slider 2 is located at the position B1, similarly to step S116, the distance sensor S
The sensor detection distance detected by S1 and SS2 respectively is lz
2. Import 1 × 2. Further, in step S120, based on the sensor detection distances lx1, lx2 and lz1, lz2 obtained in steps S116 and S119, and the moving distance DL1, the absolute coordinate system {world} is calculated using the following equations (36) and (37). Are calculated with reference to the posture components αe and γe of the tool base coordinate system Σs.

[0062]

[Equation 36]

(37)

Further, in step S121, it is determined whether or not the posture component αe around the X axis of the tool base coordinate system Σs based on the absolute coordinate system Σworld calculated in step S120 is equal to αtz stored in the RAM 102. Then, in step S122, the absolute coordinate system Σ wor calculated in step S120 is calculated.
The posture component γe around the Z-axis of the tool base coordinate system Σs based on ld is represented by γtz stored in the RAM 102.
Is determined. Here, if αe = αt
If z and γe = γtz, step S13
It is determined that there is no need to perform the calibration processing from 0 to S132, and the process proceeds to the processing in FIG. On the other hand, if αe ≠ αtz and γe ≠ γtz, the process proceeds to step S130.

In step S130, step S1
Based on the posture components αe and γe of the tool base coordinate system Σs based on the absolute coordinate system Σworld calculated in 20, the RA of the control device 5 is calculated using the following Expressions 38 and 39.
The torsion angles △ αtz and △ γtz are calculated for the posture components αtz and γtz stored in M102.

３８αtz = αe−αtz

３９γtz = γe−γtz

Next, in step S131, the torsion angle Δαt calculated in step S130 is calculated.
By using z and △ γtz as correction values, the posture components αtz and γtz stored in the RAM of the robot controller are corrected. That is, the twist angles △ αtz, △ γtz
Calibration value α of the posture component of the tool base coordinate system Σs based on the absolute coordinate system Σworld after being calibrated using
tz ′ and γtz ′ are expressed as in the following Expression 40 and Expression 41.

Αtz ′ = αtz + △ αtz

４１tz ′ = γtz + △ γtz

Next, the calculated calibration values αtz ′,
A homogeneous transformation matrix [Zt '] of calibration values indicating the position and orientation of the tool base coordinate system Σs based on the absolute coordinate system Σworld with respect to γtz' is calculated using Expression 42.

[Zt ′] = Trans (atz, btz, ctz) · Rot (X, αtz ′) · Rot (Y, βtz) · Rot (Z, γtz ′)

Then, in step S132, the homogeneous transformation matrix [Zt '] of the calculated calibration value is stored in the RAM 102 as the homogeneous transformation matrix [Zt], and the calibration is completed. Attitude component αtz of tool base coordinate system Σs based on Σworld,
The posture components αtz and γtz among βtz and γtz can be calibrated, and the following processing in FIG. 31 is performed to calibrate the remaining posture components βtz.

As shown in FIGS. 15 and 16, the robot 1 is positioned at a predetermined position A2, a distance sensor SS1 for measuring a distance from the upper surface of the block BK1 parallel to the Zworld axis, and a distance sensor SS1 for the Yworld axis. Sensor S for measuring the distance from the side of the parallel block BK2
S2 is set so that the sensor surfaces of the distance sensors SS1 and SS2 are perpendicular to each other, the sensor axis of the distance sensor SS1 is perpendicular to the upper surface of the block BK1, and the sensor axis of the distance sensor SS2 is perpendicular to the side surface of the block BK1. Then, an operator attaches the tool 4 of the robot 1 via a jig J1 that holds the distance sensors SS1 and SS2. The operator notifies the CPU 100 using the teaching box 20 that the preparation work has been completed, and the control device 5 executes the following processing assuming that the slider 2 is at the position A2. First, step S1 in FIG.
At 50, the operating position ds2 of the slider 2 is captured.

Next, in step S152, the absolute coordinate system Σ stored in the RAM 102 of the control device 5
A homogeneous transformation matrix [Zt] indicating the position and orientation of the tool base coordinate system Σs based on world is fetched. further,
In step S153, as shown in FIGS. 15 and 16, the sensor detection distances lz1 and ly1 detected by the distance sensors SS1 and SS2 when the robot 1 is located at the position A2 are captured. Here, each of the sensor detection distances ly1 and lz1 is an absolute coordinate system Σwo
rld are distances in the Y-axis direction and the Z-axis direction.

Next, in step S155, the slider 2 on which the robot 1 is mounted is moved by a predetermined distance DL2 along the second axis L12, and the slider 2 is positioned and fixed at the position B2. In this process, the robot 1 on the slider 2 is not operated.
Then, in step S156, FIGS.
When the slider 2 is located at the position B2 as shown in FIG.
1 and SS2 are the sensor detection distances respectively detected by lz
2. Import ly2. Further, in step S157, based on the sensor detection distances ly1, ly2 and lz1, lz2 obtained in steps S153 and S156 and the moving distance DL2, using the following equation 43,
The posture component βe of the tool base coordinate system Σs based on the absolute coordinate system Σworld is calculated.

[0071]

[Equation 43]

Further, in step S158, it is determined whether or not the posture component βe around the Y axis of the tool base coordinate system と し た s based on the absolute coordinate system さ れ world calculated in step S157 is equal to βtz stored in the RAM 102. Is determined. Here, if βe = βtz, the process returns assuming that the first calibration process has been completed.
On the other hand, if βe ≠ βtz, step S160
Proceed to.

In step S160, step S1
Based on the posture component βe of the tool base coordinate system Σs based on the absolute coordinate system Σworld calculated in 57, the twist angle with respect to the posture component βtz stored in the RAM 102 of the control device 5 using the following Expression 44 Δβtz is calculated.

４４βtz = βe−βtz

Next, in step S161, the torsion angle △ βtz calculated in step S157
Is used as a correction value to correct the posture component βtz stored in the RAM of the robot controller. That is, the tool coordinate system based on the absolute coordinate system {world} after being calibrated using the twist angle {βtz}
The calibration value βtz ′ of the posture component of s is expressed as in the following Expression 45.

[Formula 45] βtz ′ = βtz + △ βtz

Next, a homogeneous transformation matrix [Zt ″] of the calibration value indicating the position and orientation of the tool base coordinate system Σs based on the absolute coordinate system Σworld with respect to the calculated calibration value βtz 'is expressed by the following equation. The calculation is performed using 46.

[Zt ″] = Trans (atz, btz, ctz) · Rot (X, αtz) · Rot (Y, βtz ′) · Rot (Z, γtz)

Then, in step S162, the homogeneous transformation matrix [Zt ″] of the calculated calibration value is stored in the RAM 102 as the homogeneous transformation matrix [Zt].
As a result, the process returns assuming that the first calibration process has been completed. By the above-described first calibration processing in FIGS. 30 and 31, all the posture components αtz, βtz, and γtz in the tool base coordinate system Σs based on the absolute coordinate system Σworld
Can be calibrated.

(3-3) Second Calibration Process FIG. 32 is a flowchart of the second calibration process.

The second calibration process is, as shown in FIGS. 10 to 12, a posture component between the direction of the second axis L12 of the slider 2 and the tool machine coordinate system ΣOm provided on the installation reference plane of the robot 1. Twist angle Δθs3, which is the installation error of θs3
Is detected, and the posture component θs3 is calibrated.

First, as shown in FIG. 18 and FIG. 19, in order to measure the distance 1 to the tip reference point Otool of the tool 4, the tool 4 is machined so that the surface is flat at the tip. A rectangular parallelepiped block BK1 is attached, and a distance sensor SS2 for measuring a linear distance is fixed on, for example, a table such that its sensor surface is parallel to a reference surface L1R of a second axis L12 of the slider 2. Therefore, where
The sensor axis of the fixed distance sensor SS2 is a block BK
1 and the reference plane L1R of the second axis L12 of the slider 2
And the distance sensor SS2 is
Of the tool 4 from the sensor surface of the tool 4 is measured.

By the way, as shown in FIG. 4, each joint of the robot 1, the slider 2 and the positioner 3 is provided with a motor and a speed reducer so as to be rotatable. Therefore,
In each joint RJ1 to RJ6 of the robot 1, (a)
Let the joint number be i (i = 1, 2, 3,..., 6), and (b)
The motor command position is Pmai [pulse], (c) the command position increase / decrease per motor rotation is Nmai [pulse / rotation], (d) the reduction ratio of the speed reducer is λmai, and (e)
If the joint variable of the robot 1 is θmai [rad],
The following equation 47 is established.

[0081]

Equation 47 θmai = Pmai × (2π / Nmai) × λmai,
i = 1,2, ..., 6

In the first and second moving axes of the slider 2, (a) the axis number is j (j = 1, 2),
(B) Set the motor command position to Psj [pulse], and (c)
The amount of slider movement per rotation of the motor is Dsj [m
m], (d) the commanded position increase / decrease amount per one rotation of the motor is Nsj [pulse / rotation], and (e) the operation position (corresponding to the joint variable) of the slider 2 is dsj [mm]. The following Expression 48 holds.

[0083]

Dsj = Psj × (Dsj / Nsj), j =
1,2

Further, in the one-axis and two-axis joints RJ21 and RJ22 of the positioner 3, (a) the joint number is k
(K = 1, 2), and (b) the motor command position is Ppk
[Pulse], (c) the commanded position increase / decrease amount per motor rotation is Npk [pulse / rotation], (d) the reduction ratio of the reduction gear is λpk, and (e) the joint variable of the positioner 3 is θpk [ rad], the following equation 49 holds.

[0085]

(49) θpk = Ppk × (2π / Npk) × λpk, k = 1,
2

In step S210 of the second calibration process, the motor command positions Pmai, Ps of the machines 1, 2, 3 are read from the dual port RAM 104 in FIG.
Read j and Ppk.

Next, using equations 47 to 49, the joint variable θmai, of the robot 1, the slider 2 and the positioner 3 at the position A3 when the robot 1 is positioned and fixed as shown in FIGS. Calculate θpk and operation position dsj.

Next, in step S211, the joint variable θm of the robot 1 calculated in step S210
ai (i = 1, 2,..., 6), the operating position d of the slider 2
sj (j = 1, 2) and the joint variable θp of the positioner 3
Based on k (k = 1, 2), position A3 in FIG.
Homogeneous transformation matrix [T indicating the position and orientation of the robot 1 at
ma], a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2, and a homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3, as described below.
Stored in 102.

First, by substituting the calculated value of the joint variable θmai (i = 1, 2,..., 6) of the robot 1 into Equation 6, the A matrix Amai of the robot 1 is converted into the following Equation 50. Calculate using

Amai = Rot (Z, θmai) · Trans (amai, 0, dmai) · Rot (X, αmai), where i = 0,1,2, ..., 6.

Ama0 is calculated based on the parameter value of link L0 in Table 1 above. By substituting the A matrix Amai (i = 0, 1, 2,..., 6) calculated above into Equation 7, a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1 is calculated.

The A matrix Asj of the slider 2 is given by
By substituting the operating position dsj (j = 1, 2) of the slider 2 into the following equation, the value of the calculated operating position dsj (j = 1, 2) of the slider 2 can be expressed by the following equation (5).
By substituting for 1, each element of the A matrix is calculated.

Asj = Rot (Z, θsj) · Trans (asj, 0, dsj) · Rot (X, αsj) where j = 1,2.

Note that As0 and As3 are as shown in Table 2 above.
Is calculated based on the parameter values of the links L10 and L13. The A matrix Asj (j = 0,
By substituting (1, 2, 3) into Equation 8, the slider 2
Is calculated. [Ts] is calculated.

Further, the A matrix Apk of the positioner 3 is
By substituting the joint variable θpk (k = 1, 2) of the positioner 3 into Equation 6, it can be expressed by the following equation. The calculated value of the operating position θpk (k = 1, 2) of the positioner 3 is expressed by By substituting into Equation 52, each element of the A matrix is calculated.

Apk = Rot (Z, θpk) · Trans (0,0,0) · Rot (X, αpk) where k = 1,2.

Ap0 and Ap3 are respectively shown in Table 3 above.
Is calculated based on the parameter values of the links L20 and L23. The A matrix Apk (k = 0,
By substituting (1, 2, 3) into Equation 9, a homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3 is calculated.

The same transformation matrices [Tma], [Ts], [Tp] indicating the positions and orientations of the robot 1, slider 2 and positioner 3 at the position A3 calculated as described above are calculated and stored in the RAM 102. .

Next, in step S213, the sensor detection distance 1 between the distance sensor SS2 when step 2 is fixed at the position A3 and the block BK1 attached to the tip O Tool of the tool 4 is defined as a distance l1, and FIG. RAM via interface 103 shown
Take in 102. Further, in step S214, a homogeneous transformation matrix [Zt ″] calibrated by the first calibration process and stored in the RAM 102 as a homogeneous transformation matrix [Zt], and [Zt ”stored in the RAM 102
w], [Et], and [Ew], and homogeneous transformation matrices [Tma], [Ts], [Ts] indicating the position and orientation of the robot 1, the slider 2, and the positioner 3 at the position A3 calculated in step S211. Tp], a homogeneous transformation matrix [wX indicating the position and orientation of the tip OTool of the tool 4 with reference to the workpiece coordinate system Σwork.
t] is calculated.

[0097]

[WXt] = [Ew] ⁻¹ · [Tp] ⁻¹ · [Zw] ⁻¹ · [Zt ″] · [Ts] · [Tma] · [Et]

In Equation 53, the homogeneous transformation matrix
[Zw]^-1, [Tp]^-1, [Ew] ^-1Is RA
The homogeneous transformation matrices [Zw] and [T
p] and [Ew] are respectively subjected to inverse matrix transformation.
Is calculated by

Next, in step S216, the following processing is executed. That is, based on the operating position ds2 of the second axis L12 of the slider 2 at the position A3 calculated in step S210 and the predetermined movement amount DL3 of the second axis L12 of the slider 2, the following equation 54 is used. And the operating position ds of the slider 2 at the next position B3.
2 'is calculated.

[0100]

Ds2 '= DL3 + ds2

Next, the operation position ds2 'of the second axis L12 of the slider 2 at the position B3 is set as the operation position ds2, and the position and posture of the slider 2 at the operation position ds2 [T
s ′] is calculated using the above equation (8). A homogeneous transformation matrix [wXt] indicating the position and orientation of the tool Tool tip O Tool at the calculated position A3 on the basis of the work coordinate system Σwork is fixed, and a homogeneous transformation matrix indicating the position and orientation without operating the positioner 3 Under the condition that [Tp] is constant, a homogeneous transformation matrix [Tma ′] indicating the position and orientation of the robot 1 at the position B3 when the slider 2 is moved along the second axis L12 by a predetermined distance DL3. ] Using the following equation 55 to calculate a homogeneous transformation matrix [Tma ′].

[0102]

[Tma ′] = [Ts ′] ⁻¹ · [Zt ″] ⁻¹ · [Zw] · [Tp] · [Ew] · [wXt] · [Et] ⁻¹ where the right side Each homogeneous transformation matrix is obtained in step S216.
And the homogeneous transformation matrix calculated in step S214.

Next, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B3 is inversely transformed, and each joint variable θmai (i = 1, 2,
, 6) are calculated. Further, the motor command position Pmai (i = 1, 2, 2) of the robot 1 at the position B3
, 6) is calculated using the following equation 56 obtained by modifying the above equation 47, and the motor command position Psj (j = 1, 2) of the slider 2 is transformed from the above equation 48. The calculation is performed using the equation 57 obtained as described above.

[0104]

Pmai = θmai × (Nmai / 2π) × (1 / λmai), i = 1, 2,..., 6

Psj = dsj × (Nsj / Dsj), j =
1,2

Next, in step S219, the motor command positions Pmai (i = 1, 2,..., 6) and Psj (j) of the robot 1 and the slider 2 at the position B3.
= 1 and 2) are transferred to the motors MO1 to MO6 and MO via the dual port RAM 104, the latch circuit 105, and the servo control circuits 106 and 108 as shown in FIG.
9 and MO10 to operate the joints RJ1 to RJ6 of the robot 1 and move the slider 2 to its second position.
It is moved by a distance DL3 along the axis L12 to a position B3 shown in FIG. And position B3
Sensor SS2 and tip of tool 4 Otool
The sensor detection distance l2 from the block BK1 attached to the RAM 102 is loaded into the RAM 102 via the interface 103.

Next, at step S221, it is determined whether or not the sensor detection distance l1 at the position A3 detected at step S213 is equal to the sensor detection distance l2 at the position B3 detected at step S220. At l2, the torsion angle Δθs3, which is an installation error, becomes 0, and it is determined that the second calibration process has been completed, and the process returns. On the other hand, if l1
If ≠ 12, the process proceeds to step SS222.

In step S222, the slider 2
Angle between the direction of the second axis L12 and the tool machine coordinate system Σm provided on the installation reference plane of the robot 1 △ θs3
The amount of deviation 先端 DL3 of the tip OTool of the tool 4 due to
The calculation is performed using the following Expression 58.

５８DL3 = l2-l1

Next, in step S223, the tip OTool of the tool 4 detected in step S222 described above.
Based on the deviation amount ΔDL3 and the predetermined movement amount DL3, the torsion angle Δθ between the direction of the second axis L12 of the slider 2 and the tool mechanical coordinate system Δm provided on the robot installation reference plane is set.
s3 is calculated using the following equation 59.

５９θs3 = tan ⁻¹ (△ DL3 / DL3)

Then, the calculated twist angle △ θs
Using the following equation (60) to calibrate the angle rotation amount θs3 about the Z-axis of the third link described in the link parameter DH of the slider 2 in Table 2 using the correction value 3 as a correction value. The value θs3 ′ is calculated and the rotation amount θs3
2 is stored.

[Equation 60] θs3 ′ = θs3 + ’θs3

With the above processing, the process returns assuming that the second calibration processing has been completed.

(3-4) Third Calibration Process The third calibration process consists of the X-axis of the absolute coordinate system Σworld between the absolute coordinate system Σworld and the work base coordinate system Σp set at the origin Op of the positioner 3; The angle components αwz, βwz, and γwz of the postures around the Y axis and the Z axis are set as calibration targets, and are sequentially executed as shown in FIG. 33. (a) Angle γwz calibration processing for calibrating the angle component γwz (step S31) And (b) an angle βwz calibration process for calibrating the angle component βwz (step S32), and (c) an angle αwz calibration process for calibrating the angle component αwz (step S33). Hereinafter, each process will be described. Note that the position and orientation data relating to the homogeneous transformation matrix [Ew] are all set to 0 according to the above Expressions 30 to 35. Therefore, the work coordinate system Σwork matches the work-side mechanical interface coordinate system Σwm, and thus the position of the origin of the work coordinate system Σwork is the center Owork of the table plane of the positioner 3.

(3-4-1) Angle γwz Calibration Process (Step S31) FIG. 34 is a flowchart showing the angle γwz calibration process (step S31). In this process, the torsion angle △ γwz, which is an installation error, is detected. To calibrate the angle γwz.

First, in step S310, R
Joint variable θp1 of positioner 3 stored in OM101
Fixed value data θp1 = −90 ° and θp2 =
Capture 0 °. Next, the motor command position Ppk (k = 1, 2) of the positioner 3 with respect to the joint variable θpk (k = 1, 2) of the positioner 3 is calculated using the following equation 61 obtained by modifying equation 49. After the calculation, the calculated data is transferred to the motors MO7 and MO8 via the dual port RAM 104, the latch circuit 105, and the servo control circuit 107 to move the positioner 3 to the position of the initial setting.

Ppk = θpk × Npk / 2π × (1 / λpk) where k = 1,2. Next, as shown in FIG. 20, the operator moves the tip OTool of the tool 4 and the positioner 3 as shown in FIG.
An operator attaches a distance sensor SS1 for measuring a distance between the table and the table plane of the positioner 3 so that the sensor axis is perpendicular to the table plane of the positioner 3. The operator uses the teaching box 20 to notify the CPU 100 of the completion of the preparation work, so that the determination of the reception of the preparation completion signal in step S311 is Y.
In SE, the control device 5 executes the following processing assuming that the machines 1, 2, and 3 are at the position A4.

Next, in step S312, the values of the A matrix of the first axis and the second axis of the positioner 3 are calculated by the following equation 62 by substituting θp1 and θp2 taken in the above initial setting into the above equation 6. You.

Apk = Rot (Z, θpk) · Trans (0,0,0) · Rot (X, αpk) where k = 1,2.

Further, Ap0 and Ap3 are calculated based on the parameter values of the links L20 and L23 in Table 3 above. The A matrix Apk (i = 0,
By substituting (1, 2, 3) into Equation 9, a homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3 is calculated.

Next, the operating position dsj (j
= 1, 2) are the dual port RAM 104 in FIG.
From the motor command position Psj (j = 1, 2),
According to Expression 48, the A matrix Asj (j = 1,
2) is calculated.

Further, the A matrix Asj (j = 1, 2) of the slider 2 is calculated using the above equation 51, and the DH link parameter θs3 of the slider 2 already calibrated by the above-described second calibration process is calculated. Using the A matrix Asj of the slider 2
After calculating (j = 0,3), the slider 2 is calculated using Expression 8.
Is calculated. [Ts] is calculated.

Next, each joint variable θmai of the robot 1
(I = 1, 2,..., 6) is the motor command position Pmai (i = 1, 2) from the dual port RAM 104 in FIG.
,..., 6) are read out and calculated using Equation 47. Further, the A matrix Amai (i
= 0, 1, 2,..., 6), and then calculates a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1 using Expression 7.

Thus, the processing in step S312 is completed.
Next, in step S316, the sensor detection distance 11 of the distance sensor SS1 at the position A4 is loaded into the RAM 102 via the interface 103.

Next, in step S318, the first
[Zt ″] that has been calibrated by the calibration process and stored in the RAM 102 as a homogeneous transformation matrix [Zt],
Homogeneous transformation matrix [Zw] stored in RAM 102,
[Et] and [Ew], and a homogeneous transformation matrix [T] indicating the calculated position and orientation of the positioner 3.
p] and the joint variable θs obtained by the second calibration process
3 based on a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 obtained by using Equation (3) and a homogeneous transformation matrix “Tma” indicating the position and orientation of the robot 1,
A homogeneous transformation matrix [wXt] indicating the position and orientation of the tip Tool of the tool 4 with respect to the workpiece coordinate system Σwork at the position A4 is calculated.

[0121]

[Equation 63]

At the position A4, the tip OTool of the tool 4 based on the work coordinate system $ work
Based on a homogeneous transformation matrix [wXt] indicating the position and orientation of
The position (xtool1, ytool1, ztool1) of the tip OTool of the tool 4 at the position A4 is
This is expressed by using the following Expression 64, and the coordinate value of the position is calculated.

[Number 64] ^{xtool1 = tx w ytool1 = ty w} ztool1 = tz w

Next, in step S320, the tip O Tool of the tool 4 is moved from the position A4 in the Y-axis direction of the work coordinate system Σwork by a predetermined moving amount DL4 to the position B4 as shown in FIG. A homogeneous transformation matrix [wXt ′] indicating the position and orientation of the tip O Tool of the tool 4 with reference to the workpiece coordinate system Σwork at this time is calculated as follows.

First, the tip end moving amount DL4 of the tool 4 in the Y-axis direction of the workpiece coordinate system Σwork is added to the position component of the tip O Tool of the tool 4 at the position A4 calculated by using the above equation (64). Position B4
Of the tip Otool of the tool 4 at the position (xtool
2, ytool2, ztool2) is calculated using Equation 65.

Xtool2 = tx ^w tool2 = ty ^w + DL4 ztool2 = tz ^w

Next, the tip Oto of the tool 4 based on the work coordinate system Σwork at the position A4 is described.
3 in the homogeneous transformation matrix [wXt] indicating the position and orientation of ol
Based on the posture matrix of rows × 3 columns and the position (xtool2, ytool2, ztool2) of the tip Otool of the tool 4 at the position B4 calculated using the above equation 65, the work coordinate system Σwo at the position B4
A homogeneous transformation matrix [wXt '] indicating the position and orientation of the tip OTool of the tool 4 with respect to rld is expressed by the following equation 66.

[Equation 66]

In step S322, position A
When moving from position 4 to position B4, the slider 2 and the positioner 3 are not operated.
The values of sj (j = 1, 2) and θpk (k = 1, 2) do not change. That is, the calculated homogeneous transformation matrix [Tp] indicating the calculated position and orientation of the positioner 3 and the calculated homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 do not change. Therefore, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B4 is converted into the homogeneous transformation matrices [Ts], [Tp], [wX] indicating the position and orientation.
t ′], and calibrated by the first calibration process and R
Based on the homogeneous transformation matrix [Zt ″] stored in the AM 102 as the homogeneous transformation matrix [Zt] and the homogeneous transformation matrices [Zw], [Et], [Ew] stored in the RAM 102. , Using the following equation 67:

[Tma ′] = [Ts] ⁻¹ · [Zt ″] ⁻¹ · [Zw] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

Further, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B4 is inversely transformed, and each joint variable θmai (i = 1,
After calculating (2,..., 6), the motor command position Pmai (i = 1, 2,.
6) is calculated. The calculated motor command position Pma
The data of i is a dual port RAM as shown in FIG.
The data is transferred to the motors MO1 to MO6 via the latch circuit 104, the latch circuit 105, and the servo control circuit 106, thereby moving each joint of the robot 1 and the tip Ot of the tool 4.
cool is located at position B4.

Next, in step S323, the sensor detection distance l2 of the distance sensor SS1 at the position B4 is loaded into the RAM 102. Then, step S3
At 24, the sensor detection distance l at the position A4
It is determined whether 1 and the sensor detection distance l2 at the position B4 are equal. If l1 = l2, it is determined that the angle γwz calibration process has been completed, and the routine returns. On the other hand, if l1 ≠ l2, step S325
Proceed to.

In step S325, the amount of shift △ DL4 of the tip OTool of the tool 4 with respect to the table plane of the positioner 3 is calculated using the following equation 68 based on the detected sensor detection distances l1 and l2.

６８DL4 = l2-l1

Next, in step S326, the torsion angle △ γwz around the Z-axis of the work base coordinate system と し た Op based on the absolute coordinate system Σworld is determined based on the calculated shift amount △ DL4 and the movement amount DL4. , Using the following equation 69:

６９γwz = sin ⁻¹ (△ DL4 / DL4)

Then, as shown in the following equation (70), using the torsion angle △ γwz as a correction value, the RAM 102
Is corrected for the posture component γwz around the Z axis of the work base coordinate system Σp based on the absolute coordinate system Σworld stored in
It is stored in the RAM 102 as z.

[Mathematical formula-see original document] γwz '= γwz + △ γwz

Next, the calibration value γwz ′ of the posture component is obtained.
Is substituted into Equation 5, and a homogeneous transformation matrix [Zw '] indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld with respect to the calibration value γwz' is calculated using the following Equation 71. .

[Zw ′] = Trans (awz, bwz, cwz) · Rot (X, αwz) · Rot (Y, βwz) · Rot (Z, γwz ′)

Further, in step S329, a homogeneous transformation matrix [Zw '] indicating the calculated position and orientation is obtained by:
[Zw] is stored in the RAM 102 and updated. As a result, the process returns because the calibration process for the angle γwz has been completed.

(3-4-2) Angle βwz Calibration Process (Step S32) FIG. 35 is a flowchart showing the angle γwz calibration process (step S32). In this process, the torsion angle △ βwz, which is an installation error, is detected. To calibrate the angle βwz.

First, in step S410, R
Joint variable θp1 of positioner 3 stored in OM101
And θp2 fixed value data θp1 = 0 °, θp2 = 0 °
Take in. Next, the joint variable θp of the positioner 3
After calculating the motor command position Ppk (k = 1, 2) of the positioner 3 with respect to k (k = 1, 2) using Equation 62 obtained by transforming Equation 49, the calculated data Through the dual port RAM 104, the latch circuit 105, and the servo control circuit 107.
7. Transfer to the MO8 to move the positioner 3 to the initial setting position. Next, as shown in FIG. 21, a distance sensor SS1 for measuring the distance between the tip OTool of the tool 4 and the table plane of the positioner 3 by the operator.
Is mounted by the operator such that its sensor axis is perpendicular to the table plane of the positioner 3. The operator is informed that the preparation work is completed.
Is notified to the CPU 100 by using, the determination of the reception of the ready signal in step S411 is YE
In S, the control device 5 executes the following processing assuming that each of the machines 1, 2, 3 is at the position A5.

Next, in step S412, the values of the A matrix of the first axis and the second axis of the positioner 3 are calculated by the above equation (6).
Is calculated by substituting the joint variables θp1 and θp2 fetched in the above-mentioned initial setting.

Further, Ap0 and Ap3 are calculated based on the parameter values of the links L20 and L23 in Table 3 above. The A matrix Apk (i = 0,
By substituting (1, 2, 3) into Equation 9, a homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3 is calculated.

Next, the operating position dsj (j
= 1, 2) is the dual port RAM 10 in FIG.
4, the motor command position Psj (j = 1, 2) is read out, and is calculated using the above equation (48).

Further, Asj (j = 1, 2) of the slider 2 is calculated by using the above equation 51, and the joint variable θs3 of the DH link parameter of the slider 2 already calibrated by the second calibration process is used. A matrix Asj of slider 2
After calculating (j = 0,3), a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 is calculated using the above equation (8).

Next, each joint variable θmai of the robot 1
(I = 1, 2,..., 6) is the motor command position Pmai (i = 1, 2) from the dual port RAM 104 in FIG.
,..., 6) are read out and calculated using the above equation (47). Further, the A matrix A of the robot 1 is obtained by using the above equation (50).
After calculating mai (i = 0, 1, 2,..., 6), a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1 is calculated using the above equation (7).

Thus, the processing in step S412 is completed.
Next, in step S416, the sensor detection distance 11 of the distance sensor SS1 at the position A5 is stored in the RAM.
Take in 102.

Next, in step S418, the first
And the same transformation matrix [Zt ″] calibrated by the calibration process and stored in the RAM 102, and the angle γwz is calibrated by the angle γwz calibration process of the third calibration process and stored in the RAM 102. Order transformation matrix [Z
w '] and the homogeneous transformation matrices [Et] and [Ew] stored in the RAM 102, and the computed homogeneous transformation matrix [T
p] and the joint variable θs obtained by the second calibration process
3 based on a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 obtained by using Equation 3 and a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1,
A homogeneous transformation matrix [wXt] indicating the position and orientation of the tip Tool of the tool 4 at the position A5 with reference to the workpiece coordinate system Σwork is calculated.

[0143]

[Equation 72]

The tip Otoo of the tool 4 based on the work coordinate system $ work at the position A5.
Based on the homogeneous transformation matrix [wXt] indicating the position and orientation of l, the tip OTool of the tool 4 at the position A5
Position (xtool3, ytool31, ztool)
3) is expressed using the following equation 73, and the coordinate value of the position is calculated.

[Number 73] ^{xtool3 = tx w 'ytool3 = ty} w' ztool3 = tz w '

Next, in step S420, the tip O Tool of the tool 4 is moved from the position A5 in the X-axis direction of the work coordinate system Σwork by a predetermined movement amount DL5 to the position B5 as shown in FIG. A homogeneous transformation matrix [wXt ″] indicating the position and orientation of the tip O Tool of the tool 4 based on the workpiece coordinate system Σwork at this time is calculated as follows.

First, the tip movement amount DL5 of the tool 4 in the X-axis direction of the work coordinate system Σwork is added to the position component of the tip O Tool of the tool 4 at the position A5 calculated using the above equation 74. Position B5
Of the tip Otool of the tool 4 at the position (xtool
4, ytool4, ztool4) is calculated using equation 74.

[Number 74] ^{xtool4 = tx w '+ DL5 ytool4} = ty w' ztool4 = tz w '

Next, the tip Oto of the tool 4 at the position A5 with reference to the workpiece coordinate system Σwork.
3 in the homogeneous transformation matrix [wXt] indicating the position and orientation of ol
Based on the attitude matrix of rows × 3 columns and the position (xtool4, ytool4, ztool4) of the tip Otool of the tool 4 at the position B4 calculated using the above equation 74, the work coordinate system Σwo at the position B5
A homogeneous transformation matrix [wXt ″] indicating the position and orientation of the tip OTool of the tool 4 on the basis of rld is expressed by the following equation (75).

[0148]

[Equation 75]

At step S422, position A
When moving from position 5 to position B5, the slider 2 and the positioner 3 are not operated.
The values of sj (j = 1, 2) and θpk (k = 1, 2) do not change. That is, the calculated homogeneous transformation matrix [Tp] indicating the calculated position and orientation of the positioner 3 and the calculated homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 do not change. Accordingly, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B5 is replaced with the homogeneous transformation matrices [Ts], [Tp], [wX] indicating the position and orientation.
Based on t ′] and the homogeneous transformation matrices [Zt ″], [Zw ′], [Et], and [Ew] stored in the RAM 102, the following equation is used to calculate.

[0150]

[Tma ′] = [Ts] ⁻¹ · [Zt ″] ⁻¹ · [Zw ′] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

Further, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B5 is inversely transformed, and each joint variable θmai (i = 1,
After calculating (2,..., 6), the motor command position Pmai (i = 1, 2,.
6) is calculated. The calculated motor command position Pma
The data of i is a dual port RAM as shown in FIG.
The data is transferred to the motors MO1 to MO6 via the latch circuit 104, the latch circuit 105, and the servo control circuit 106, thereby moving each joint of the robot 1 and the tip Ot of the tool 4.
The position of “ool” is located at the position B5.

Next, in step S423, the sensor detection distance l2 of the distance sensor SS1 at the position B5 is loaded into the RAM 102. Then, step S4
At 24, the sensor detection distance l at the position A5
It is determined whether 1 and the sensor detection distance l2 at the position B5 are equal. If l1 = l2, it is determined that the angle βwz calibration processing has been completed, and the routine returns. On the other hand, if l1 ≠ l2, step S425
Proceed to.

In step S425, the amount of deviation △ DL5 of the tip Otool of the tool 4 with respect to the table plane of the positioner 3 is calculated using the following equation 77 based on the detected sensor detection distances l1 and l2.

７７DL5 = l2-l1

Next, in step S426, the torsion angle △ βwz around the Y axis of the work base coordinate system ΣOp based on the absolute coordinate system Σworld is determined based on the calculated shift amount △ DL5 and the movement amount DL5. , Using the following equation 78.

７８βwz = sin ⁻¹ (△ DL5 / DL5)

Then, as shown in the following Expression 79, the RAM 102 is used by using the torsion angle △ βwz as a correction value.
Is corrected for the posture component βwz around the Z axis of the work base coordinate system Σp based on the absolute coordinate system Σworld stored in
It is stored in the RAM 102 as z.

[Expression 79] βwz ′ = βwz + △ βwz

Next, the calibration value βwz ′ of the posture component
Is substituted for the calibration value βwz ′, and a homogeneous transformation matrix [Zw ″] indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld with respect to the calibration value βwz ′ is expressed by the following equation 80
Is calculated using

[Zw ″] = Trans (awz, bwz, cwz) · Rot (X, αwz) · Rot (Y, βwz ′) · Rot (Z, γwz)

Further, in step S429, a homogeneous transformation matrix [Zw ″] indicating the calculated position and orientation is
[Zw] is stored in the RAM 102 and updated. This completes the calibration process for the angle βwz, and returns.

(3-4-3) Angle αwz Calibration Processing (Step S33) FIG. 36 is a flowchart showing the angle αwz calibration processing (step S33). In this processing, the torsion angle △ αwz which is an installation error is detected. To calibrate the angle αwz.

First, in step S510, R
Joint variable θp1 of positioner 3 stored in AM101
And θp2 fixed value data θp1 = 0 °, θp2 = 0 °
Take in. Next, the joint variable θp of the positioner 3
After calculating the motor command position Ppk (k = 1, 2) of the positioner 3 with respect to k (k = 1, 2) using Equation 61 obtained by transforming Equation 49, the calculated data Through the dual port RAM 104, the latch circuit 105, and the servo control circuit 107.
7. Transfer to the MO8 to move the positioner 3 to the initial setting position. Next, as shown in FIG. 22, the operator sets a distance sensor SS1 for measuring the rear distance between the tip O Tool of the tool 4 and the table plane of the positioner 3 with its sensor axis perpendicular to the table plane of the positioner 3. The operator attaches it so that The operator informs the CPU 100 of the completion of the preparation work by using the teaching box 20, so that the determination of the preparation completion signal in step S511 becomes YES, and the control device 5 determines that each of the machines 1, 2, and 3 is in position. The following process is performed assuming that the document is in A5.

Next, in step S512, the values of the A matrix of the first axis and the second axis of the positioner 3 are calculated by the above equation (6).
Is calculated by substituting the joint variables θp1 and θp2 fetched in the above-mentioned initial setting.

Further, Ap0 and Ap3 are calculated based on the parameter values of the links L20 and L23 in Table 3 above. The A matrix Apk (i = 0,
By substituting (1, 2, 3) into Equation 9, a homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3 is calculated.

Next, the operating position dsj (j
= 1, 2) is the dual port RAM 10 in FIG.
4, the motor command position Psj (j = 1, 2) is read out, and is calculated using Expression 48.

Further, the A matrix Asj (j = 1, 2) of the slider 2 is calculated by using Expression 51, and the joint variable θs3 of the DH link parameter of the slider 2 already calibrated by the second calibration process is calculated. And the remaining A matrix Asj (j =
After calculating (0, 3), a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 is calculated using the above equation (8).

Next, each joint variable θmai of the robot 1
(I = 1, 2,..., 6) is the motor command position Pmai (i = 1, 2) from the dual port RAM 104 in FIG.
,..., 6) are read out and calculated using Equation 47. Further, the A matrix Amai (i
= 0, 1, 2,..., 6), and then calculates a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1 using Expression 7.

Thus, the processing in step S512 is completed.
Next, in step S516, the sensor detection distance 11 of the distance sensor SS1 at the position A6 is stored in the RAM.
Take in 102.

Next, in step S518, the first
And the angles γwz and βwzga are calibrated by the homogeneous transformation matrix [Zt ″] calibrated by the calibration process and stored in the RAM 102, and the angle γwz calibration process and the angle βwz calibration process of the third calibration process. And RA in advance
[Zw ″] stored in M102 and RAM 10
2 and a homogeneous transformation matrix [Tp] indicating the calculated position and orientation of the positioner 3 and a second calibration process. Based on a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 obtained using the obtained joint variable θs2 and a homogeneous transformation matrix [Tma] indicating the position and orientation of the robot 1, the following equation 81 is obtained. Using the same transformation matrix [wXt] indicating the position and orientation of the tip O Tool of the tool 4 based on the workpiece coordinate system Σwork at the position A6.
Is calculated.

[0167]

[Equation 81]

At the position A6, the tip OTool of the tool 4 with reference to the workpiece coordinate system 基準 work
Based on a homogeneous transformation matrix [wXt] indicating the position and orientation of
The position (xtool5, ytool5, ztool5) of the tip OTool of the tool 4 at the position A6 is
It is expressed using the following equation 82, and the coordinate value of the position is calculated.

[Number 82] ^{xtool5 = tx w '' ytool5 =} ty w '' ztool5 = tz w ''

Next, in step S520, the tip O Tool of the tool 4 is moved from the position A6 in the Y-axis direction of the work coordinate system Σwork by a predetermined moving amount DL6 to the position B6 as shown in FIG. A homogeneous transformation matrix [wXt ′ ″] indicating the position and orientation of the tip O Tool of the tool 4 based on the workpiece coordinate system Σwork at this time is calculated as follows.

First, the tip end moving amount DL6 of the tool 4 in the X-axis direction of the work coordinate system Σwork is added to the position component of the tip O Tool of the tool 4 at the position A5 calculated using the equation (82). Position B6
Of the tip Otool of the tool 4 at the position (xtool
6, ytool6, ztool6) using the following equation 83.

[Number 83] ^{xtool6 = tx w '' ytool6 =} ty w '' + DL6 ztool6 = tz w ''

Next, the tip Oto of the tool 4 at the position A6 with reference to the workpiece coordinate system Σwork.
3 in the homogeneous transformation matrix [wXt] indicating the position and orientation of ol
Based on the posture matrix of rows × 3 columns and the position (xtool6, ytool6, ztool6) of the tip Otool of the tool 4 at the position B6 calculated using the above equation 83, the work coordinate system Σwo at the position B6
A homogeneous transformation matrix [wXt ′ ″] indicating the position and orientation of the tip OTool of the tool 4 based on rld is expressed by the following equation 84.

[Equation 84]

At step S522, position A
When moving from position 6 to position B6, the slider 2 and the positioner 3 are not operated.
The values of sj (j = 1, 2) and θpk (k = 1, 2) do not change. That is, the calculated homogeneous transformation matrix [Tp] indicating the calculated position and orientation of the positioner 3 and the calculated homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 do not change. Accordingly, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B5 is replaced with the homogeneous transformation matrices [Ts], [Tp], [wX] indicating the position and orientation.
t ′], and homogeneous transformation matrices [Zt ″], [Zw ″], [Et], [Ew] stored in the RAM 102.
Is calculated based on the following equation (85).

[Tma '] = [Ts] ⁻¹ · [Zt ″] ⁻¹ · [Zw ″] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

Further, the homogeneous transformation matrix [Tma '] indicating the position and orientation of the robot 1 at the position B6 is inversely transformed, and each joint variable θmai (i = 1,
After calculating (2,..., 6), the motor command position Pmai (i = 1, 2,.
6) is calculated. The calculated motor command position Pma
The data of i is a dual port RAM as shown in FIG.
The data is transferred to the motors MO1 to MO6 via the latch circuit 104, the latch circuit 105, and the servo control circuit 106, thereby moving each joint of the robot 1 and the tip Ot of the tool 4.
The position of “ool” is located at the position B6.

Next, at step S523, the sensor detection distance l2 of the distance sensor SS1 at the position B6 is loaded into the RAM 102. Then, step S5
At 24, the sensor detection distance l at the position A6
It is determined whether or not 1 is equal to the sensor detection distance l2 at the position B6. If l1 = l2, it is determined that the angle αwz calibration process has been completed, and the routine returns. On the other hand, if l1 ≠ l2, step S525
Proceed to.

In step S525, based on the detected sensor detection distances l1 and l2, the amount of deviation △ DL6 of the tip OTool of the tool 4 with respect to the table plane of the positioner 3 is calculated using the following equation 86.

８６DL6 = l2-l1

Next, in step S526, the twist angle △ αwz about the X axis of the work base coordinate system ΣOp based on the absolute coordinate system Σworld is determined based on the calculated shift amount △ DL6 and the movement amount DL6. Is calculated using the following equation 87.

８７αwz = sin ⁻¹ (△ DL6 / DL6)

Then, as shown in the following Expression 88, the RAM 102 is used by using the torsion angle △ αwz as a correction value.
Is corrected for the posture component αwz around the Z-axis of the work base coordinate system Σp based on the absolute coordinate system Σworld stored in
It is stored in the RAM 102 as z.

[Mathematical formula-see original document] αwz '= αwz + ｗαwz

Next, the calibration value αwz ′ of the posture component is obtained.
Is substituted into Equation 5, and a homogeneous transformation matrix [Zw ′ ″] indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld with respect to the calibration value αwz ′ is expressed by the following Equation 8.
9 is used.

[Zw ″ ′] = Trans (awz, bwz, cwz) · Rot (X, αwz ′) · Rot (Y, βwz) · Rot (Z, γwz)

Further, in step S529, a homogeneous transformation matrix [Zw '"] indicating the calculated position and orientation is obtained.
Is stored in the RAM 102 and updated as [Zw]. Then, since the calibration process for the angle αWZ is completed, the process returns.

(3-5) Fourth Calibration Process FIG. 37 is a flowchart of the fourth calibration process (step S34). In the fourth calibration process, the absolute coordinate system Σw
The position components awz, bwz, and cwz of the ord and the work base coordinate system Σp set at the origin Op of the positioner 3 are set as calibration targets, and these error parameters are represented by the position error △ awz as shown in FIGS. , △ bwz,
Δcwz. In the fourth calibration process, errors 当 該 awz, △ bwz, △ cwz of the position are detected, and calibration is performed to eliminate the error.

Data relating to the homogeneous transformation matrix [Ew] indicating the position and orientation of the work coordinate system Σwork with reference to the work-side mechanical interface coordinate system Σwm is given by the following equation (3).
As shown by 0 to Expression 35, all are set to 0. Therefore, the work coordinate system Σwork is set at the center of the table plane of the positioner 3 so as to coincide with the work-side mechanical interface coordinate system Σwm. Therefore, a rectangular parallelepiped block BK1 machined so that the surface becomes flat is placed so that one vertex thereof coincides with the center of the tabes plane of the positioner 3. That is, since one vertex of the block BK1 matches the origin of the work-side mechanical interface coordinate system Σwm, it matches the origin of the work coordinate system Σwork. Further, with one vertex of the block BK1 as a center, the block BK1
After rotating the block BK1 so that one side of the block BK1 on the right side from one vertex coincides with the positive direction of the X-axis of the workpiece-side mechanical interface coordinate system Σwm, the block BK1 is placed on the table plane of the positioner 3. Fix it. Then, as shown in FIG.
A sensor unit including three distance sensors SS1, SS2, and SS3 is attached to the tip of each of the sensors SS1 and SS3.
2 and SS3 are mounted as tools via jigs J1, J2 and J3 for arranging the sensor axes orthogonal to each other. Distance sensor SS in the above sensor unit
The distance between 1 and the origin Otool of the tool coordinate system Σtool is known, and it is stored in the ROM 101 in advance as lx0. The distance sensor SS1 measures the distance lx by measuring the side surface of the rectangular parallelepiped block BK1 placed in alignment with the surface defined by the Y axis and the Z axis of the work coordinate system Σwork, and the work coordinate system Σwo
The distance parallel to the X-axis of the work side mechanical interface coordinate system Σwm between the origin Owork of rk and the origin Otool of the tool coordinate system Σtool is distance (lx-lx0).
Used to calculate as Further, the distance sensor SS2 in the sensor unit and the tool coordinate system Σtool
Is known to the origin Otool, and it is ly0
Is stored in the ROM 101 in advance. The distance sensor SS2 measures the distance ly by measuring the distance ly by measuring the side surface of the rectangular parallelepiped block BK1 placed in alignment with the plane defined by the X axis and the Z axis of the work coordinate system Σwork. Is used to calculate the distance parallel to the Y axis of the workpiece-side mechanical interface coordinate system Σwm between the origin Owork of the tool coordinate system Σtool and the origin Otool of the tool coordinate system Σtool as the distance (ly-ly0). Further, the distance sensor S in the sensor unit
The distance between S3 and the origin Otool of the tool coordinate system Σtool is known, and it is previously set as lz0 in the ROM 101.
Is stored in The distance sensor SS3 measures the distance lz by measuring a plane (ie, the plane of the positioner 3) formed by the X axis and the Y axis of the work coordinate system Σwork, and the origin Ow of the work coordinate system Σwork.
Work-side mechanical interface coordinate system between ork and tool coordinate system 原点 tool origin Otool Ｚwm Z
It is used to calculate the distance parallel to the axis as the distance (lz-lz0).

First, in step S610, the absolute coordinate system {world} calibrated in the third calibration process is set.
calibration values αwz ′, βwz ′ of posture components around the X-axis, Y-axis and Z-axis of the work base coordinate system Σp based on d
Import γwz '. Next, in step S611, each side surface of the rectangular parallelepiped block BK1 is moved to the absolute coordinate system Σw.
In order to rotate by the calibration value γwz ′ of the posture component about the Z axis of the work base coordinate system Σp based on the absolute coordinate system Σworld so as to be parallel to the X axis and the Y axis of Using the second axis L22 of the positioner 3
The rotation angle (joint variable) θp2 around is calculated.

[0183]

Where θp2 = (− 1) × γwz ′ where the joint variable θp1 of the first axis L21 of the positioner 3
Is set to 0, and RAM1 is added together with θp2 calculated by Equation 90.
02 is stored.

Then, the motor command position Ppk (k = 1, 2) for the joint variables θp1 and θp2 is calculated by using equation (61).

The data of the motor command position Ppk is transferred to the motors MO7 and MO8 via the dual port RAM 104, the latch circuit 105 and the servo control circuit 107, and the positioner 3 is rotated around its second axis L22 by a rotation angle θp2 [ rad].

Further, based on the joint variables θp1 and θp2, the values of the A matrix of the first axis and the second axis of the positioner 3 are calculated using the above equation (62).

Further, based on the values of the DH link parameters of the positioner 3 in Table 3, the A matrices Ap0 and Ap
3 is also calculated. Next, using the equation 9, the positioner 3
Is calculated. [Tp] is calculated.

Next, in step S614, the homogeneous transformation matrix data [wXt] indicating the position and orientation of the tool 4 with respect to the work coordinate system Σwork, which has been created in advance under the following conditions and stored in the ROM 101, is obtained. take in. This data indicates that the tool, which is a sensor unit having three distance sensors SS1 to SS3, has the above-described block BK.
1 and the position of the tool 4 with respect to the table surface of the positioner 3 for measuring a predetermined distance as described above, and the attitude of the tool 4 with respect to the work coordinate system Σwork, and with the work coordinate system Σwork as reference. This is a homogeneous transformation matrix when all the positions of the tool 4 are set to 0.

In step S615, the ROM 1
01 is the predetermined operating position dsj of the slider 2 stored in
(J = 1, 2) to the motor command position Psj (j =
The data of the motor command position Psi is transferred to the motors MO9 and MO10 via the dual port RAM 104, the latch circuit 105 and the servo control circuit 108, and the slider 2 is operated. You.

The operation position dsj of the slider 2
Based on (j = 1, 2), the slider 2
Of the slider 2 using the joint variable θs3 of the DH link parameter of the slider 2 which has already been calibrated by the second calibration process, and calculates the A matrix Asj (j = 0) of the slider 2 , 3), a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 is computed using Expression 8 and stored in the RAM 102.

Further, similarly, the D-
Since the A matrices As0 and As3 are also calculated based on the link parameter value of H, a homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2 is calculated by using the following equation (8).
Is calculated for the operation position dsj (j = 1, 2).

Next, in step S618, RA
Homogeneous transformation matrix [Zt ″] stored in M102,
[Zw ″], [Et], and [Ew] are fetched. Then, the position and orientation of the robot 1 that operates the robot 1 such that the position Otool of the tip of the tool 4 is moved to the origin Owork of the work coordinate system Σwork on the calculation [Tm
a] is calculated from the calculated homogeneous transformation matrix [Tp] indicating the position and orientation of the positioner 3, the calculated homogeneous transformation matrix [Ts] indicating the position and orientation of the slider 2, and the fetched homogeneous transformation. Matrices [Zt ″], [Zw ″], [E
t], [Ew], and a homogeneous transformation matrix [wXt] indicating the position and orientation of the tool 4 with respect to the work coordinate system 取 り work taken in step S614 as a reference.

[0193]

[Tma] = [Ts] ⁻¹ · [Zt ″] ⁻¹ · [Zw ″] · [Tp] · [Ew] · [wXt] · [Et] ⁻¹

Furthermore, the position OTool of the tip of the tool 4
Is a homogeneous transformation matrix [Tm] indicating the position and orientation of the robot 1 at the origin Owork of the work coordinate system 上 の work on the computation.
a] is inversely transformed to obtain each joint variable θmai of the robot 1.
(I = 1, 2,..., 6), and using the above equation (56), the motor command position Pmai (i = 1, 1) of the robot 1
2,..., 6) are calculated. These motor command positions P
mai data is transmitted to the motor M via the dual port RAM 104, the latch circuit 105, and the servo control circuit 106.
It is transferred to O1 to MO6, whereby the robot 1
Are rotated, and the position Otool of the tip of the tool 4 is positioned at the origin Owork of the work coordinate system Σwork on the calculation.

Next, in step S620, the position OTool of the tip of the tool 4 is calculated in the work coordinate system Σ.
Distance sensor SS located at the origin Owork of the work
1, SS2, SS3 sensor detection distance lx, ly, lz
Is detected and stored in the RAM 102.

The stored detection distances lx, ly, lz
Is the work coordinate system @wo from each of the distance sensors SS1-SS3.
This is the distance to the origin Owork of rk. Therefore, R
The distances lx0, ly0, and lz0 from the distance sensors SS1-SS3 stored in the OM 101 to the origin Otool of the TOOL coordinate system Σtool are respectively compared, and the position of the origin Owork of the work coordinate system Σwork and the origin Otool of the tool coordinate system Σtool are compared. It is determined in step S621 whether the positions match. Machines 1, 2,
If there is no error between the three, they match. This is because all the position data among the position and orientation data of the tool 4 based on the work coordinate system 取 り 出 し work, which is taken out in step S614 and is the basis of the subsequent calculation, is set to 0 as described above. When they match in this way, it is determined that the fourth calibration process has been completed, and the routine returns. On the other hand, if they do not match, the process proceeds to step S622.

In step S622, the shortest distances lx0, ly0, lz0 between the tool coordinate system Σtool and the end faces of the distance sensors SS1, SS2, SS3, and the sensor detection distances lx, l detected in step S620.
y, lz and the calibration value α of the posture component around the X-axis, Y-axis, and Z-axis of the work base coordinate system Σp based on the absolute coordinate system Σworld taken in step S610.
Based on wz ′, βwz ′, and γwz ′, the error 位置 awz, △ bwz, △ cwz of the position component of the work base coordinate system Σp based on the absolute coordinate system Σworld is calculated using the following equation 92. .

[0198]

Δawz = {(lx−lx0) − (lz−lz0) · tanβwz ′} · cosβwz ′ Δbwz = {(ly−ly0) − (lz−lz0) · tanαwz ′} · cosαwz ′ Δcwz = {(Lz-lz0) · cosαwz '· cosβwz'

Next, in step S623, the errors △ awz, △ bwz, △ cw of the position components calculated above are calculated.
Using z as a correction value, the position components awz, bwz, and cwz of the work base coordinate system Σp based on the absolute coordinate system Σworld stored in the RAM 102 are given by the following equation (9).
3 and the calibration values awz ′, bwz ′,
cwz 'is calculated.

[0200]

Awz '= awz + △ awz bwz' = bwz + △ bwz cwz '= cwz + △ cwz

Further, the calculated calibration value a of the position component
Substituting wz ', bwz', and cwz 'into Equation 5 above, the homogeneous transformation indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld with respect to the calibration values awz', bwz ', and cwz'. The matrix [Zw ″ ″] is calculated using the following equation (94).

[Zw ″ ″] = Trans (awz ′, bwz ′, cwz ′) · Rot (X, αwz) · Rot (Y, βwz) · Rot (Z, γwz)

Next, the calculated calibration values awz ',
A homogeneous transformation matrix [Zw ″ ″] indicating the position and orientation of the work base coordinate system Σp based on the absolute coordinate system Σworld with respect to bwz ′ and cwz ′ is defined as [Zw] in the RAM.
Stored in 102 and updated. As a result, the process returns assuming that the fourth calibration process has been completed.

(4) Teaching operation and playback operation By executing the installation error calibration processing of the embodiment according to the present invention immediately after the installation of the robot system,
FIG. 4 shows an installation relationship between the robot 1, the slider 2, and the positioner 3, and a homogeneous transformation matrix [Zw], [Zt], [Ts] stored in the RAM 102 in the control device 5 corresponds to an actual installation state. Can be modified to the value given. Thereafter, after performing a teaching operation on the robot system, the robot system is caused to perform an automatic operation as a reproducing operation.

If the calibration process is not executed, the problem that occurs in the teaching operation and the reproduction operation is determined by the above-mentioned homogeneous transformation matrices [Zw], [Zt], and [Ts], and the calibration process is not executed. This will be described below using the homogeneous transformation matrices [Zwq], [Ztq], and [Tsq] indicating the installation relation at the time. The contents of the work performed by the robot system are such that both the work handling device 12 and the tool moving device 11 are moved, and the relative operation between the work and the tool 4 is performed.

Of the calculations executed by the control device 5 for the robot system of this embodiment, the absolute coordinate system Σworld
The homogeneous transformation matrix [worldXwork] indicating the position and orientation of the workpiece coordinate system Σp based on the following equation 95 can be expressed by the following equation 95, and the homogeneous transformation matrix indicating the position and orientation of the tool coordinate system Σtool based on the absolute coordinate system Σworld Conversion matrix [wo
rldXtool] can be expressed by the following equation 96.

[0206]

[Equation 95] [worldXwork] = [Zwq] · [Tp] · [Ew]

[WorldXTool] = [Ztq] · [Tsq] · [Tma] · [Et]

Therefore, from the above equations (95) and (96), the tool coordinate system {too} based on the workpiece coordinate system {work} is used.
H [homogeneous transformation matrix [workXtoo]
l] can be calculated using the following equation 97.

[WorkXtool] = [worldXwork] ^-1 · [worldXtool] = [Ew] ^-1 · [Tp] ^-1 · [Zwq] ^-1 · [Ztq] · [Tsq] · [Tma] · [Et] ]

In the teaching operation, for example, an integer i-th homogeneous transformation matrix [workXto] indicating the position and orientation of the tool coordinate system Σtool based on the work coordinate system Σwork
ol] using the above equation (97), and then [workX
tool] i in the RAM 102 and the next integer (i + 1) -th homogeneous transformation matrix thereof is [workXto
ol] i + 1 is stored in the RAM 102. Here, the stored data of both homogeneous transformation matrices [workXt
When [tool] i and [workXtool] i + 1 include a large installation error, the same operation is repeated in the same manner, and a work program based on the stored data including the large error is created.

When the reproducing operation is performed based on such a work program, the operations from the i-th to the (i + 1) -th operations are performed by the homogeneous transformation matrix [workXtooo] having an installation error.
The operation is performed based on the moving distance Lq having a large installation error, which is calculated by [l] i and the homogeneous transformation matrix [workXtool] i + 1. The required time Tsq required for the operation during this time is the specified i-th relative speed Vi and the moving distance Lq having the large installation error.
Is calculated by the following equation (98). As a result, the actual moving distance L on the work and the expected required time based on the i-th relative speed Vi are greatly different from each other. That is, there is a problem that the specified relative speed Vi cannot be realized.

[Equation 98] Tsq = Lq ÷ Vi

Therefore, by performing the above-described installation error calibration processing of this embodiment immediately after the installation of the robot system, the installation error is eliminated, so that when performing the reproduction operation performed after the teaching operation, It is possible to realize substantially the same work content as the work content instructed in the teaching work, and to solve the above-described problem that "a desired relative speed cannot be realized between the work and the tool 4", for example. Can be.

(5) Modified Example (5-1) First Modified Example In the above-described embodiment, as shown in FIG. Since ε3 and ε4 do not exist, as shown in FIG. 27, only a third calibration process (step S3) for calibrating the installation error ε2 and a fourth calibration process (step S4) for calibrating the installation error ε1 are sequentially performed. It may be configured to execute.

The formulas handled in this modified example are basically the same as those in the above-described embodiment, but a description will be given of a case where the slider 2 is not provided. Since the slider 2 does not exist, the homogeneous transformation matrix [Ts] of the position and orientation of the slider 2 becomes unnecessary. That is, Equation 4 becomes unnecessary. Further, there is no tool base coordinate system Δs and a tool side mechanical interface coordinate system Δtm set at the mechanical origin of the slider 2. Therefore, the absolute coordinate system @wo
The homogeneous transformation matrix [Zt] representing the position and orientation of the tool base coordinate system Σs based on rld is unnecessary, and the robot 1
The tool machine coordinate system Σm set to the mechanical origin Om of the above and the absolute coordinate system Σworld are handled as being coincident. Here, since the homogeneous transformation matrices [Ts] and [Zt] are not required, a mathematical expression relating to the position and orientation of each component of the robot system, that is, Equation 11 in the above embodiment is used.
Is replaced by the following Expression 99.

[Tma] · [Et] = [Zw] · [Tp] · [Ew] · [wXt]

In the third calibration process (step S3) for calibrating the installation error ε2, the following changes are made.
First, in the calibration process of the angle γwz in step S31, the operation position dsj (j
= 1, 2) to the calculation of the homogeneous transformation matrix [Ts] are not required. Further, in step S318, it is not necessary to read the homogeneous transformation matrix [Zt], and the tip Oto of the tool 4 based on the work coordinate system Σwork is not required.
The homogeneous transformation matrix [wXt] indicating the position and orientation of ol is replaced by the following equation 100 from equation 63 in the above embodiment.

[Equation 100] Also, the position B calculated in step S322
The position and orientation [Tma '] of the robot 1 in 4 is replaced by the following expression 101 from expression 67 in the above embodiment.

[Tma ′] = [Zw] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

In the calibration processing of the angle βwz in step 32, the homogeneous transformation matrix [T] is read from the reading of the operating position dsj (j = 1, 2) of the slider 2 in step S411.
s] is not required. Further, step S41
8, the reading of the homogeneous transformation matrix [Zt] is unnecessary, and the homogeneous transformation matrix [wX] indicating the position and orientation of the tip OTool of the tool 4 with reference to the work coordinate system Σwork.
t] is replaced by the following equation 102 from equation 72 in the above embodiment.

[Equation 102] Also, the position B calculated in step S422
The position and orientation [Tma '] of the robot 1 in 4 is replaced by the following expression 103 from Expression 76 in the above embodiment.

[Tma ′] = [Zw ′] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

In the calibration processing of the angle αwz in step 33, the operating position dsj of the slider 2 in S511
The steps from reading (j = 1, 2) to calculating the homogeneous transformation matrix [Ts] become unnecessary. Further, in step S518, it is not necessary to read the homogeneous transformation matrix [Zt].
Tip O of tool 4 based on workpiece coordinate system @work
The homogeneous transformation matrix [wXt] indicating the position and orientation of the tool is replaced by the following equation 104 from equation 82 in the above embodiment.

[Equation 104] Further, the position B calculated in step S522
The position and orientation [Tma '] of the robot 1 in 4 is replaced by the following expression 105 from expression 85 in the above embodiment.

[Tma ′] = [Zw ″] · [Tp] · [Ew] · [wXt ′] · [Et] ⁻¹

In the fourth calibration process (step S4) for calibrating the installation error ε1, the following changes are made.
First, step S615 is skipped and not executed. Then, in step S618, the homogeneous transformation matrix [Z
It is not necessary to read out t], and the position and orientation [Tma] of the robot 1 calculated in step S618 is replaced by the following equation 106 from equation 91 in the above embodiment.

[Tma] = [Zw ″] · [Tp] · [Ew] · [wXt] · [Et] ⁻¹

(5-2) Second Modification In the above embodiment, as shown in FIG. 6B, when there is no positioner 3 in the industrial robot system, the installation error ε2 does not exist. As shown in FIG. 28, a first calibration process (Step S1) for calibrating the installation error ε3, a second calibration process (Step S2) for calibrating the installation error ε4, and a fourth calibration process for calibrating the installation error ε1. May be configured to sequentially execute only the calibration process (step S4).

The formulas handled in this modified example are basically the same as those in the above-described embodiment, but the following description deals with the absence of the components. Since the positioner 3 does not exist, the homogeneous transformation matrix [Tp] of the position and orientation of the positioner 3 becomes unnecessary. That is, Equation 9 becomes unnecessary. Also, there is no work base coordinate system に p and a work side mechanical interface coordinate system Σwm set at the mechanical origin of the positioner 3. Therefore, a homogeneous transformation matrix [Ew] representing the position and orientation of the work coordinate system Σwork with respect to the work side mechanical interface coordinate system Σwm is unnecessary. And the absolute coordinate system Σ wor
A homogeneous transformation matrix indicating the position and orientation of the work coordinate system Σwork based on ld is treated as a homogeneous transformation matrix [Zw]. Homogeneous transformation matrix [Tp] and homogeneous transformation matrix [Ew]
Becomes unnecessary, so the mathematical expression relating to the position and orientation of each component of the robot system, that is, Expression 11 in the above embodiment is replaced by Expression 107 below.

[Zt] · [Ts] · [Tma] · [Et] = [Zw] · [wXt]

In the second calibration process (step S2) for calibrating the installation error ε4, the following changes are made.
First, the calculation of the joint variable θpk (k = 1, 2) of the positioner 3 in step S210 becomes unnecessary, and in step S211 the calculation of the A matrix of the positioner 3 and the homogeneous transformation matrix [Tp indicating the position and orientation of the positioner 3 are performed. ]
Becomes unnecessary. Further, in S214, the tip Ot of the tool 4 based on the work coordinate system Σwork
The homogeneous transformation matrix [wXt] indicating the position and orientation of “ool” is replaced by the following equation 108 from equation 53 in the above embodiment.

[WXt] = [Zw] ⁻¹ · [Zt ″] · [Ts] · [Tma] · [Et] Further, the positioner B calculated in step S216
The position and orientation [Tma '] of the robot 1 in 3 is replaced by the following expression 109 from expression 55 in the above embodiment.

[Tma ′] = [Ts ′] ⁻¹ · [Zt ″] ⁻¹ · [Zw] · [wXt] · [Et] ⁻¹

In the fourth calibration process (step S4) for calibrating the installation error ε1, the following changes are made. First,
As a preparatory work for performing the calibration process, a disk (not shown) corresponding to the table plane of the positioner 3 is set on a work coordinate system which is a reference point of the work. Then, in the calibration process, steps S610 and S611
Skips and does not execute. Then, in step S618, it is not necessary to read the homogeneous transformation matrix [Ew], and the position and orientation [Tma] of the robot 1 calculated in step S618 is replaced by the following equation 110 from equation 91 in the above embodiment. In this modification, since the third calibration process (step S3) is not executed, the homogeneous transformation matrix [Zw] is the same as the initial setting.

[Tma] = [Ts] ⁻¹ · [Zt ″] ⁻¹ · [Zw] · [wXt] · [Et] ^{−1 Further} , in step S622, Expression 21, Expression 22,
According to Equation 23, the absolute coordinate system Σworld set in advance
Posture components αwz, βwz, γw of the homogeneous transformation matrix [Zw] indicating the position and posture of the workpiece coordinate system Σwork with reference to
z, and these are represented by αwz ′, βwz ′, and γw, respectively.
Treated as z '. And the absolute coordinate system $ world
The position and orientation of the workpiece coordinate system Σwork with reference to the error Δawz, Δbw of the position component of the homogeneous transformation matrix [Zw]
After calculating z and Δcwz, the calibration value aw of the position component
Compute z ', bwz', and cwz '.

(5-3) Third Modification In the above embodiment, as shown in FIG. 6C, when the slider 2 and the positioner 3 are not provided in the industrial robot system, only the installation error ε1 is set. Only exists,
As shown in FIG. 29, only the fourth calibration process (step S4) for calibrating the installation error ε1 may be performed.

The formulas handled in this modified example are basically the same as those in the above-described embodiment, but a description will be given below of the correspondence due to the absence of the components. Since the slider 2 does not exist, the homogeneous transformation matrix [Ts] of the position and orientation of the slider 2 becomes unnecessary. That is, Equation 4 becomes unnecessary. Further, there is no tool base coordinate system Σs and a tool side mechanical interface coordinate system Σtm set at the mechanical origin Os of the slider 2. Therefore, the absolute coordinate system Σ
The homogeneous transformation matrix [Zt] representing the position and orientation of the tool base coordinate system Σs based on the world is unnecessary, and the tool mechanical coordinate system Σm set at the mechanical origin Om of the robot 1 and the absolute coordinate system Σworld are one Treat as having done. Furthermore, since the positioner 3 does not exist,
The homogeneous transformation matrix [Tp] of the position and orientation of the positioner 3 becomes unnecessary. That is, Equation 9 becomes unnecessary. Positioner 3
Base coordinate system Σp and workpiece side mechanical interface coordinate system Σwm
There is no. Therefore, the workpiece coordinate system Σ wor based on the workpiece-side mechanism mechanical interface coordinate system Σ wm
The homogeneous transformation matrix [Ew] representing the position and orientation of k is unnecessary. Then, a homogeneous transformation matrix indicating the position and orientation of the work coordinate system Σwork with respect to the absolute coordinate system Σworld is
Treated as a homogeneous transformation matrix [Zw]. Since the homogeneous transformation matrices [Ts], [Zt], [Tp] and [Ew] are not required, a mathematical expression relating to the position and orientation of each component of the robot system, that is, Equation 11 in the above embodiment is
Replace with the following equation 111.

[Tma] · [Et] = [Zw] · [wXt]

In the fourth calibration process (step S4) for calibrating the installation error ε1, the following changes are made.
First, as a preparatory work for performing the calibration process, a disk (not shown) corresponding to the table plane of the positioner 3 is set on a work coordinate system which is a reference point of the work. Then, in the calibration process, first, step S610, step S610
611 and step S615 are skipped and not executed. Then, in step S618, the homogeneous transformation matrix [Z
t] and the homogenous transformation matrix [Ew] are unnecessary, and the homogenous transformation matrix [Tma] indicating the position and orientation of the robot 1 calculated in step S618 is calculated from Expression 91 in the above embodiment to Expression 112 below. replace. In this modification, since the third calibration process (step S3) is not executed, the homogeneous transformation matrix [Zw] is the same as the initial setting.

[Tma] = [Zw] · [wXt] · [Et] ^{−1 Further} , in step S622, Expression 21, Expression 22,
According to Equation 23, the absolute coordinate system Σworld set in advance
Posture components αwz, βwz, γw of the homogeneous transformation matrix [Zw] indicating the position and posture of the workpiece coordinate system Σwork with reference to
Using z, these are represented by αwz ′, βwz ′, and γ, respectively.
Handle as wz '. And the absolute coordinate system @world
The position and orientation of the workpiece coordinate system Σwork with respect to d are determined by the errors Δawz, Δbw of the position components of the homogeneous transformation matrix [Zw].
After calculating z and Δcwz, the calibration value aw of the position component
Compute z ', bwz', and cwz '.

When the fourth calibration process is performed without performing the third calibration process as in the second or third modification, the absolute coordinate system used as input data in step S610 is used as a reference. The posture components stored in the RAM 102 are used as the calibration values of the posture components of the work base coordinate system.

(5-4) Fourth Modification In the above embodiment, the positioner 3 is used for the work handling device 12, but the present invention is not limited to this.
A robot having a degree of freedom (not shown; hereinafter, referred to as a second robot) may be used. Differences in the processing in this case from the above embodiment will be described in detail below. Here, the difference is that the third and fourth calibration processes of the first to fourth calibration processes are different.
Is a calibration process.

(A) The homogeneous transformation matrix [Tp] indicating the position and orientation of the second robot is expressed by the following equation 113.

[Tp] = Ap0 ・ Ap1 ・ Ap2 ・ Ap3 ・ Ap4 ・ Ap5 ・ Ap6

(B) In the angle γwz calibration process (step S31 in FIG. 34) of the third calibration process, a disk (not shown) corresponding to the table plane of the positioner 3 is placed at the tip of the second robot. Let it be gripped. Then, the installation reference plane of the second robot is set to the work base coordinate system Σp. Here, in step S310, the disk is inclined by 90 ° with respect to the XY plane of the absolute coordinate system Σworld, that is, αwz = 0, βwz = −90 °, γ
wz = 0, and a homogeneous transformation matrix [Tp] · [Ew] indicating the position and orientation of the disk at that time is stored in the RAM 102 in advance.
To be stored. Next, the homogeneous transformation matrix [Tp] · [E
w], a homogenous transformation matrix [Tp] indicating the position and orientation of the second robot is calculated, and the homogenous transformation matrix [Tp] is inversely transformed to obtain each joint variable θpk of the second robot. (K = 1, 2,..., 6) and operate the second robot in the same manner as in the above embodiment.

(C) In the angle βwz calibration process (step S32 in FIG. 35) of the third calibration process, a disk (not shown) corresponding to the table plane of the positioner 3 is placed at the tip of the second robot. Let it be gripped. Then, the installation reference plane of the second robot is set to the work base coordinate system Σp. Here, in step S410, the disk is set in a state of being parallel to the XY plane of the absolute coordinate system Σworld, that is, αwz = 0, βwz = 0, γwz = 0, and the disk at that time is set. Homogeneous transformation matrix [T indicating the position and orientation
p] · [Ew] are stored in the RAM 102 in advance. Next, a homogeneous transformation matrix [Tp] indicating the position and orientation of the second robot is calculated based on the homogeneous transformation matrix [Tp] · [Ew], and the homogeneous transformation matrix [Tp] is inversely transformed. By doing, each joint variable θpk (k
= 1, 2,..., 6), and the second
Operate the robot.

(D) In the angle γwz calibration process (step S33 in FIG. 35) of the third calibration process, a disk (not shown) corresponding to the table plane of the positioner 3 is placed at the tip of the second robot. Let it be gripped. Then, the installation reference plane of the second robot is set to the work base coordinate system Σp. Here, in step S510, the disk is set to be in a state of being parallel to the XY plane of the absolute coordinate system Σworld, that is, αwz = 0, βwz = 0, γwz = 0, and the disk at that time is set. Homogeneous transformation matrix [T indicating the position and orientation
p] · [Ew] are stored in the RAM 102 in advance. Next, a homogeneous transformation matrix [Tp] indicating the position and orientation of the second robot is calculated based on the homogeneous transformation matrix [Tp] · [Ew], and the homogeneous transformation matrix [Tp] is inversely transformed. By doing, each joint variable θpk (k
= 1, 2,..., 6), and the second
Operate the robot.

(E) In the fourth calibration process (FIG. 37), a disk (not shown) corresponding to the table plane of the positioner 3 is gripped by the tip of the second robot. Then, the installation reference plane of the second robot is set to the work base coordinate system Σp. Here, in step S611, the disk is set in a state of being parallel to the XY plane of the absolute coordinate system Σworld, and a homogeneous transformation matrix [Tp] · [ Ew] in advance in the RAM 102
To be stored. Next, the homogeneous transformation matrix [Tp] · [E
w], a homogenous transformation matrix [Tp] indicating the position and orientation of the second robot is calculated, and the homogenous transformation matrix [Tp] is inversely transformed to obtain each joint variable θpk of the second robot. (K = 1, 2,..., 6) and operate the second robot in the same manner as in the above embodiment.

(5-5) Fifth Modification In the above embodiment, if the slider 2 in the industrial robot system shown in FIG. 1 is a one-axis slider instead of the two-axis slider shown in FIG. In the first calibration process (step S1) for calibrating the installation error ε3, steps S110 to S132 are executed, and “αe”
And “γe” are calculated. Then, the calibration process can be performed by skipping step 150 and thereafter and not executing it. Also,
In the case of a three-axis slider, only two axes corresponding to the present embodiment are set to “αe”, “βe”, and “γ” according to the present embodiment.
By calculating “e”, the installation relation can be calibrated.

[0232]

As described above, according to the present invention, a tool for performing a predetermined operation and a first tool for moving the tool are provided.
A method or apparatus for calibrating an installation error of an industrial robot system including a tool moving device including a joint means, a work as a work, and a work handling apparatus including a second joint means for moving the work. Controlling the first joint means so that the position of the tool becomes the origin of the work base coordinate system set for the work;
The tip of the tool before and after the control of the joint means of
Measure each distance between a rectangular parallelepiped block placed so that one vertex coincides with the origin of the work base coordinate system, and, based on the measured distances, refer to a predetermined absolute coordinate system. The position error of the work base coordinate system is calculated based on the calculated error, and the position of the work base coordinate system based on the absolute coordinate system is calibrated based on the calculated error. Next, controlling the second joint means so as to move the position of the tool from a predetermined first position to a predetermined second position, and controlling the tool before and after the control of the second joint means The distance between the tip and the second joint means is measured, the displacement of the tool is calculated based on the measured distance, and the work base coordinate system based on the absolute coordinate system is used as a reference. Is calculated, and based on the calculated error, the position and orientation of the work base coordinate system with respect to the absolute coordinate system is calibrated. Therefore, by executing the calibration process immediately after the installation of the industrial robot system, the installation error is eliminated, and thus, when performing the reproduction operation performed after the teaching operation,
Work contents substantially the same as the work contents instructed in the teaching work can be realized, and the accuracy of the relative position and speed of the tool and the work can be improved. Thereby, for example, the above-described problem that a desired relative speed cannot be realized between the workpiece and the tool can be solved.

[Brief description of the drawings]

FIG. 1 is a perspective view illustrating an industrial robot system according to an embodiment of the present invention and an installation error calibration control device thereof.

FIG. 2 is a mechanism diagram showing links and joints in each device of the industrial robot system of FIG. 1;

FIG. 3 is a block diagram of the calibration control device of FIG. 1;

FIG. 4 is a perspective view showing a configuration of each joint in each device (excluding a slider) of the industrial robot system of FIG. 1;
FIG. 3 is a block diagram of a servo control circuit in the calibration control device.

FIG. 5 is a schematic diagram showing installation errors of the industrial robot system according to the embodiment of FIG. 1;

FIG. 6A is a schematic diagram showing each installation error of the industrial robot system according to the first modification, and FIG. 6B is a schematic diagram showing each installation error of the industrial robot system according to the second modification. It is a figure and (c) is a schematic diagram which shows each installation error of the industrial robot system of the 3rd modification.

FIG. 7 is a plan view showing the relation of each coordinate system provided in the industrial robot system of FIG.

FIG. 8 is a front view showing a relation between respective coordinate systems provided in the industrial robot system of FIG. 1;

FIG. 9 is a side view showing the relationship between respective coordinate systems provided in the industrial robot system of FIG. 1;

10 is a plan view showing each installation error in each coordinate system provided in the industrial robot system of FIG.

11 is a front view showing each installation error in each coordinate system provided in the industrial robot system of FIG.

12 is a side view showing each installation error in each coordinate system provided in the industrial robot system of FIG.

FIG. 13 is a perspective view showing an apparatus configuration when the robot is moved by a predetermined distance DL1 along the first axis L11 of the slider when performing the first calibration process in the industrial robot system of FIG. 1; is there.

FIG. 14 is a diagram illustrating an apparatus configuration near a block BK1 when the robot is moved along a first axis L11 of a slider by a predetermined distance DL1 when executing the first calibration process in the industrial robot system of FIG. FIG.

15 is a perspective view showing an apparatus configuration when the robot is moved by a predetermined distance DL2 along the second axis L12 of the slider when performing the first calibration process in the industrial robot system of FIG. 1; is there.

FIG. 16 shows an apparatus configuration near the block BK1 when the robot is moved by a predetermined distance DL2 along the second axis L12 of the slider when performing the first calibration process in the industrial robot system of FIG. FIG.

FIG. 17 is a plan view showing an installation error when executing a second calibration process in the industrial robot system of FIG. 1;

FIG. 18 is a block diagram showing a distance sensor and a block B when a second calibration process is performed in the industrial robot system of FIG. 1;
It is a perspective view which shows the positional relationship with K1.

FIG. 19 is a block diagram showing a distance sensor and a block B when a second calibration process is performed in the industrial robot system of FIG. 1;
It is a top view which shows the positional relationship with K1.

20 is a front view showing an installation error when executing the angle γwz calibration process of the third calibration process in the industrial robot system of FIG. 1;

FIG. 21 is a front view showing an installation error when performing the angle βwz calibration process of the third calibration process in the industrial robot system of FIG. 1;

FIG. 22 is a front view showing an installation error when executing the angle αwz calibration process of the third calibration process in the industrial robot system of FIG. 1;

23A is a plan view showing an installation error when a fourth calibration process is performed in the industrial robot system of FIG. 1, and FIG. 23B is a front view thereof.

FIG. 24 is a side view showing an installation error when performing a fourth calibration process in the industrial robot system of FIG. 1;

25A is a plan view showing an arrangement of distance sensors when a fourth calibration process is performed in the industrial robot system of FIG. 1, and FIG. 25B is a front view thereof.
(C) is a side view thereof.

FIG. 26 is a main routine of an installation error calibration process executed in the calibration control device according to the embodiment of FIG. 1;

FIG. 27 is a main routine of an installation error calibration process executed in the calibration control device of the first modified example.

FIG. 28 is a main routine of an installation error calibration process executed in the calibration control device of the second modified example.

FIG. 29 is a main routine of an installation error calibration process executed in the calibration control device of the third modified example.

FIG. 30 is a flowchart showing a first part of the first calibration process (step S1) in FIGS. 26 and 28.

FIG. 31 is a flowchart showing a second part of the first calibration process (step S1) in FIGS. 26 and 28.

FIG. 32 is a flowchart showing a second calibration process (step S2) in FIGS. 26 and 28.

FIG. 33 is a flowchart showing a third calibration process (step S3) in FIGS. 26 and 27.

FIG. 34 shows the angle γwz calibration process of FIG. 33 (step S).
It is a flowchart which shows 31).

FIG. 35 shows an angle βwz calibration process of FIG. 33 (step S
It is a flowchart which shows 32).

FIG. 36 shows an angle αwz calibration process shown in FIG. 33 (step S
It is a flowchart which shows 33).

FIG. 37 is a fourth view of FIGS. 26, 27, 28 and 29;
9 is a flowchart showing a calibration process (step S4).

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Robot manipulator (robot), 2 ... Slider, 3 ... Positioner, 4 ... Tool, 5 ... Calibration control device, 11 ... Tool moving device, 12 ... Work handling device, 20 ... Teaching box, 21 ... Operation box, 100 ... CPU, 101 ROM, 102 RAM, 103 interface, 104 dual port RAM, 105 latch circuit, 106, 107, 108 servo control circuit, 110 clock generator, 111 frequency divider, RJ1 to RJ6 , RJ11 to RJ13, RJ21 to RJ23 ... joints, L1 to L6, L11 to L13, L21 to L23 ...
Link, MO1 to M10: Motor, SN1 to SN10: Sensor, RE1 to RE10: Reduction gear, J1, J2, J3: Jig, ε1, ε2, ε3, ε4: Installation error, Δαtz, Δβtz, Δγtz, Δθs3, Δαwz ,
Δβwz, Δγwz: twist angle, [Zt]: homogeneous transformation matrix indicating the position and orientation of the tool base coordinate system based on the absolute coordinate system, [Zt ′], [Zt ″]… based on the absolute coordinate system A calibration value of a homogeneous transformation matrix indicating the position and orientation of the tool base coordinate system, [Zw] ... a homogeneous transformation matrix indicating the position and orientation of the work base coordinate system with reference to the absolute coordinate system, [Zw '], [Zw''],[Zw'''], [Z
w '''']... Calibration values of a homogeneous transformation matrix indicating the position and orientation of the work base coordinate system with reference to the absolute coordinate system, BK1, BK2... cuboid blocks, SS1, SS2, SS3. Tool tip.

Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) G05B 19/18-19/46 B25J 3/00-3/04 B25J 9/10-9/22 B25J 13/00-13 / 08 B25J 19/02-19/06

Claims (4)

(57) [Claims] 1. A tool moving device including a tool for performing a predetermined work, a first joint means for moving the tool, a work to be worked, and a second for moving the work.
A method for calibrating an installation error of an industrial robot system comprising a work handling device including a joint means of the first and second means, wherein the position of the tool becomes the origin of a work base coordinate system set on the work. Of the tool before and after the control of the first joint means and the origin of the work base coordinate system.
A first step of measuring each distance between a rectangular parallelepiped block placed so that the vertices coincide with each other; and a reference to a predetermined absolute coordinate system based on each distance measured in the first step. Calculating a position error of the work base coordinate system, and calibrating the position of the work base coordinate system based on the absolute coordinate system based on the calculated error; The second joint means is controlled so as to move from a predetermined first position to a predetermined second position, and the tip of the tool and the second joint before and after the control of the second joint means are controlled. A third step of measuring each distance between the first and second means, and calculating the amount of displacement of the tool based on each of the distances measured in the third step, and calculating the displacement of the tool based on the absolute coordinate system Base seat Calculating a position and orientation error of the system, and calibrating the position and orientation of the work base coordinate system based on the absolute coordinate system based on the calculated error. How to calibrate the installation error of the robot system. 2. The tool moving device further includes third joint means for moving the first joint means along a predetermined axial direction, and the installation error calibrating method further comprises: the first joint means. Is controlled via a jig at the tip of the tool by controlling the third joint means to move the third joint means from a third predetermined position to a fourth predetermined position along the predetermined axial direction. Using the distance measuring means, each surface of the rectangular parallelepiped block placed before and after the control of the third joint means is placed so as to be parallel to each axial direction of the absolute coordinate system. A fifth step of measuring a distance, and a torsion angle of a position and orientation of a tool base coordinate system set in the tool based on the absolute coordinate system based on each distance measured in the fifth step. Calculated, and the torsional angle calculated above A sixth step of calibrating the position and orientation of the tool base coordinate system set in the tool based on the absolute coordinate system based on the above condition; Originally, the first joint means and the third joint means move the first joint means from a predetermined fifth position to a predetermined sixth position along the predetermined axial direction. A seventh step of measuring each distance between the tip of the tool and a predetermined solid position before and after the control of the first joint means and the third joint means; The torsion angle of the tool base coordinate system with respect to the predetermined axial direction of the third joint means is calculated based on each distance measured in step 7, and the third torsion angle is calculated based on the calculated torsion angle. Above place of joint means And calibrating the torsion angle of the tool base coordinate system with respect to a fixed axis direction. 3. A tool moving device including a tool for performing a predetermined work, a first joint means for moving the tool, a work to be worked, and a second for moving the work.
An industrial robot system comprising: a work handling device including: a joint means for controlling the first joint means so that the position of the tool becomes an origin of a work base coordinate system set for the work. 1 control means, provided at the tip of the first joint means, so that one vertex of the tip of the tool coincides with the origin of the work base coordinate system before and after the operation of the first control means. First measuring means for measuring each distance between the placed rectangular parallelepiped block, and the work based on a predetermined absolute coordinate system based on each distance measured by the first measuring means. First calibration means for calculating an error of the position of the base coordinate system, and for calibrating the position of the work base coordinate system with reference to the absolute coordinate system based on the calculated error; A second control means for controlling the second joint means so as to move the first joint means from the first position to a predetermined second position; and a second control means provided at a tip of the first joint means. The tip of the tool and the second
A second measuring means for measuring each distance between the joint means, and calculating a displacement amount of the tool based on each distance measured by the second measuring means, and calculating a predetermined absolute coordinate system. Second calibration means for calculating an error of the position and orientation of the work base coordinate system as a reference, and calibrating the position and orientation of the work base coordinate system with the absolute coordinate system as a reference based on the calculated error. An installation error calibration control device for an industrial robot system, comprising: 4. The tool moving device further includes third joint means for moving the first joint means along a predetermined axial direction, and the installation error calibration control device further comprises: the first joint means. Third control means for controlling the third joint means to move the means from a third predetermined position to a fourth predetermined position along the predetermined axial direction; And distances from the respective surfaces of the rectangular parallelepiped block placed before and after the control of the third joint means so that the respective surfaces are parallel to the respective axial directions of the absolute coordinate system. And a torsion angle of a position and orientation of a tool base coordinate system set in the tool based on the absolute coordinate system based on each distance measured by the third measurement means. And the torsion angle calculated above Third calibration means for calibrating the position and orientation of the tool base coordinate system set in the tool with reference to the absolute coordinate system based on the condition, and the condition for keeping the position and orientation of the tool constant with respect to the workpiece The first joint means and the third joint so as to move the first joint means from a predetermined fifth position to a predetermined sixth position along the predetermined axial direction. A fourth control means for controlling the means, a fourth measuring means for measuring each distance between the tip of the tool and a predetermined solid position, and a fourth measuring means for measuring each distance measured by the fourth measuring means. Calculating a torsion angle of the tool base coordinate system with respect to the predetermined axial direction of the third joint means, based on the calculated torsion angle, based on the calculated torsion angle. Tool-based coordinate system Placement error calibration control device of the fourth industrial robot system according to claim 3, characterized in that a calibration means for calibrating the angle Gillet.