JPH08323664A - Robot controller - Google Patents

Abstract

PURPOSE: To improve hand tip drive speed and positioning accuracy of a robot by calculating a target position for a controller of the robot based on a calculated controlled variable and a target value of a feedback operation means allocated to a model which corresponds to a judged region. CONSTITUTION: Even if a positional space of a hand tip H of a robot RBT + RBC is divided into a plurality of regions, the movement of the hand tip H in each region is represented by a simple linear model, and a feedback operation means adapted to it is a means which facilitates the design and adjustment comparatively, the hand tip H can be driven and positioned comparatively at a high speed with high accuracy in each region. Consequently, the whole operation region of the hand tip H is a collection of each region, and comparatively high speed and high accuracy drive and positioning are realized over the whole region. Since it is possible to cope with non-linear characteristics of a multi-axis arm simply and a comparatively inexpensive general-purpose robot (RBT + RBC) can be used as it is, the robot can be installed easily.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a hand support mechanism having three or more axes for positioning a hand (working end; hand) in a three-dimensional space, and a position tracking means for recognizing a drive source and a hand position. The present invention relates to a control device for a robot having a controller that drives a hand support mechanism via the drive source to drive the hand to a given target position, and particularly to a control device for causing the hand to perform a required motion. Although not intended to be limited, the present invention relates to a control device for providing a controller of a robot with target position information for scanning and driving the hand of the robot along the object surface with a set gap from the object surface.

[0002]

2. Description of the Related Art The above-mentioned robot drives a hand to a given target position. For example, in order to detect a flaw on the surface of a steel plate, a surface flaw detector (eg, photosensor, eddy current sensor, ultrasonic sensor) is used. When supporting and scanning the surface of the steel plate thoroughly (for example, x, y scanning), it is preferable that the scanning speed is as high as possible within the range where flaw detection is favorably performed, because workability is high. On the other hand, if the gap between the surface flaw detector and the steel plate surface fluctuates, the flaw detection accuracy decreases, but since the steel plate has large and small undulations (bends), the sensor detects the distance between the steel plate surface and the surface flaw detector or hand. However, it is preferable to use feedback control in which the hand is driven (for example, the z direction) so that the detection distance is constant, that is, scanning control is used in combination. The design and manufacture of robots that meet these needs is labor intensive, time consuming and expensive. On the other hand, various so-called general-purpose robots having three or more axes for performing three-dimensional hand positioning are already on the market, and recently relatively inexpensive robots have been around. By using such a commercially available robot, the labor, time and cost for installing the robot are saved.

[0003]

However, when a commercially available robot (general-purpose robot) is used, when scanning a steel plate surface having a relatively large area, it is difficult to satisfy both high scanning speed and high positional accuracy at the same time. Funny Robot hand
At a position close to the support base point of the arm, the movement moment of the robot arm is small, so high-speed movement with high positioning accuracy is possible, but movement occurs as the hand moves away from the support base point (the robot arm extends). Since the momentum becomes large, the positioning accuracy cannot be obtained when moving at high speed. Therefore, the scanning speed, that is, the low speed, at which the required positioning accuracy is obtained at the position where the hand is separated from the support base point, is determined. In particular, in the case of the scanning control (z direction) in which the hand is driven so that the distance between the surface of the steel plate and the surface flaw detection device or the hand is constant, if the response speed is slow, the surface flaw detection accuracy decreases. Scanning speed of the hand (x, y direction) corresponding to the speed
Has to be set low.

In the field of robot specialization, a dynamic model of the robot is used to calculate an acceleration command for each joint (hand support mechanism) from the position, velocity, and acceleration commands of the robot's hand trajectory, and the current corresponding to this is calculated. A high level of research is known to energize a drive motor (a hand support mechanism drive motor). However, if the hand drive range is wide and precise scanning drive is performed, the dynamic model becomes complicated,
In particular, the dynamic model that also realizes the copy control becomes extremely complicated. A huge amount of calculation is required to calculate the acceleration command for each joint.

If the feedback controller is constructed by regarding the robot as a linear model which is most easily used in the drive design of the conventional machine, the design and adjustment thereof are easy. However, since the motion characteristics of the robot are non-linear characteristics, the control accuracy is deteriorated in the motion region where the dissociation from the linear region matching the planned linear model is large. This tendency becomes remarkable especially when a simple linear model that is easy to design and adjust is used, and when the range of movement of the hand is wide.

A first object of the present invention is to increase the hand driving speed and the positioning accuracy of the robot, and the second object is to facilitate the design and adjustment of a control device for causing the hand to perform a desired trajectory motion. The third purpose is to provide a control device for scanning drive while scanning the hand, and the fourth purpose is to increase the speed and accuracy of the scanning drive and the scanning drive. A fifth object is to facilitate the design and adjustment of the control device.

[0007]

[Means for solving the problem] The hand (H) is three-dimensional (x, y, z)
Position tracking means (RBC) and hand (H) for recognizing the position (xs, ys, zs) of the hand (H1) and the hand (H1) and the hand support mechanism with three or more axes for driving in space. ) To a given target position, a robot (RBT +) equipped with a controller (RBC) that drives a hand support mechanism via the drive source.
When the target position (xo, yo, zo) is given to the controller (RBC), the hand (H) is driven to the target position (xo, yo, zo), which is represented by a non-linear model. To be done. Therefore, in the present invention, this robot (RBT + RBC) is regarded as a combination of a plurality of models (preferably a simple linear model), and each model is
Assign the feedback computing means that best fits the model. That is, a plurality of feedback calculation means (5 PI
I-x1 to 4, ACX / PII-y1 to 4, ACY).

Then, the position (xs, ys, zs) of the hand (H) is in a region (to) to which each of a plurality of models fits, and the determination means (RDT of 5) determines. The target position (xo, yo, zo) with respect to the controller (RBC) of the robot is calculated based on the calculated control amount and the target value of the feedback calculation means assigned to (the model corresponding to) the region.

That is, the control device of the present invention is a robot.
Region determination means (RDT of 5) for determining which of a plurality of preset regions (~) the position (xs, ys, zs) of the hand (H) of (RBT + RBC) is; ), Which is suitable for feedback control in each of the assigned areas, for the original target position (Xo, Yo)
A plurality of feedback calculation means (5) for calculating the control amount for zeroing the deviation (Xo-xs, Yo-ys) of the position (xs, ys) of (H)
PII-x1 to 4, ACX / PII-y1 to 4, ACY); and the feedback calculation means (PII) assigned to the determined area.
-x1 to 4, ACX / PII-y1 to 4, ACY) calculated control amount and hand (H)
From the position (xs, ys) of the hand, the fingertip for the original target position (Xo, Yo)
The target position (xo, yo) of the hand (H) for zeroing the deviation (Xo-xs, Yo-ys) of the position (xs, ys) of (H) is calculated and the control is performed.
A target position calculation means (5) to be applied to the laser (RBC).

A control device of the present invention for performing copying control is connected to a hand (H) of a robot (RBT + RBC) and an opposing object (1).
A distance measuring device (2-4) for detecting a distance (Gs) from the area; a region determining means (5) for determining which of a plurality of regions the position (xs, ys, zs) of the hand (H) is set in advance. ); Each is assigned to each of the plurality of areas, and is suitable for feedback control in the assigned area, in order to make the deviation (Go-Gs) of the detection distance (Gs) from the target distance (Go) zero. Feedback control means (PII-G1 to 4, AC
G); and detected from the control amount calculated by the feedback calculation means (PII-G1 to 4, ACG) assigned to the determined area and the position (xs, ys, zs) of the hand (H) Deviation of distance (Gs)
Target position of hand (H) to make (Go-Gs) zero (xo, yo, zo)
And a target position calculating means (5) for calculating and giving to the controller (RBC).

One embodiment of the present invention is a robot (RBT + RBC).
A distance measuring device (2-4) that is connected to the hand (H) of the hand and detects the distance (Gs) from the counter object (1); a plurality of positions (xs, ys, zs) of the hand (H) set in advance. Region determination means (RDT of 5) for determining which one of the regions (to) is;
Original target position (Xo, Yo) suitable for feedback control in each assigned area.
The control amount for zeroing the deviation (Xo-xs, Yo-ys) of the position (xs, ys) of the hand (H) with respect to, and the deviation (Go- of the detection distance (Gs) with respect to the target distance (Go). Gs) is a controlled variable to make it zero,
A plurality of feedback calculation means (5 PII-x1 ~
4, ACX / PII-y1 to 4, ACY / PII-G1 to 4, ACG); and feedback calculation means assigned to the determined area
(PII-x1 ~ 4, ACX / PII-y1 ~ 4, ACY / PII-G1 ~ 4, ACG) calculated from the control amount and the position of the hand (H) (xs, ys, zs), the original target position ( Deviation of the position (xs, ys) of the hand (H) with respect to (Xo, Yo) (Xo-xs,
Yo-ys), and the target position (xo, yo, zo) of the hand (H) to make the deviation (Go-Gs) of the detection distance (Gs) zero, and the controller (RBC) Target position calculation means (5);

In this embodiment, target position calculation means
(5) changes the original target position (Xo, Yo) at the successively set pitch in order to move the hand (H) along a predetermined locus. In addition, feedback calculation means (PII-x1 to 4, ACX / PII-y1 to 4, ACY / PII
-G1 to 4, ACG) is a plurality of first arithmetic means (PII-x1 to 4, PII-y1 to 4, PII-G1 to 4) having a number smaller than the number of the plurality of regions,
When the determined area is an area in which the first calculation means does not correspond to one-to-one, the calculated control amount of the first calculation means corresponding to each of a plurality of areas adjacent thereto is determined for the adjacent area. Second computing means for computing the sum of the extracted amount corresponding to the distance of the hand (H) and the addition as the control amount
(ACX / ACY / ACG) is included. In addition, the first computing means (PII-x1
~ 4, PII-y1 ~ 4, PII-G1 ~ 4) is the proportional term operation, the first
Integral term operation, second integral term operation and addition of these operation results are included.

In order to facilitate understanding, the symbols of corresponding elements shown in the drawings are added for reference.

[0014]

According to the controller of the present invention, the position space of the hand (H) of the robot (RBT + RBC) is divided into a plurality of regions (-).
The movement of the hand (H) in each area is divided into
Even if the feedback calculation means represented by a simple linear model and adapted to this is relatively simple and easy to design and adjust, the hand (H) is driven at a relatively high speed and high precision in each area. Can be positioned. And then the minions
The entire operation region of (H) is a set of each region, and relatively high speed and high precision driving and positioning are realized over the entire region. You can easily deal with the nonlinear characteristics of multi-axis arm,
Since a commercially available relatively inexpensive general-purpose robot (RBT + RBC) can be used as it is, the robot can be easily installed.

According to the control device of the present invention for performing the copying control, the above-described action and effect are similarly obtained in the copying control in which the hand (H) is set to the predetermined distance (Go) with respect to the facing object (1). To be In addition, for example, a commercially available relatively inexpensive general-purpose robot (RBT + RBC) has a hand (H) at the detection end of a distance detector (2-4).
When (2) is installed, the target position calculation means (5)
Since the target position (xo, yo, zo) for setting (Gs) as the target distance (Go) is calculated and given to the robot controller (RBC),
The general-purpose robot (RBT + RBC) can be used as it is to implement the copying drive of the hand (H) without changing or adding the internal mechanism or control algorithm of the robot (RBT + RBC).

According to the embodiment of the present invention, the hand (H) is driven to the original target position (Xo, Yo) and the hand (H) is set to a predetermined distance (Go) with respect to the facing object (1). The copying control is realized at the same time, and the above-described actions and effects are similarly obtained in both.
When the target position calculation means (5) changes the original target position (Xo, Yo) at the successively set pitch in order to move the hand (H) along a predetermined locus, the hand (H) becomes the opposite object (1). On the other hand, the distance (Gs) is maintained at the target distance (Go), and the object moves. By changing the original target position (Xo, Yo) to draw a scanning pattern,
(H) scans and moves along the surface of the counter object (1). Target distance
By keeping (Go) constant, the scanning of the hand (H) with respect to the opposing object (1) is realized.

According to the embodiment of the present invention, the feedback calculating means is composed of a plurality of first calculating means and second calculating means (ACX / ACY).
/ ACG), the setting and adjustment of the feedback calculation corresponding to the area one-to-one need only be performed by the first calculation means, and the design and adjustment of the control device are easy. The first computing means (PII-x1 to 4, PII-y1 to 4, PII-G1
The calculation of (4) to (4) includes a proportional term operation, a first integral term operation, a second integral term operation, and addition of these operation results, that is, PII control. Easy to design and adjust with control technology.

Other objects and features of the present invention will become apparent from the following description of embodiments with reference to the drawings.

[0019]

FIG. 1 shows an embodiment of the present invention. In this embodiment, the robot (RBT + RBC) has 6 joints J1 to J1.
6-axis robot mechanism including 6 and hand H, RBT, mode
Driver (circuit for energizing the electric motor that is the motion power source of the joints J1 to J6) MD1 to MD6, controller R
It is a commercially available relatively inexpensive general-purpose robot composed of BC and operation board OPB. It should be noted that each joint (each axis) has a home position switch for detecting whether or not it is at the origin of the joint position (j1s to js6), a sensor for other mechanism protection and an external body protection, and Equipped with switches.

The horizontal rotary joint J1 is supported by the robot base, and the vertical axis Lz1 supported by the horizontal rotary joint J1 supports the horizontal linear joint J2. The horizontal arm Lxy, which is supported by the horizontal linear motion joint J2 so as to be able to advance and retreat in the horizontal direction, is a vertical linear motion joint J.
The rotary arm Jz of the wrist is supported by the vertical arm Lz2 that supports the vertical arm Lz2 that supports the vertical arm Lz2.
4 are supported. The rotary joint J4 supports a vertical rotary joint J5 that rotates about an axis parallel to the paper surface in FIG. 1, and this joint J5 has a front-back rotary joint J6 that rotates about an axis perpendicular to the paper surface in FIG. The joint J6 supports the hand (H). Each of the joints J1 to J6 is provided with an electric motor and a rotary encoder which are not shown, and the electric motor and the rotary encoder of each joint are connected to the mechanism of each joint, and It is electrically connected to each of the motor drivers MD1 to MD6. The horizontal plane on which the reference point of the robot base exists is the reference plane SS (horizontal plane), and the reference point is the robot mechanism RB.
The origin of T's x, y, z coordinates and polar coordinates.

The robot controller RBC has an external controller connection terminal, and an operation command can be input from both the operation board OPB and the external controller connection terminal. The position command of the hand H in the motion command is x,
y, z three-dimensional coordinate input mode or r, θ polar coordinate input mode
Specify one of the passwords and enter. Controller RBC
When the three-dimensional coordinate input mode is specified,
The input target position (x, y, z) is converted into the target positions (j1 to j6) of the six joints J1 to J6 and the motor drivers MD1 to MD1 are used.
Give to MD6. Each motor driver determines the required rotation direction of the motor from the target position and the current position and activates the motor. Two sets of finger speed pulses generated by the rotary encoder of each joint are given to the motor driver and the robot controller RBC. The motor driver controls the energization of the electric motor according to the position deviation (target position-current position), the energization current value of the electric motor and the rotation speed (for example, PW).
M control). Robot controller RBC and mode
The tactile driver determines the direction of movement of the joint (forward or reverse) with two pairs of finger speed pulses, and counts up the finger speed pulse in the case of forward movement and counts down in the reverse direction to determine the current position of the joint. Always grasp. In the motor driver, the current position (j1s to j6s) is the target position (j1 to j6s).
When the condition 6) is met, the power supply to the motor is stopped. When the r, θ polar coordinate input mode is designated, the robot controller RBC sets the given target position (ro, θo) to the six joints J1 to J6.
j6). The operation after conversion is the same as described above.

Therefore, the robot controller RBC
By giving a three-dimensional coordinate value (xo, yo, zo) or polar coordinate value (ro, θo) as the target position, the hand H is driven to the target position. Robot controller RB
By sequentially changing the target position given to C, the hand H moves. In this embodiment, the target position of the hand H is set to 3
Since it is specified by the dimensional coordinate value (xo, yo, zo), only the mode using the three-dimensional coordinate will be described below.

Hand H of the above robot (RBT + RBC)
The detecting end 2 of the distance measuring device is fixed to the distance measuring circuit 3, and the distance detecting circuit 3 generates an analog signal representing the distance Gs between the detecting end 2 and the object (steel plate 1) facing it. Robot controller RBC has a connector on the external controller connection terminal.
The sonal computer 5 is connected. An external digital interface is connected to the personal computer 5, and in this embodiment, this digital interface is used.
An A / D converter 4 and up / down counters 11 to 16 incorporating a direction determination circuit are incorporated in the face. The A / D converter 4 has a distance detection circuit 3
An analog signal representing the detection distance Gs is given. The counters 11 to 16 are supplied with finger speed pulses of two sets each of the rotary encoders of the joints J1 to J6. The computer 5 uses the robot controller RBC to set a base point posture (housing) when a robot initialization is input from the operation board.
The robot controller RBC drives each joint to the home position (joint origin; axis position origin) in response to this, and when this is completed, the ready is notified to the computer 5. To do. The computer 5 clears the counters 11 to 16 in response to the ready. It should be noted that the robot controller RBC also operates at each joint position (j1s to j6s) when the home position drive is completed.
Position register to keep track of (1 area of internal memory)
To clear. It should be noted that the position (xhs, yhs, zh) on the x, y, z three-dimensional coordinate of the hand (H) at this axis position origin.
s) is the base point position.

After that, the computer 5 gives the robot controller RBC a target position (xo, yo, zo) designating an axis position deviating from the axis position origin, and the target position (x
(o, yo, zo) are sequentially changed, the hand H moves from the base point position, the rotary encoder of each joint generates finger speed pulses, and the counters 11 to 16 respond to these pulses.
Is counted up or down, and the robot controller RBC increments or decrements the data of the position register, and the counters 11 to 16 are counted.
The count data and the data of the position register of the controller RBC are the current position (j1s to j6) of each joint.
s; axis position). The current position (xs, ys, zs) of the hand H can be known by the inverse conversion from the axial position to the three-dimensional coordinates. The computer 5 converts the data (j1s to j6s) of the counters 11 to 16 into the current position data (xs, ys, zs) of the hand (H) by inverse conversion, and then the data of the hand (H). The current position is grasped, and the A / D converter 4 is instructed to perform data conversion to obtain digital data representing the detection distance Gs, and the current gap Gs (detection end 2 and steel plate 1 surface The distance).

FIG. 2 shows the functions of the computer 5 realized by the program loaded by the present inventor in block divisions. Here, the block division will be described.

The scan schedule SCS outputs the original target positions Xo and Yo and the target gap Go according to the scan schedule input by the operator so that the former becomes a two-dimensional x, y scan in time series.

Here, an example of the scan schedule is shown in FIG.
Will be described with reference to. The scan schedule is roughly the start point, the end point, and the x-direction scanning pitch P of the x, y two-dimensional surface scan.
It is defined by the x- and y-direction scanning pitch Py, the gap G, and the pitch cycle Ts, and the x-scan starting point Xs, the x-scan ending point xe, and the x-direction scanning pitch Px, y between the start point and the end point.
The positions Xc and Yc to be changed are included when at least one of the directional scanning pitch Py, the gap G, and the inter-pitch movement time Ts (pitch cycle) is changed. An area for storing these data (table SCT: FIG. 6) is allocated to one area of the internal memory of the computer 5. The operator operates the table S before driving the hand H for scanning.
The starting point coordinate value of the x, y two-dimensional surface scan is input to the position information Xc, Yc of the address 1 of CT, and Xs of the control information is
Input the x-coordinate value of the starting point coordinates, and in Xe of the control information,
An x-scanning end point coordinate value corresponding to the x-direction scanning width is input, the x-scanning pitch Px is set in Px of the control information, the y-scanning pitch is set in Py, and the upper surface of the target object at the scanning start point is set in G. Input the value obtained by adding the target gap to the height of, and enter the pitch period in Ts.

Next, the data of the address 1 is copied to the address 2 of the table SCT, and the G of the address 2 is subtracted from the target gap (the height of the upper surface of the target object from the G of the address 1). Value).

Then, until the end point of the x, y two-dimensional surface scan,
x-scan start point Xs, x-scan end point xe, x-direction scan pitch P
When none of the x- and y-direction scanning pitch Py, the gap G, and the inter-pitch moving time Ts is changed, x, y2 is added to the position information Xc, Yc of the address 2 of the table SCT.
The end point coordinate value of the plane scan is input, and the end data is input in the control information field. When any of the x-scan start point Xs, the x-scan end point xe, the x-direction scan pitch Px, the y-direction scan pitch Py, the gap G, and the inter-pitch movement time Ts is changed by the end point of the x, y two-dimensional surface scan. , Te
First in Xc and Yc of position information of address 2 of Bull SCT
The coordinate value of the second change point is input, and the x-scan start point Xs, the x-scan end point xe, the x-direction scan pitch Px, the y-direction scan pitch Py, the gap G, and the inter-pitch movement time Ts are assumed to be after changes. That is, the data of address 2 is copied to address 3, the copied Xc and Yc are changed to the coordinate values of the first change point, and the control information is changed in the same manner.
Similarly, the data at address 2 or 3 is copied to address 4, and the data at address 4 is copied to the second address in the same manner as described above.
Change to the second change. When the data input for all the changed points is completed in this way, for example, when the data of the last changed point is written in the address m-1, the x, y two-dimensional surface scanning end point is written in the next address m. Write coordinate values and end data (Fig. 6).

When the operator inputs the manual drive start, the computer 5 follows the data of the scan schedule table SCT (FIG. 6) and the original target position data X.
o, Yo and the target gap Go are generated, and while the generation is repeated in time series, the original target position data Xo, Yo are sequentially changed. This is the outline of the scan schedule SCS in FIG. The details of the contents will be described later with reference to the flowcharts shown in FIGS.

In the inverse transformation RCN shown in FIG. 2, the current positions j1 to j6 of the six axes of the robot mechanism RBT (counters 11 to 11).
16 count data) is converted back to the current position xs, ys, zs on the x, y, z coordinates of the hand H. This inverse transformation is also performed by the robot controller RBC,
This is a copy (port) of the inverse conversion program of the robot controller RBC.

The deviation (Xo-xs) of the current position xs from the original target position Xo is calculated by four PII (proportional, integral 1, integral 2) calculators PII-x1, PII-x2, PI of the x-axis system.
I-x3 and PII-x4, the original target position Y
The deviation (Yo-ys) of the current position ys with respect to o is the four PII calculators PII-y1, PII-y2, P of the y-axis system.
II-y3 and PII-y4, the detection gap Gs (A / D converter 4) for the target gap Go is given.
Deviation (Go-Gs) of the digital data of 4) of the z-axis system
PII calculators PII-G1, PII-G2 and PII
-G3 and PII-G4.

On the other hand, the region determination RDT indicates that the current position xs, ys, zs of the hand H divides the entire movement space of the hand H 9
It is determined which one of the areas 1 to 3 exists, and data indicating the determined area is given to the weighting adders ACX, ACY, ACG. The division of the 9 areas to will be described with reference to FIG. As shown in FIG. 7A, the entire movement space of the hand H has a vertical cross section VS whose one side coincides with the z-axis Lz1 which is the reference axis of the robot mechanism RBT, and 360 degrees around the z-axis Lz1.
It is a cylindrical space that the vertical cross section VS traverses when rotated once. Relatively low (small z coordinate value) in this space
The first circle having a small radius centered on the z-axis Lz1 and the fourth circle having a large radius are defined as the reference positions, and the first circle centered on the z-axis Lz1 is located at a relatively high position (the z coordinate value is large). A second circle having the same diameter as the 1st circle and a third circle having the same diameter as the 4th circle are defined, and the center of the first circle is ± c1 / 2 in the height direction and ± c2 / in the radial direction.
The second ring-shaped region is the second region, the second circle is the center, the height direction is ± c1 / 2 and the radial direction is the region ± c2 / 2 is the second region, and the third circle is the center. The ring-shaped region of ± c1 / 2 in the depth direction and ± c2 / 2 in the radius direction is the second region, and ± c1 in the height direction with the fourth circle as the center.
A ring-shaped region of ½ and ± c2 / 2 in the radial direction was defined as the first region. Then, the inner and outer ring-shaped regions of these ring-shaped regions are referred to as regions to. FIG. 7B
7 shows the distribution of each area on the vertical surface VS shown in FIG. The region determination RDT is based on the current position of the hand H √ (xs ²
+ Ys ² ), zs are compared with the radius and height of each of the first to fourth circles, and the height width c1 and the radius width c2 (these are fixed values determined by design). It is determined which of the areas 1 to 3 the current position (xs, ys, zs) is, and the data indicating the determined area is given to the weighting adders ACX, ACY and ACG.

First PII calculators PII-x1, PII-y1 and PII-G1 for the x-, y- and z-axis systems, respectively.
Is a PII feedback calculator with linear characteristics in the first region, and a second PII calculator PII-x2, P2.
II-y2 and PII-G2 are PII feedback calculators having a linear characteristic in the second region, and third PII calculators PII-x3, PII-y3 and PII.
-G3 is a PII filter that has linear characteristics in the third region.
And a fourth PII computing unit PI.
The I-x4, PII-y4 and PII-G4 are PII feedback calculators that have linear characteristics in the fourth region.

These PII calculators calculate the given deviation as proportional term + first integral term + second integral term and output the obtained values to the weighting adders ACX, ACY, ACG. The PII calculators PII-x1, PII-y1 and PII-G1 of 1 have a coefficient of the proportional term (proportional gain), a coefficient of the first integral term (integral gain) and an integral time constant (every one sampling cycle, The coefficient for multiplying the latest deviation added to the integrated value), the coefficient of the second integral term (integral gain) and the integral time constant are optimally set for the simplified linear model of the first region of the robot RBT + RBC.
That is, the controller is set to the optimum one for the simple linear model of the first region. Similarly, the second PII calculators PII-x2, PII-y2 and PII-G2 are set to the optimum controller for the simplified linear model of the second region, and the third PII calculators PII-x3 and PII are set. -Y3
And PII-G3 are set to the optimum controller for the simplified linear model in the third region, and the fourth PII calculator P
II-x4, PII-y4 and PII-G4 are the fourth
It is set to the optimum controller for the simplified linear model of the area.

The calculation processing of the x-axis weighted adder ACX, the y-axis ACY, and the z-axis ACG is the same. Therefore, the calculation processing of the x-axis weighting adder ACX will be described, and the description of the other parts will be omitted.

The x-axis weighting adder ACX determines the region determination R
If the data given by DT represents a region, the PII calculation data of the first PII calculator PII-x1 (deviation Xo-xs is expressed as proportional term + first integral term + second integral term). The calculated value is output. x-axis weighting adder AC
X is the PI of the second PII calculator PII-x2 when the data given by the region determination RDT represents a region.
Outputs I operation data. Similarly, if it represents an area, the PII operation data of the third PII operator PII-x3 is set to the fourth value if it represents an area.
The PII calculator PII-x4 outputs the PII calculation data. The data obtained by adding the current position xs to this output is given to the robot controller RBC as the target position xo.

The x-axis weighting adder ACX determines the region determination R
If the data given by DT represents a region, the PIs of the first and second PII calculators PII-x1, x2
Current position (zs), first circle, and second I calculation data
Inverse proportional sum x = x by the z-direction distances z1 and z2 with the circle
1 · z2 / (z1 + z2) + x2 · z1 / (z1 + z
2) is calculated and output. x1 and x2 are PII operation data of the first PII operator PII-x1 and the second PII operator PII-x2, respectively.

The x-axis weighted adder ACX determines the region determination R
If the data given by DT represents a region, the PIs of the third and fourth PII calculators PII-x3 and x4 are PI.
Current position (zs), third circle, and fourth of I operation data
Inverse proportional sum x = x by z-direction distances z3 and z4 with the circle
3 · z4 / (z3 + z4) + x4 · z3 / (z3 + z
4) is calculated and output. x3 and x4 are PII operation data of the third PII operator PII-x3 and the fourth PII operator PII-x4, respectively.

The x-axis weighting adder ACX determines the region determination R
If the data provided by DT represents a region, the PIs of the second and third PII calculators PII-x2 and x3 are PI.
The inverse proportional sum x = x2 · Dr3 / (Dr2 + Dr3) + x of the I position data √ (xs ² + ys ² ) and the radial distances Dr2 and Dr3 between the second circle and the third circle.
3 · Dr2 / (Dr2 + Dr3) is calculated and output.
x2 and x3 are respectively the second PII calculator PII
-X2 and the PII of the third PII operator PII-x3
This is calculation data.

The x-axis weighted adder ACX determines the region determination R
If the data given by DT represents a region, the PIs of the first and fourth PII calculators PII-x1, x4
The inverse proportional sum x = x1 · Dr4 / (Dr1 + Dr4) + x of the I position data √ (xs ² + ys ² ) and the radial distances Dr1 and Dr4 between the first circle and the fourth circle.
4 · Dr1 / (Dr1 + Dr4) is calculated and output.
x1 and x4 are respectively the first PII calculator PII
-X1 and the PII of the fourth PII operator PII-x4
This is calculation data.

The x-axis weighting adder ACX determines the region determination R
If the data given by DT represents a region, the current position zs, √ (xs ² + ys ² ) and the ^second position of the PII operation data of the first to fourth PII operation units PII-x1 to x4 are calculated. Inverse proportional sum of distances z1 and z2 in the height direction from the 1,4 circle and the second and third circles and radial distances Dr1 and Dr2 from the first, second circle and the third and fourth circles x = x ₁ · Dr2 / (Dr1 + Dr2) + x 2 · Dr1 / (Dr1 + Dr2) x 1 = x1 · z2 / (z1 + z2) + x2 · z1 / (z1 + z2) x 2 = x3 · z2 / (z1 + z2) + x4 · z1 / (z1 + z2) calculated And output.

As in the case of the area ~ determination, the data obtained by adding the current position xs to the output calculated as described above is also used as the target position xo when the area ~ is determined. Given to RBC.

PII calculators PII-x1 to x4, PII
-Y1 to y4 and PII-G1 to G4 respectively have a proportional gain, a first integral gain and a first integral time constant, and a second integral gain and a second integral time constant.
A simple linear model of T + RBC was derived and set based on it.

That is, the first PII calculator P of the x-axis system
For II-x1, place the hand H on the first circle (center of area)
Then, a sine wave having an amplitude of ± 1 mm is applied to the robot controller RBC as a target position xo signal,
This sine wave is increased from 1 Hz to 30 Hz, the input and output (input frequency, rotational vibration of the joint J1 of the hand H) at this time are sampled, and a board diagram is obtained based on the input and output data. Then, the model parameters were calculated. The first PII calculator PII-x1 of the x-axis system was set as the optimum controller for the linear model in which the calculated model parameters were set. That is, as shown in FIG. 8A, in the feedback system that is a combination of the first PII calculator PII-x1 of the x-axis system and the set linear model (RBC + RBT), the linear model (RBC + RBT) ( Hand H
2) proportional delay, a first integral gain and a first integral time constant, and a second integral that minimize the response delay and overshoot.
The integral gain and the second integral time constant are set in the first PII calculator PII-x1.

The PII characteristics of the second, third and fourth PII calculators PII-x2 to x4 of the x-axis system were similarly set. However, the hand H was placed on the second circle (center of the area), the third circle (center of the area), and the fourth circle (center of the area), and the transfer function of the robot RBT + RBC was obtained. That is, a linear model was established.

Y-axis system PII calculators PII-y1 to y4
The PII characteristic of was also set in the same manner as the setting of the above-mentioned x-axis PII calculator. However, robot RBT + RBC
The sine wave (input) corresponding to the amplitude of ± 1 mm for obtaining the transfer function of was applied to the robot controller RBC as the target position yo signal. The horizontal vibration of the joint J2 of the robot RBT + RBC is sampled as the output of the robot,
A board diagram was obtained from these inputs and outputs. FIG. 8B shows a y-axis PII calculator (PII-y1 to y).
4) shows the configuration of the feedback system which is a combination of the robot RBT + RBC.

The PII characteristics of the PII calculators PII-G1 to G4 of the z-axis system (gap G control system) were set in the same manner as the setting of the PII calculator of the x-axis system described above. However, the sine wave (input) having an amplitude of ± 1 mm for obtaining the transfer function of the robot RBT + RBC is the target position zo.
The signal was given to the robot controller RBC as a signal. The vertical vibration of the joint J3 of the robot RBT + RBC was sampled as the output of the robot, and the board diagram was obtained from these inputs and outputs. In (c) of FIG. 8, a PII calculator (P
II-G1 to G4) and a robot RBT + RBC are combined to form a feedback system. When the scanning control is not set, the feedback system of the z-axis system is similar to the above-mentioned x-axis system and y-axis system, but in this embodiment, the PII calculator PII is used to perform the scanning control.
-G1 to G4 calculate the control amount by performing PII calculation on the deviation Go-Gs of the detection gap Gs with respect to the target gap Go, and add the current position zs to this to calculate the target position z.
It is given to the robot controller RBC as o.

The contents of the "copy drive" control of the computer 5 are shown in FIGS. This "copy drive" control is performed by "operator initialization" after the operator inputs data to the scan schedule table SCT as described above.
Is input, and the drive instruction is input to the computer 5, the computer 5 starts. As described above, the computer 5 instructs the robot controller RBC to set the base point posture (home position) when the “robot initialization” is input, and in response to this, the robot controller RBC causes the robot controller RBC to respond. Each joint is driven to the home position, and when this is completed, the ready is notified to the computer 5. When the computer 5 receives this ready, it clears the counters 11 to 16, displays drive ready on the operation board, and waits for a drive instruction to be input. The operator is
After this drivability is displayed, a driving instruction is input to the computer 5.

First, referring to FIG. When the drive instruction is given, the computer 5 writes 1 in the register (internal memory) Rn for designating the read address of the table SCT (step 1). In the following, the word step will be omitted in parentheses and only the number symbol will be described. Next, the address Rn of the table SCT (register R
(n data) is read (2), and it is checked whether the read data indicates the end (driving end) (3). If not, the read data is read. In the registers Xc, Yc, Xs, Xe, Px, Py, G and Ts are respectively registered in registers RXo, RYo, RXs, R.
Write to Xe, RPx, RPy, RG and RTs (4).

As already described, when the address is 1, Xc and Yc are the x and y coordinate positions of the drive start position, Xs and Xe are the start and end coordinates of the x scan, and P is the P coordinate.
x and Py are target positions x in the x-scanning direction and the y-scanning direction
o, yo change pitch, G is designated gap, and T
s is a target position (xo, yo, zo) change period, that is, a pitch period. When the address is 2 or more (n, Rn ≧ 2), Xc and Yc are the control information (Xs, Xe, Px, P).
This is the position where y, G, Ts) is changed.

Write the read data to the register (4)
And the computer 5 outputs the data of the registers RXo, RYo and RGo to the target positions xo, yo, zo, respectively.
It is given to the robot controller RBC as (5). The robot's hand H is thereby driven to the start position. The computer 5 reads the joint positions j1s to j6s (count data of the counters 11 to 16) and the detection gap Gs until the hand H reaches the start position.
j1s to j6s are converted into x, y, z coordinate values xs, ys, zs (6), and it is checked whether the hand H has reached the start position (7). When the hand H reaches the start position, the computer 5 writes 1 (outward direction) in the register RDF indicating the x-scanning direction (8), and the RTs timed timer RT.
Start s (9). This time limit is the table SCT
Is the Ts input to. With the above, the hand H is positioned at the start position.

Next, the computer 5 specifies the address 2 of the table SCT (10, 29), reads the data of the address 2 and writes it in the register (29-32). Here, the target gap G is set (by reading the data of the address 2). That is, at address 1, G is the height of the upper surface of the copying object 1 plus the target gap value, but at address 2, it is only the target gap value.

Next, the computer 5 checks the data of the register RDF, and if it is 1 indicating the forward direction, the original target coordinate value Xo is updated by a large amount by one pitch Px in the x direction ( 12 and 13), when the timer RTs time-overs, the timer RTs are restarted (14, 15),
The joint positions j1s to j6s are read in (16), and x,
It is converted into the position xs, ys, zs of the hand H on the y, z coordinates (17), and it is determined whether the obtained position xs, ys, zs is in the region to (18). Then, the PII calculation corresponding to the area described above (PII-x1 to x in FIG. 2) is performed.
4, ACX, PII-y1 to y4, ACY and PII
-G1 to G4, ACG described above) is performed to calculate the control amount, and the current position xs, ys, zs is added to the control amount to obtain target values xo, yo, zo (19), the robot controller. Give to RBC (20). This process (1
0 to 20) are performed until the end point Xe of the x-scan.

When the end point Xe of the x-scan is reached, the original target coordinate value Yo is updated to a value larger by one pitch Py (2 in FIG. 4).
1), write 0 to the register RDF to indicate the backward direction (2
2) When the original target coordinate value Xo is updated to a value smaller by one pitch Px (23) and the timer RTs is time over,
The timer RTs is restarted (14, 15), the joint positions j1s to j6s are read (16), and x, y, z are read.
It is converted to the position xs, ys, zs of the hand H on the coordinates (1
7), it is determined which one of the areas the obtained positions xs, ys, zs are (18), the PII calculation corresponding to the area is performed to calculate the control amount, and the control amount is calculated as the current position xs, y.
s and zs are added to obtain target values xo, yo and zo (1
9), give to the robot controller RBC (20).
This processing (10, 11-24, 25-14 to 20)
The process is performed up to the starting point Xs of x scanning.

When the starting point Xs of x scanning is reached, the original target coordinate value Yo is updated to a value larger by one pitch Py (2 in FIG. 5).
6), 1 which indicates the forward direction is written in the register RDF (2
7), the same processing as the above-mentioned forward processing (10 to 20)
To the end point Xs of x scanning.

In this way, the computer 5 forward scans in the x direction, updates y position for one pitch at the end point (y scanning), backward scans in the x direction, and y for one pitch at the end point. The position update (y scan) is repeated in this order. When the change point of the control information (Xc, Yc of the address i + 1 when scanning based on the control information of the address i of the table SCT) is reached, this is detected in step 10 and the address Rn is set to 1 After incrementing (29), the control data at the address Rn + 1 of the table SCT is read (3
0), if it is end data, then the target position x
Stop updating o, yo, zo. If it is not end data, the read control information Xs, Xe, Px, Py, G and Ts are stored in registers RXs, RXe, RPx, RPy, R.
The G and RTs are written, and the position information Xc, Yc of the address next to the currently read address is read and written in the registers RXo and RYo (31, 32). Step 10 detects the arrival at this (position Xc, Yc of the next address). Thus registers RXo and RYo and RXs, RXe, RPx, RPy, R
After rewriting the data of G and RTs, the above-mentioned forward scan in the x direction, y position update for one pitch at the end point (y scan), backward scan in the x direction and one pitch at the end point. The y-position update (y-scan) is repeated in this order in a manner continuous with that immediately before updating the register data.

When it is detected in step 10 that the control information change position has been reached, the read address of the table SCT is incremented by 1 and (29) when the data of the table SCT is read, the control information is changed to the end data. If it is negative, the copying drive is ended there. That is, the robot controller RBC
The update of the target positions xo, yo, and zo is stopped. The hand H stops moving there.

By the "copy control" described above, the hand H
Is a specified gap G in the z direction with respect to the steel plate 1 placed on the reference surface SS shown in FIG. 1, and Px /
Two-dimensional scanning drive is performed at a speed of Ts. The scan area is
This is a roughly rectangular area in which Xc and Yc of address 1 of the bull SCT are the starting points and Xc = Xm and Yc = Ym of the address m are the ending points, but x is between the starting point and the ending point of the two-dimensional scanning.
When the scanning start point Xs or the end point Xe is changed, the area deviates from the rectangle. When the designated gap G is not changed, a constant gap is maintained with respect to the upper surface of the steel plate 1.

As described above, the non-linear characteristics of the multi-axis arm robot can be easily dealt with, and a commercially available relatively inexpensive general-purpose robot (RBT + RBC) can be used as it is.
Easy to install robot. In the above-described embodiment, the motion space of the hand H of the robot RBT + RBC is set to a plurality of areas.
It is divided into, and the area in it is defined as the reference area,
The motion of the hand H in each reference region is represented by a simple linear model (transfer function), and the corresponding feedback calculation PII-x1 to x4, PII-y1 to y4, PII
-Since G1 to G5 are simple and relatively easy to design and adjust, these settings and adjustments are easy, and at the same time, the robot has a high speed and high precision that best suits the transfer characteristics of each reference area of the robot. Positioning can be realized.

In the areas other than the reference area-, the weighted additions ACX, ACY, ACG are included in the feedback calculated values of a plurality of reference areas in contact with the area (-) in which the hand H exists. Since the control amount is a sum obtained by weighting and adding the position difference corresponding to the position difference, it is possible to realize high-speed and high-accuracy positioning that is suitable for the transfer characteristics of each of the areas other than the reference area. For example, when the hand H moves from one of the reference areas to another one, the control amount output by the weighted addition ACX, ACY, ACG changes from one corresponding to the reference area to another corresponding to the reference area. The position changes continuously, and highly accurate positioning is realized even in a region outside the reference region. The hand H moves smoothly and does not move discontinuously. Even if the number of divisions of the motion space of the hand H is as large as 9, the number of feedback calculations to be set and adjusted is four addressed to the reference area, and the number of feedback calculations is small with respect to the number of area divisions. Fewer feedback calculation settings and adjustments are required. Non-linear characteristics of multi-axis arm can be easily dealt with, and a relatively inexpensive general-purpose robot (RBT + R) on the market is available.
Since BC) can be used as it is, the robot can be easily installed.

Commercially available relatively inexpensive general purpose robot RBT
By attaching the detection end 2 of the distance detector (2-4) to the hand (H) of + RBC, the general-purpose robot RBT + RBC is used as it is without changing or adding the internal mechanism or control algorithm of the robot RBT + RBC. The (H) copying drive can be realized.

[Brief description of drawings]

FIG. 1 is a block diagram showing a schematic configuration of an embodiment of the present invention.

FIG. 2 is a block diagram showing functions of the computer 5 shown in FIG.

FIG. 3 is a “copy drive” of the computer 5 shown in FIG.
It is a flowchart showing a part of the contents of control.

FIG. 4 is a “copy drive” of the computer 5 shown in FIG.
It is a flowchart showing a part of the contents of control.

5 is a "copy drive" of the computer 5 shown in FIG.
6 is a flowchart showing the rest of the control contents.

FIG. 6 is a plan view showing data items that the operator inputs in advance to the computer 5 for “copy drive”.

7A is a perspective view showing a cross-section VS of a part of the motion space of the hand H of the robot shown in FIG. 1, and FIG. 7B is a plan view showing a region division on the cross-section VS.

8 is a block diagram showing the relationship between the transfer function of the robot RBT + RBC shown in FIG. 1 and the function of the computer 5 acting on the robot. 1: Steel plate (opposite object) 2: Distance detection end 3: Distance detection circuit 4: A / D
Converter 5: Computer RBT: Robot mechanism J1: Horizontal rotary joint J2: Horizontal direct acting joint J3: Vertical direct acting joint J4: Revolving joint J5: Vertical rotating joint J6: Front / rear rotating joint Lz1: First vertical axis Lxy : Horizontal axis Lz2: Second vertical axis SS: Reference horizontal plane MD1 to MD6: Motor driver RBC: Robot controller OPB: Operation board 11 to 1
6: Up-down counter PII-x1 to x4, PII-y1 to y4, PII-G1 to G4: Feedback calculator ACX, ACY, ACG: Weighted adder

Claims (6)

[Claims] 1. A hand support mechanism having three or more axes for driving a hand in a three-dimensional space, a position tracking means for recognizing the position of the driving source and the hand, and a hand for driving the hand to a given target position. Region determination means for determining which of a plurality of preset regions the position of the hand is in a robot equipped with a controller that drives a hand support mechanism via the drive source; A plurality of feedback calculation means for calculating a control amount which is suitable for feedback control in the allocated area and which makes the deviation of the position of the hand from the original target position zero; Of the hand for adjusting the deviation of the position of the hand from the original target position to zero based on the control amount and the position of the hand calculated by the feedback calculating means assigned to the region. A controller for a robot, comprising: target position calculation means for calculating a target position and giving it to the controller. 2. A hand support mechanism of three or more axes for driving the hand in a three-dimensional space, a position tracking means for recognizing the position of the drive source and the hand, and a hand for driving the hand to a given target position. A distance measuring device of a robot equipped with a controller for driving a hand support mechanism via the drive source, the distance measuring device being connected to the hand and detecting a distance to an opposing object; the position of the hand is located in any of a plurality of preset regions. Area determining means for determining whether or not each is assigned to each of the plurality of areas, and is a control amount suitable for feedback control in the assigned area for making the deviation of the detected distance from the target distance zero. A plurality of feedback calculation means for calculating; and a deviation of the detection distance from the control amount calculated by the feedback calculation means assigned to the determined area and the position of the hand. A controller for a robot for copying, comprising: a target position calculating means for calculating a target position of a hand for making the difference zero and giving it to the controller. 3. A hand support mechanism having three or more axes for driving the hand in a three-dimensional space, a position tracking means for recognizing the position of the drive source and the hand, and a hand for driving the hand to a given target position. A robot equipped with a controller that drives a hand support mechanism via the drive source, a distance measuring device that is coupled to the hand and detects a distance to an opposing object; the position of the hand is located in any of a plurality of preset regions. Area determination means for determining whether there is any; control for reducing the deviation of the hand position from the original target position to zero, which is assigned to each of the plurality of areas and is suitable for feedback control in the assigned area A plurality of feedback calculation means for calculating an amount and a control amount for making the deviation of the detected distance from the target distance zero; and, the feedback calculation means is assigned to the determined region. From the control amount calculated by the feedback calculation means and the position of the hand, the target position of the hand for zeroing the deviation of the position of the hand from the original target position and the deviation of the detection distance is calculated, and the control is performed. A control device for a robot, comprising: a target position calculation means to be applied to the robot. 4. The robot controller according to claim 1 or 3, wherein the target position calculation means changes the original target position at successive set pitches in order to move the hand along a predetermined locus. 5. The feedback calculating means has a plurality of first calculating means which are smaller in number than the plurality of areas, and the determined areas do not correspond one-to-one with the first calculating means. For a region, 1 for each of the adjacent regions
The second calculation means for calculating the sum as a control quantity, which is extracted from the calculation control quantity of the first calculation means corresponding to the pair 1 and corresponds to the distance of the hand to the adjacent region, and added. The robot control device according to claim 2, claim 3, or claim 4. 6. The calculation of the first calculation means is proportional term calculation, first calculation
6. The robot controller according to claim 5, including an integral term operation, a second integral term operation, and addition of the results of these operations.