JP4530714B2 - Drive control system for articulated robot - Google Patents

Description

  The present invention relates to an articulated work robot, and particularly, in an environment where the work table moves relative to the work robot, work such as painting and drawing (hereinafter referred to as machining work) is performed on the work mounted on the work table. The present invention relates to a drive control system for a work robot when performing the above through a machining head of the work robot.

FIG. 9 shows a workpiece W for performing an operation in which the machining head of the work robot follows the ridgeline. When performing an operation to follow the edge of the workpiece W (hereinafter referred to as a workpiece) mounted at a certain location with respect to the coordinate system of the work robot (hereinafter referred to as the robot base coordinate system or the world coordinate system), as follows: A simple robot program.
PROGRAM RYOSEN
MOVES P1
MOVES P2
MOVES P3
MOVES P4
END

The above program is a copying operation program for the workpiece W that is stationary / fixed with respect to the base coordinate system of the robot. Therefore, if the workpiece W always slides on a conveyor or the like, the position / posture of the workpiece W is accordingly determined. However, when the robot moves and changes as seen from the base coordinate system of the robot, the tip of the robot, that is, the tip of the machining head is separated from the workpiece ridge line, and the copying operation cannot be performed.
FIG. 10 illustrates a system capable of reading a pulse counter for each processing cycle from the encoder ENC attached to the conveyor CNV. In FIG. 10, the encoder ENC attached to the conveyor CNV is E1 (pulse ) When counting, conveyor vector (unit vector)

とし、エンコーダ1パルスに相当するコンベア移動量をRefとすると、P1点はロボットのベース座標系から見て

If the amount of conveyor movement corresponding to one encoder pulse is Ref, point P1 is viewed from the base coordinate system of the robot.

だけ移動したことになる。
するとプログラムによる動作指令位置に上記ベクトルΔＤを加算した位置をサーボ部へ指令することで、見かけ上コンベアCNVに追従しながら、同コンベア上のワークＷに対する倣い動作が可能となる。前記プログラム中の 「MOVES P1」について動作した時の処理の流れをブロックで図１１に示す。

Just moved.
Then, a position obtained by adding the vector ΔD to the operation command position by the program is commanded to the servo unit, and the copying operation for the workpiece W on the conveyor can be performed while apparently following the conveyor CNV. FIG. 11 is a block diagram showing the processing flow when operating on “MOVES P1” in the program.

  However, the methods shown in FIGS. 10 and 11 are only applicable to the conveyor vector Vcnv, that is, the linear movement type conveyor. For example, a work is mounted on a table that rotates on a horizontal plane and the rotating table is rotating. The rotary axis of the rotary table is the base of the robot when performing various machining operations on the workpiece, when machining other workpieces on the same table with another robot, and for the flexibility of the machining work line. There has been a problem that it is not possible to cope with the processing work on the workpiece when it is necessary to incline with respect to the coordinates, or when the rotary table itself is mounted and arranged on a linear moving conveyor.

  The inventor of the present application has intensively studied to solve the above-described problems. As a result, when the workpiece is on a coordinate system rotated by a predetermined angle around three orthogonal X, Y, and Z coordinate axes called Euler angles, It was found that the problem can be solved by using a rotation matrix to calculate the position on the workpiece as seen from the base coordinate system.

  Accordingly, an object of the present invention is to provide a base for a robot, regardless of whether the work table on which the work is mounted is driven in either a linear movement or a rotational movement, and the rotation axis of the work table is inclined. An object of the present invention is to provide a drive control system for an articulated robot that can control the position and orientation of the machining head of the robot so that the workpiece and the machining head of the robot are relatively stationary as viewed from the coordinate system.

In order to achieve the above object, a drive control system for an articulated robot according to the present invention comprises:
A workpiece, a work table on which the workpiece is mounted at a predetermined position, and an articulated work robot having a machining head arranged at a position different from the work table and performing a machining operation on the workpiece on the work table; A robot control device that controls a posture of the machining head so as to perform a machining operation on the workpiece by the machining head, and a detection unit that detects a relative displacement of the workpiece with respect to the work robot based on a motion of the work table. System,
The robot controller is
First storage means for storing a work command program comprising a series of work command procedures for performing the machining work when the workpiece is stationary relative to the work robot;
First calculation means for generating a first command value for each drive mechanism of the work robot based on a work command program read from the first storage means;
A second calculation for correcting a first command value given from the first calculation means based on a detection signal from the detection means when the work and the work robot are relatively moving based on the movement of the work table. Means, and
When the work table rotates, the detection means rotates in a first direction and a second direction of the rotation angle displacement amount α around the rotation axis of the work table relative to the work robot and the table surface of the work table rotation axis. Comprising first and second detection means for detecting the inclination angle components β and γ, respectively,
The second calculation means performs a rotation calculation function using a rotation matrix defined by a rotation angle displacement amount α which is a detection signal from the first detection means and detection signals β and γ from the second detection means. In addition, the rotational angle displacement amount α is sequentially given during the rotational movement of the work table by a detection signal from the first detection means, and the tilt angle component is a parameter value measured in advance with respect to the work robot base coordinate system of the work table. The control device includes a second storage unit that stores the rotation matrix and the parameter values α, β, and γ.
In that case, the work table can be further moved linearly, and the second calculation means further has a straight line calculation function for generating a correction amount accompanying the linear movement of the work table, A designation means for designating each calculation function of the second calculation means according to each motion state of the work table can be provided.
Furthermore, in that case, the designation means can be configured to designate both the rotation calculation function and the straight line calculation function of the second calculation means.

The drive control system for an articulated robot according to the present invention is:
A rotation calculation function using a rotation matrix defined by a rotation angle displacement amount α around the rotation axis of the work table with respect to the work robot and inclination angle components β and γ of the rotation axis to the work table surface is performed. Therefore, even when the work mounted on the work table rotates relative to the base coordinate system of the robot and when the rotation axis of the table is inclined, the work and the robot processing head are relatively moved. Can be stationary.

Examples according to embodiments of the present invention will be described below in detail with reference to FIGS.
FIG. 1 is a partial plan view of an arc-shaped conveyor (hereinafter referred to as a conveyor or a work table) TB having a rotatable horizontal plane, and shows a state of conveyor synchronization processing for controlling the robot to follow the movement of the conveyor. In FIG. 1, a work W shown in FIG. 10 is attached to a predetermined position on the work table TB. In addition, an articulated work robot RBT is disposed on the front side of the work table TB. Reference symbol ENC is an encoder that detects a change in the rotation angle around the Z axis of the work table TB. Reference numeral CTR is a control device for the robot RBT, and always stores the current rotation angle position data as viewed from the reference position of the work table TB by a detection signal from the encoder ENC. Reference numeral TRG is a synchronization start trigger sensor arranged at a predetermined position on the outer periphery of the table in order to synchronize the relative position between the workpiece W on the work table TB and the robot RBT. The position can be detected and this detection signal is given to the control device CTR. In FIG. 1, when the work W on the work table TB passes through the line L0 and reaches the line L1, the trigger signal is generated.

  On the other hand, the machining head WHD of the robot RBT is positioned in advance from the standby position to the conveyor synchronization start position A corresponding to the line L2, and when the workpiece W reaches the line L2, the machining operation to the workpiece W by the machining head WHD is performed. For example, a ridge line copying operation is performed. This ridge line copying operation is continuously performed up to the conveyor synchronization end position B corresponding to the line L3.

Specifically describing this process, as described above, the program for the edge copying operation is as follows.
PROGRAM RYOSEN
MOVES P1
MOVES P2
MOVES P3
MOVES P4
END
It is.

For the “MOVES P1” in the program in which the robot RBT follows the edge of the workpiece W using the above program, each control axis goes through the steps of “program analysis”, “trajectory generation to P1”, and “distributed target position calculation”. Commanded to servo section.
FIG. 2 shows the distribution position command values P1 [0], P1 [1], and P1 [2] at this time.
A coordinate conversion process as shown by P1 ′ [0] in FIG. 3 is performed on all of the distribution position command values P1 [0], P1 [1], P1 [2]. The ridgelines P0 to P3 of the workpiece W can be copied while following the rotation of the workpiece W.

  Here, in the coordinate conversion, as shown in FIG. 4, the pulse counter value of the encoder ENC is converted into the angle correction amount dθ (deg) for each control period, and the rotation axis of the work table TB is based on this. A rotation matrix for correction around (Z axis) is obtained. Based on this rotation matrix, the world coordinate system distribution position command value P1 [0] of the robot RBT is coordinate-transformed to calculate a corrected position command value P1 '[0]. The calculation method of this corrected position command value is apparent with reference to FIG. 5 and the following equation 3. As seen from the rotation axis coordinate system, the world coordinate system position command value vector “O-P1 [0]” of the robot RBT. Is transformed to calculate the conveyor synchronization position vector “O-P1 ′ [0]” by (Equation 4). Subsequently, the position vector “P1 ′ [0]” viewed from the world coordinate system is converted by (Equation 5).

なお、上記（式３）において、Ｒｚ（θ）はＺ軸の周りに角度θ回転させたときに対応する回転行列である。

In the above (Expression 3), Rz (θ) is a rotation matrix corresponding to the rotation of the angle θ around the Z axis.

  FIG. 6 is a block diagram showing the processing flow when operating on “MOVES P1” in the program. In contrast to the processing flow shown in FIG. 11, in FIG. 11, the linear movement amount converted from the encoder pulse is added to the distribution target position calculated by the program analysis, whereas in FIG. A command value is given to the servo unit of each control axis by performing coordinate conversion based on the angle converted from the encoder pulse with respect to the distribution target position calculated by the above. In addition, the difference is that the rotation matrix shown in (Equation 3) is adopted in the coordinate transformation.

  In the above example, the table surface of the work table TB is assumed to be horizontal, but the shape of the work W itself and its processing work are diverse, and flexibility in work line configuration and arrangement is required. It may be required to incline the worktable surface. In addition, since the machining work on the workpiece W is required to have very high accuracy, the inclination of the table surface is not negligible compared to the horizontal plane, and further, the saddle in the machine tool and the rotary table mounted on the saddle are mounted. As described above, when the rotating work table TB is mounted on a linearly moving conveyor, it is necessary to deal with the condition that the rotation axis of the work table TB, that is, the Z axis is not completely vertical. .

A system for performing drive control of a robot that can cope with the case where the rotation axis of the work table is inclined will be described with reference to FIGS.
FIG. 7 shows that in the Cartesian coordinate system (X, Y, Z), the work table surface is inclined when the work table surface on the XY plane, that is, the horizontal plane is rotated by β around the Y axis. The state which became the inclined surface SLF is shown. In this case, the Z-axis and the X-axis are also rotated by an angle β to become the Z′-axis and the X′-axis, respectively. At this time, the Z ′ axis is inclined in the X direction. (First direction) Although not shown, in the state shown in FIG. 7, when the angle is further rotated around the X axis by an angle γ, the inclination direction of the Z axis has an inclination angle γ (second direction) also in the Y axis direction. .

  Note that the orthogonal coordinate system (X, Y, Z) of the work table is parallel to the base coordinate system of the robot and the respective axes. In FIG. 7, the rotation matrix Rzyx (α, β, γ) corresponding to the rotation of α, β, and γ around the respective axes of Z, Y, and X is expressed by the following (formula 6). Here, C represents cosine cos, and S represents sine sin.

図８は、図１乃至図６で説明した水平のワークテーブル面を持つ場合も含めた作業ロボットの制御を説明するための機能的制御ブロック線図を示す。同図８において、参照符号１０はデータメモリであって、ロボットに対する作業の詳細を記述した作業指令プログラムをストアしている第１記憶手段１０Ａ、及び前記回転行列Rzyx (α、β、γ)の内容ならびにワークテーブルの設置及び動作状態に関する前記エンコーダやトリガセンサ等からの信号をパラメータデータとしてストアする第２記憶手段１０Ｂから構成される。

FIG. 8 is a functional control block diagram for explaining the control of the work robot including the case of having the horizontal work table surface described in FIGS. In FIG. 8, reference numeral 10 denotes a data memory, which is a first storage means 10A storing a work command program describing details of work for the robot, and the rotation matrix Rzyx (α, β, γ). It comprises second storage means 10B for storing signals from the encoder, trigger sensor, etc. regarding the contents and worktable installation and operating state as parameter data.

Reference numeral 12 denotes first calculation means, which analyzes the work command program and calculates position data to which the machining head of the robot should move at the next sample processing time as a first command value.
Reference numeral 16 denotes second calculation means, which includes a rotation calculation function (part) 16A and a straight line calculation function (part) 16B. Reference numeral 14 denotes designation means for designating each of the rotation calculation function (part) 16A and the straight line calculation function (part) 16B, and is previously determined by the configuration and arrangement of the work table. That is, the rotation calculation function (part) 16A is designated when the work is rotated by the work table, and the straight line calculation function (part) 16B is designated when the work is only linearly moved on the conveyor as shown in FIG. Is done. As described above, when the work table rotates on the conveyor, both the rotation calculation function (part) 16A and the straight line calculation function (part) 16B are designated.

  In the second calculation means, the first command value given from the first calculation means 12 is corrected by the rotation calculation function (part) 16A or the straight line calculation function (part) 16B, and at the next sample processing time, The final position data to be moved by the machining head is given to the servo system 28. Here, when both calculation functions are designated, this is achieved by simply adding the correction amount given by the straight line calculation function (part) 16B to the result of the rotation calculation function (part) 16A.

In the rotation calculation function (unit) 16A, the same calculation as the above-described (Expression 4) and (Expression 5) is performed. However, Rzyx (α, β, γ) is used instead of the rotation matrix Rz (θ).
In the calculation, the rotation matrix and the parameter values α, β, γ used therein are read from the stored memory area from the second storage unit 10B. When the work table corresponds to FIG. 1, that is, when there is no Z-axis inclination and no rotation around the Y-axis and X-axis, β and γ are both zero, and α corresponds to θ. Of course, the contents of the rotation matrix shown in (Equation 6) coincide with the contents of (Equation 3).

In FIG. 8, reference numeral 22 denotes first detection means for detecting the amount of change in the rotation angle position of the work table, that is, the rotation angle α of the Z axis. In addition, when the work table is linearly moved like a conveyor, the amount of movement in the conveyor moving direction is obtained.
Reference numeral 24 is second detection means for detecting the tilt angles of the rotating work table, that is, the tilt angles β and γ in the Y direction and the X direction of the Z axis. Normally, these inclination angles β and γ can be measured in advance by a rotary encoder or the like as the rotation amounts around the Y axis and the X axis, respectively. It is also possible to directly measure the table surface with a level. In the former case, β and γ can always be measured as well as the angle α.
Reference numeral 26 is a trigger sensor for synchronizing the work on the work table with the robot.
Reference numeral 20 denotes a central processing unit (hereinafter referred to as CPU), which performs control in the robot controller according to a control program stored in the program memory 18. That is, the execution of the calculation in the first calculation means 12 and the second calculation means 16, the fetching of the detection data from each detection means 22, 24, 26 to the data memory 10, and the execution of the calculation from the data memory 10 The operation command program, the rotation matrix, the parameter value, and the like are fetched.

  The preferred embodiments of the present invention have been described above, but the technical idea of the present invention is not limited to the above-described embodiments, and those skilled in the art can implement various modifications based on the present invention. Of course, such modifications are included in the scope of the technical idea of the present invention.

BRIEF DESCRIPTION OF THE DRAWINGS It is a figure which shows one Example of this invention and shows the partial plane of the circular arc conveyor which has a rotatable horizontal surface. It is a figure which shows the mode of distribution position command value P1 [0] in FIG. 1, and P1 [1]. It is a figure which illustrates the coordinate transformation process of this invention. It is explanatory drawing for calculating | requiring the rotation matrix for a correction | amendment around the rotating shaft of a work table based on this, converting the pulse counter value of an encoder into the angle correction amount d (theta) for every control period. It is a figure which illustrates the calculation method of the correction position command value of this invention. It is a block diagram which shows the flow of a process when it operate | moves about program "MOVES P1". In the Cartesian coordinate system (X, Y, Z), when the work table surface that is in the XY plane, that is, the horizontal plane is rotated by β around the Y axis, the work table surface is inclined to form the inclined surface SLF. It is a figure which shows the state which became. It is a functional control block diagram for demonstrating control of the work robot including the case where it has the horizontal worktable surface demonstrated in FIG. 1 thru | or FIG. It is a figure which shows the workpiece for performing the operation | movement in which the process head of a working robot follows a ridgeline. It is a figure which illustrates the system which can read a pulse counter for every process period from the encoder attached to the conveyor. It is a block showing the flow of processing when operating on “MOVES P1” in a program that can follow the workpiece.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Data memory 10A 1st memory | storage means 10B 2nd memory | storage means 12 1st calculating means 14 Specification means 16 2nd calculating means 16A Rotation calculating function (part)
16B Straight line calculation function (part)
18 Program memory 20 CPU
22 First detection means 24 Second detection means 26 Trigger synchronization sensor 28 Servo system ENC Encoder SLF Inclined surface RBT Work robot TB Worktable WHD Work machining head

Claims (3)

A workpiece, a work table on which the workpiece is mounted at a predetermined position, and an articulated work robot having a machining head arranged at a position different from the work table and performing a machining operation on the workpiece on the work table; A robot control device that controls a posture of the machining head so as to perform a machining operation on the workpiece by the machining head, and a detection unit that detects a relative displacement of the workpiece with respect to the work robot based on a motion of the work table. System,
The robot controller is
First storage means for storing a work command program comprising a series of work command procedures for performing the machining work when the workpiece is stationary relative to the work robot;
First calculation means for generating a first command value for each drive mechanism of the work robot based on a work command program read from the first storage means;
A second calculation for correcting a first command value given from the first calculation means based on a detection signal from the detection means when the work and the work robot are relatively moving based on the movement of the work table. Means,
With
When the work table rotates, the detection means rotates in a first direction and a second direction of the rotation angle displacement amount α around the rotation axis of the work table relative to the work robot and the table surface of the work table rotation axis. Comprising first and second detection means for detecting the inclination angle components β and γ, respectively,
The second calculation means performs a rotation calculation function using a rotation matrix defined by a rotation angle displacement amount α which is a detection signal from the first detection means and detection signals β and γ from the second detection means. In addition, the rotational angle displacement amount α is sequentially given during the rotational movement of the work table by a detection signal from the first detection means, and the tilt angle component is a parameter value measured in advance with respect to the work robot base coordinate system of the work table. The control apparatus includes a second storage unit that stores the rotation matrix and the parameter values α, β, and γ. The work table is further capable of linear movement, and the second calculation means further has a linear calculation function for generating a correction amount associated with the linear movement of the work table, and the control device includes the work table. 2. The drive control system for an articulated robot according to claim 1, further comprising designation means for designating each calculation function of the second calculation means in accordance with each motion state. The drive control system for an articulated work robot according to claim 2, wherein the specifying means specifies both the rotation calculation function and the straight line calculation function of the second calculation means.

Images