JP2923713B2 - Drive control method for cylindrical coordinate robot - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION The present invention relates to a robot
The first arm installed vertically and the first arm
Has translational freedom in a plane approximately parallel to the robot installation surface
At the same time, within a predetermined plane defined for the first arm.
A second arm having a degree of freedom of rotation directly or indirectly;
Connected to the tip of the two arms at a substantially right angle to the axis of the second arm
Rotational freedom around direction and freedom around its own axis
A third arm having a degree and a substantially
The present invention relates to a drive control method for a cylindrical coordinate robot having an end effector coupled to a corner .

[0002]

2. Description of the Related Art As a method of causing an industrial robot to perform a desired operation, points on a path to be followed by a tip of an end effector of the robot are sequentially taught, and a curve or a straight line connecting these teaching points smoothly is interpolated. The method is widely known (teaching playback). Therefore, in order to control the driving of the robot by using this method, the joint displacement (the degree of freedom of translation and the degree of freedom of rotation) of the robot from the XYZ absolute coordinate system (X system) expressing the position and orientation of the tip of the end effector of the robot is considered. Is included). Coordinate transformation to the joint coordinate system (α system) expressing the above, that is, inverse transformation is required. In addition, since it is necessary to perform this inverse conversion in real time in synchronization with the operation of the robot, an efficient inverse conversion method is required.

In general, robot coordinate conversion includes a forward conversion from an α-system to an x-system and the above-described inverse conversion. It is known that the former can analytically obtain an exact solution regardless of the axis configuration of the robot. Similarly, for the latter, a method of analytically obtaining an exact solution has been attempted. For example, RCPaul, "Robot Manipulators; Mat
hematics, Programming and Control "MIT Press (198
1) shows a specific example. That is, the conversion formula of the inverse conversion is obtained algebraically from the conversion formula of the forward conversion.

However, depending on the axis configuration of the robot, an exact analytical solution may or may not be obtained.
Generally , various postures at all positions on the workpiece
The robot's motion function while holding the end effector
The axis configuration of the robot is complicated from the viewpoint of improvement
For, if it is not possible to obtain the analytically exact solution is predominantly. Therefore, a solution of the inverse transformation is approximately obtained by using a method based on numerical analysis, for example, a convergence operation method.

Another attempt has been made to approximately find the solution of the inverse transformation. For example, there is a method disclosed in JP-A-64-8407. In this method, a parameter group indicating the position and orientation of the tip of the end effector of the robot is determined from the work content of the robot, and a first parameter group that needs to be accurately controlled and a second parameter group that does not need to be controlled. The robot is classified into groups, and at least one of the joint displacements of the robot that does not affect the first parameter group is approximated using a Jacobian matrix (Jacobian) as shown in the following equation. That is, the current position and orientation and next to be moved position of the end effector tip of the robot and the respective vectors X ₀ and vector X ₁ vector representing the orientation at that location, corresponding to the vector X ₀ and the vector X ₁ If the vectors indicating the joint displacements of the robot are represented by vectors θ ₀ and θ ₁ , and the Jacobian is represented by J (vector θ), the above-described approximate solution vector θ ₁ is given by

[0006]

(Equation 1)

Is required. Then, using an approximate solution vector theta ₁ of a portion of the joint displacement obtained in this way,
The solution of the remaining joint displacement is determined so as to satisfy the value of the first parameter. In this case, by excluding the joint displacement obtained as an approximate solution from the inverse transformation equation, the unknown number of the inverse transformation equation can be reduced by that amount, and in many cases, the remaining joint displacement is analyzed analytically. To find the solution.

[0008]

The first method, that is, a method for obtaining a numerical analysis solution by a convergence operation method has an advantage that a highly accurate solution with a small position error and attitude error can be obtained. Since it requires time, there is a problem that it is not suitable for driving and controlling a robot in real time. There is also a problem that a solution does not converge depending on the axis configuration of the robot. In fact, the figure
In the cylindrical coordinate robot having the configuration described later with reference to FIG.

In the second method, that is, a method of separately obtaining an approximate solution of only a part of the joint displacements using the Jacobian, the remaining joint displacements are approximated by an inverse transformation equation. This has the advantage that the solution can be easily obtained. However, in order to obtain an approximate solution, it is necessary to obtain an inverse matrix of Jacobian every time the end effector moves, so that the calculation process becomes complicated and it is not always a simple calculation method. Further, there is a problem that both the position and the attitude of the end effector tip of the robot have errors.

[0010] The above problem has a complicated shaft configuration.
If it is a common problem in industrial robot in general to
Although it can be said , in particular, a cylindrical coordinate robot having a complicated axis configuration, which is a target of the present invention , that is, a robot installed substantially perpendicular to the robot installation surface.
A first arm and a robot coupled to the first arm
With translational freedom in a plane approximately parallel to the installation surface
In addition, directly or in a predetermined plane determined with respect to the first arm.
Is indirectly provided with a second arm having a rotational degree of freedom,
And connected to the tip of the
The degree of freedom of rotation and the degree of freedom of rotation about its own axis.
And a third arm having substantially the right angle with the tip of the third arm.
Combined For <br/> in cylindrical coordinates robot having an end effector, the need to precisely control actually the robot
This is a problem that needs to be resolved immediately .

According to the present invention, the robot is installed substantially perpendicular to the robot installation surface.
A first arm and a robot coupled to the first arm
With translational freedom in a plane approximately parallel to the installation surface
In addition, directly or in a predetermined plane determined with respect to the first arm.
Is indirectly provided with a second arm having a rotational degree of freedom,
And connected to the tip of the
The degree of freedom of rotation and the degree of freedom of rotation about its own axis.
And a third arm having substantially the right angle with the tip of the third arm.
The present invention has been made to solve the above-described problem with respect to a cylindrical coordinate robot having a combined end effector, and it is possible to accurately and quickly move the end effector tip of the robot to a designated position and posture. in and to provide a drive control method for <br/> kill cylindrical coordinates robot.

[0012]

According to a first method of the present invention, a first arm installed substantially perpendicular to a robot installation surface and a first arm coupled to the first arm are provided in a plane substantially parallel to the robot installation surface. A second arm having a degree of freedom of translation and having a degree of freedom of rotation directly or indirectly in a predetermined plane defined with respect to the first arm; and a second arm coupled substantially perpendicularly to a tip end of the second arm. A third arm having a degree of freedom of rotation about the axis of the arm and about its own axis;
A drive control method for a cylindrical coordinate robot having an end effector coupled to the distal end of the third arm at a substantially right angle,
(A) specifying the position and orientation of the tip of the end effector; and (b) a plane that includes the axis of the end effector and that is perpendicular to a predetermined plane intersects a plane whose normal direction is the axial direction of the end effector. (C) selecting any one of a reciprocal of a cosine of an angle formed between the intersection line and the axial direction of the third arm and a reciprocal of a sine of the angle.
(E) calculating the solution of a quartic equation using the reciprocal selected in step (b) as a variable; (d) obtaining the value of the angle corresponding to the position and orientation from the solution of the quaternary equation; ) First, second and third using the value of the angle.
Determining the respective drive amounts of the arms; and (f) providing drive outputs corresponding to the drive amounts to the respective drive mechanisms of the first, second, and third arms, respectively. And a step of driving.

The second method relates to a drive control method for a cylindrical coordinate robot having first, second and third arms and an end effector similar to the robot in the first method. A step of designating a plurality of positions and postures of the tip of the effector; and (b) determining whether the posture is within a predetermined specific area;
(C) when the posture is in the specific region, a plane including the axis of the end effector and perpendicular to a predetermined plane intersects with a plane intersecting a plane whose normal direction is the axial direction of the end effector; A reciprocal of the cosine of the angle with respect to the axial direction of the third arm or a reciprocal of the sine of the angle is selected, and a solution of a quartic equation having the selected reciprocal as a variable is calculated. (D) determining the value of the angle corresponding to the position and orientation from the solution of the quartic equation; and (d) when the orientation is outside the specific region, one of the cosine of the angle or the sine of the angle Is selected as a variable, a solution of a quartic equation relating to the variable is calculated, and a value of the angle corresponding to the position and orientation is obtained from the solution of the quartic equation; and (e) using the value of the angle Of the first, second and third arms Determining a driving amount,
(F) driving the first, second and third arms by applying a drive output according to the drive amount to each of the first, second and third arm drive mechanisms, respectively. It is.

[0014]

In the drive control method according to the present invention, a plane perpendicular to a predetermined plane defined by the first arm and including the axis of the end effector intersects a plane perpendicular to the axial direction of the end effector. The focus is on the geometric relationship between the virtual angle between the intersection line and the axial direction of the third arm and the turning angle of the second arm in the predetermined plane.

That is, in the first method, a quartic equation that holds for the reciprocal of the sine or cosine of the virtual angle is directly analytically solved, and the turning angle is determined from the obtained solution. Then, the drive amount of each arm is strictly determined by substituting the turning angle value into an inverse conversion formula relating to the position and orientation of the end effector. Further, each arm is driven based on the obtained drive amount of each arm. As a result, the tip of the end effector moves accurately to the designated position without error, and maintains the designated posture.

In the second method, if it is determined that the posture is within a predetermined range among a plurality of designated postures, the reciprocal of the sine or cosine of the virtual angle is established. Is solved directly analytically, and the turning angle is determined from the obtained solution. On the other hand, if it is determined that the posture is out of the predetermined range, a quartic equation that holds for the sine or cosine of the virtual angle is directly analytically solved, and the turning angle is determined from the obtained solution. . Therefore, regardless of the designated position in the absolute coordinate system, the tip of the end effector moves accurately to the designated position without error, and maintains the designated posture.

[0017]

【Example】

A. FIGS. 1 and 2 are a perspective view and a schematic side view, respectively, showing a mechanical configuration of a laser welding robot RB as a cylindrical coordinate robot according to an embodiment of the present invention. Each degree of freedom of the laser welding robot RB is schematically represented.

In both figures, the installation surface 10 of the robot RB
The X system is defined as the absolute coordinate system fixed above, and the first arm 21 extends along the Z axis from the base 11 of the robot RB. The first arm 21 is moved around the Z axis, that is, around the axis A1 of the first arm 21.
It is possible to turn in the Θ direction by a first drive mechanism provided inside.

A second arm 22 is provided so as to intersect the first arm 21 at a substantially right angle. The second arm 22 turns in the Θ direction with the turning of the first arm 21. Therefore, the second arm 22
Can be said to have a degree of freedom of rotation in the XY plane indirectly through the rotation of the first arm 21. Moreover, the second arm 22 can be moved up and down in parallel with the Z-axis by a second driving mechanism provided inside the first arm 21, and further, by a third driving mechanism provided inside the second arm 22. It can translate in the XY plane. The axis A1 of the first arm 21 and the second arm 2
The position coordinates of the intersection M of the second axis A2 with the axis A2 with respect to the X system
(0, 0, z _s ).

A third arm 23 is connected to the distal end 31 of the second arm 22 at a substantially right angle to the second arm 22, and the third arm 23 is provided inside the distal end 31 of the second arm 22. The provided fourth drive mechanism allows the second arm 22 to rotate in the α direction about the axis A2. Here, representing the relative distance between the axis A2 and the point P and the point M of the tip of the second arm 22 by the variable x _s.

Further, the third arm 23 can be rotated in the β direction about its own axis A3 by a fifth driving mechanism provided inside the distal end portion 32 of the third arm 23. The length of the third arm 23 is fixed to a constant value d _1.

Also, a torch 4 as an end effector
0 is connected to the distal end portion 32 of the third arm 23 at a substantially right angle, and the length of the torch 40, that is, the point N at the distal end of the torch 40
And the axis A4 of the torch 40 the distance Q point of the tip of the third arm 23 has a constant value d _2.

The torch 40 is provided with a work detection sensor (not shown), and the torch 40 itself can rotate in the γ direction about its own axis A4 as a rotation axis. The torch 40 itself has perfect rotational symmetry about its axis A4, and has an angle γ corresponding to its roll angle.
Has meaning only to determine the position of the sensor. Therefore, the angle γ is not a value determined by the coordinate transformation, but an angle that should be specified separately and independently. This is the reason that there is no formula for determining the angle γ in the coordinate transformation described later.

Table 1 exemplifies the range of all the degrees of freedom of the respective arms excluding the rotational degree of freedom of the torch 40 itself. The symbol z ₀ in Table 1 is the height of the point M when the second arm 22 comes into contact with the base 11.

[0025]

FIG. 3 is an explanatory view showing the position and orientation of the tip (point N) of the torch 40, and is represented by an x 'system with the point Q as the origin. In the figure,
Each axis direction of the x 'system is set so as to correspond to each axis direction of the X system. That is, the x 'axis, the y' axis, and the z 'axis are parallel to the X axis, the Y axis, and the Z axis, respectively. Where origin Q
Is represented as Q (x _Q , y _Q , z _Q ) in the X-system display.

First, the attitude of the point N is determined by the vector QN and z ′.
Angle with the axis (zenith angle) θ and x ′ of vector QN
It is determined by the angle (azimuth) φ between the projection vector onto the y ′ plane and the x ′ axis.

The position is represented by position coordinates (x ', y',
z ′). Therefore, the position coordinates (x, y, z) of the point N in the X system display and the position coordinates (x ',
y ′, z ′), x = x _Q + x ′, y = y _Q +
The relationship of y ′ and z = z _Q + z ′ holds.

B. FIG. 4 is a block diagram showing an electric configuration of a control system of the laser welding robot RB. This control system includes a CPU 511
And a controller 51 including a memory 512 and the like, and is controlled by software. By operating the teaching box 52, teaching data is taken in. Various command signals from the controller 51 (drive signals D _1, etc.) is given through the data bus 57 to each component (such as the first drive mechanism 531).

The operator can set various data necessary for the software control of the controller 51 by using the input / output device 56 such as a keyboard.

On the other hand, the drive signal D ₁ to the motor M ₁ constituting the first drive mechanism 531 is transmitted, the motor M ₁ is operated, the encoder E attached to the motor M _1
1 detects the turning angle Θ of the second arm 22 around the axis A1. Similarly, the second to fifth driving mechanisms 532 to 535
Drive signals D _{2 to} D ₅ are transmitted to the respective motors M
₂ ~M ₅ operates, displacement z _s, x _s and rotation angle alpha, beta
There is detected by the encoder E ₂ to E _5, respectively.

The laser power supply 54 is connected to the laser oscillator 5
5 is a power supply for supplying power to the
It operates in response to a command from 1. The laser beam LB generated by the laser oscillator 55 is transmitted to the robot RB
Irradiated toward the work from the tip of the torch 40 while being sequentially reflected by mirrors provided in the respective arms.

C. Driving Control Method FIGS. 5 and 6 are flowcharts showing steps for controlling the driving of the robot RB. FIGS.
5 is a flowchart for explaining step S6 in FIG.

(C-1) Step S 1 First, a specific area is set. That is, the operator gives data of a specific area to the controller 51 in advance using the input / output device 56. The specific area is an angle area specially defined with respect to the zenith angle θ in the posture. When the zenith angle θ of the teaching point or the interpolation point falls within this area, Japanese Patent No. 2750634 describes
The g method disclosed in the patent gazette, that is, the intersection angle δ (described later)
(See paragraph [0054])
Fourth-order equations that can be easily analyzed are related to the physical arrangement of the axes of each arm.
When deriving the cosine or sine of the angle δ,
Variable method (g-cos δ method or g-sin δ method)
Without applying paragraph numbers to paragraph numbers [0180] to [0210].
In this case, the improved g method described later is applied. Immediately
The reciprocal of the cosine or sine of the intersection angle δ is used as a variable.
A quartic equation of the above angle δ that can be set and rigorously analyzed is derived.
Will be issued. The reason for setting a specific area such as this, paragraphs [0169] - [0179] smell
As described later , the outline is as follows.
is there. That is, Japanese Patent No. 2750634
In the method of the g method disclosed in
To find the solution of the inverse transformation (drive amount of each arm)
Then, once the torch axis outside the path is parallel to the y axis
Move the first arm by the azimuth angle φ to move the torch to
It is assumed that only a virtual turn is performed. At this time
Makes the physical relationship of each axis easier and the intersection at that position
Focusing on the angle δ and considering the physical relationship of each axis above,
Be sure to derive a quartic equation of the intersection angle δ that can be analyzed rigorously
(From the solution of the intersection angle δ, the
The pivot angle of one arm is determined, and the physical relationship of each axis and the original
From the physical positional relationship between the interpolation point and the assumed point,
Are obtained. However, the intersection angle
When deriving a quartic equation of δ, the cosine or sine of the
, The zenith angle θ is included in the coefficient of the quartic equation
Therefore, if the zenith angle θ is within the specified area,
Range in which the coefficient value of the quartic equation can be controlled electrically
It will be outside, and in electrical calculation processing, apparently high
The solution cannot be determined with accuracy. However, the g method
The basic idea is the physical relationship of each axis, the interpolation point and the reality
And the positional relationship with the above assumed point outside the possible route
Also the because is that, mosquitoes sure of the exact each arm in theory
The momentum should always be determined. This discrepancy is eventually 4
If there is a cause in the setting of the variables that we focused on when deriving the following equation
I can say. To do so, change the variable setting to the value of the zenith angle θ.
It changes with. Therefore, the intersection with the specific area
Deriving a quartic equation using the cosine or sine of the angle δ as a variable
When the g method is basically adopted, the calculation capacity of the computer
When it is impossible to solve the quartic equation with high accuracy,
By changing the setting of the variable of the quartic equation for the angle δ,
The solution that should be able to be obtained strictly is limited by the computational capacity of the computer.
Judgment factors to be able to derive exactly within the range
Can be said to be within a certain range of the zenith angle θ. In this embodiment, the specific area is 0 ° to (0 ° + Δθ) or (180 ° −
Δθ) to 180 °.

(C-2) Step S2 (Teaching Step) Teaching data X _i (i = 1, 2,..., N) is created and stored. The teaching data X _i is a concept including the position (x, y, z) and posture (θ, φ) of the teaching point P _i , and the driving amount (actually measured value) of each arm that gives the position and the like. It is used as

First, the operator sets each teaching point P
The tip of the torch 40 is manually positioned for each _i . At that time, the driving amount of each arm of each teaching point P _i (alpha-based value of) is read by the encoder E ₁ to E _5.

Then, the position of each teaching point P _i (x _i , y _i ,
z _i ) and attitude (θ _i , φ _i ) are calculated. As for the forward conversion, an exact solution can be analytically obtained as described above.
1 is programmed. The calculated position (x _i , y _i , z _i ) and posture (θ _i , φ _i ) are stored in the memory as teaching data X _i .

(C-3) Step S3 When the above preparation steps are completed, the CPU 511 issues an oscillation command signal to the laser power supply 54. Thereby, the laser power supply 54 turns on the laser oscillator 55.

The next step group S10 corresponds to a so-called reproducing operation.

[0040] (C-4) Step S4 reads the teaching data X _i related teaching point P _i is performed. Then, based on the read drive amounts of the respective arms, the controller 51 determines the drive mechanisms 531 to 5
The drive signals D _{1 to} D ₅ are issued to 35. as a result,
The tip of the torch 40 (point N) are moved to the teaching point P _i.

[0041] (C-5) interpolates between the teaching points P _{i + 1} adjacent to the step S5 teaching point P _i and the point P _i, data X _ij interpolated point P _ij [position (x _ij, y _ij , z _ij ) and posture (θ _ij , φ _ij )] are calculated. As such an interpolation method, as is well known, there are a linear interpolation method, a method of performing interpolation using a quadratic curve, and the like.
A description of these interpolation methods will be omitted. The data X _ij interpolated point P _ij is the value of X system. Therefore, it is necessary to determine an appropriate drive amount (α system) of each arm for moving the tip of the torch 40 to the interpolation point P _ij .

(C-6) Step S6 The inverse conversion from the X system to the α system is performed.

Before describing the process of the inverse transformation, first, the relationship between the torch 40 and the second arm 22 and the third arm 23 will be geometrically considered. This geometric consideration and its
For the g method based on this, see Japanese Patent No. 275063
Although disclosed in Patent Publication No. 4,
Point will be described in detail. FIG. 18 shows the four points M, N,
FIG. 3 is a three-dimensional diagram showing a positional relationship between P and Q in an XYZ space.

Now, the second arm 22 pivots around the axis A1 at the angle Θ, the third arm 23 rotates around the axis A2 at the angle α, and further rotates around the axis A3 at the angle β. Consider a case. That is, the tip of the torch 40
(X, y, z) and orientation (θ, φ) of the interpolation point
Each arm that can achieve this state when given the information
Of the drive amount (Θ, α, β, x _s , z _s )
I can.

In FIG. 18, when the position N and the posture of the tip of the torch 40 are given, the position of the point Q is automatically determined.

Here, a circle C satisfying the following conditions (i) to (iii) is defined.

(I) The center is the point Q.

(Ii) The radius is d.

(Iii) Exist in a plane whose normal line is the vector QN.

As a result, the point P exists on the circumference of the circle C,
The vector QP is a radius vector of the circle C. Then, as is apparent from the configuration of each axis of the robot RB shown in FIGS.

[0051]

(Equation 2)

[0052]

(Equation 3)

The following relationship is established.

Now, consider a case where a plane S including the vector QN and perpendicular to the XY plane intersects with the circle C.
Are point R and point T, respectively. An angle between the vector QR and the vector QP is defined as an intersection angle δ.

Consider a case where the vector QN and the circle C are projected onto the XY plane. That is, a projection vector of the vector QN onto the XY plane is defined as a vector Q'N ', and an ellipse formed by projecting the circle C onto the XY plane is defined as an ellipse E'.
Each point on the XY plane corresponding to R and T is referred to as a point P ′,
R 'and T'.

Next, the exact solution of the inverse transformation is to be obtained based on the above-described geometric relationship. Here, the following steps are performed in accordance with the flowcharts shown in FIGS. Will be described.

[0057] (C-6-1) Step S61 whether zenith angle theta _ij interpolated points P _ij is in a specific region is determined. At this time, when it is determined that the zenith angle θ _ij is within the specific area, the improved g method described in detail in step S6B is applied. Here, first, the zenith angle θ _ij
Will be described in detail below. In this case, the g method shown in steps S62 to S69 and S6A is applied.

(C-6-2) g method (C-6-2-1) g-cos δ method Step S62 Whether or not the parameter y ₀ is a value in the range of −Δy ₀ to + Δy ₀ (near 0) Value or not). The reason why this determination is necessary will be clear in the following description, but this step itself is significant in determining the format of the g method described later. Note that Δy ₀ is a constant value (a value sufficiently smaller than 1) predetermined by the operator, and the value is determined in consideration of the calculation capacity of the CPU 511 to be used.

First, a general case where the parameter y ₀ is not a value near 0 (steps S63 to S69) will be described in detail. The present applicant calls the control method for such a case the g method or the g-cos δ method.

Since the parameter y ₀ is determined by Equation 49 described later, this step includes a calculation step of the parameter y ₀ as a preceding stage of the judgment (not shown).

Step S63: Virtually turning the second arm 22 around the Z axis
think of. That is, the point at the tip of the second arm 22 is
From the virtual state φ (in the example of FIG.
It is in the opposite direction to the degree. Only)
Therefore, the vector Q in the direction of the axis A4 of the torch 40 is₁N₁of
Projection vector Q on XY plane₁'N₁ ′ Is parallel to the X axis
It has become. A circle corresponding to the circle C in this state
To C₁, Circle C₁Ellipse generated by projecting XY on the XY plane
To the ellipse E₁'. Also, the vector Q₁N₁Including XY
Plane S perpendicular to the plane₁Is a circle C₁The point that intersects with point R₁,
T₁And the circle C₁Upper three points P₁,R ₁, T₁Ellipse
E₁', The corresponding points on point P₁', R₁′,
T₁'.This turning is at the interpolation point N on the workpiece.
Connect the tip of the torch 40 to the axis A4 of the torch 40 outside the path.
(FIG. 2) is a point N included in a plane parallel to the XZ plane. ₁ Moved to
Moving. At this time, the point N ₁ At the corner
Degrees α, β and translation x _s , Z _s Note that is immutable
There is a need to.

As described above, the vector MP is virtually changed to the angle φ.
By turning only, it becomes easy to derive a geometrical relational expression between the point P and the point Q. This will be described below.

FIG. 19 is an explanatory diagram showing a case where the vector Q ₁ N ₁ and the circle C ₁ are projected on the XZ plane. In the figure, the vector Q ₁ ″ N ₁ ″ is the XZ of the vector Q ₁ N ₁
The projection vector onto the plane is represented by a point P ₁ ″, R ₁ ″, T ₁ , where the ellipse E ₁ ″ represents the ellipse resulting from the projection of the circle C ₁ onto the XZ plane.
₁ "indicates a projection point to the point P _1, R _1, T ₁ in the XZ plane. As shown for vector QN 3, the torch 40, i.e. of the orientation of the vector Q ₁ N _1, vector Since the angle between Q ₁ N ₁ and the Z axis is the angle θ, FIG.
In FIG. 9, the angle between the vector Q ₁ ″ N ₁ ″ and the Z axis is also represented by the angle θ. Therefore, the straight line R ₁ "T ₁ "
Is also the angle θ.

Then, returning to FIG. 18, assuming that an intersection line between the plane S ₁ and the plane including the circle C ₁ is an intersection line CL, a component vector in the direction of the intersection line CL of the vector P ₁ Q ₁ (the magnitude is , D
₁ 'cos [delta]) is parallel to the XZ plane, the vector P ₁ Q
₁ in the normal direction of the component vector of the surface S ₁ (magnitude,
d ₁ 'sin δ) is parallel to the Y axis. Therefore, the position coordinates of the point Q ₁ and the point P ₁ are respectively changed to the point Q ₁ (x ₀ , y ₀ , z
₀ ) and a point P ₁ (x, y, z).
In consideration of the fact that the cosine cos δ is negative and the sine sin δ is positive, the relationship between the point P ₁ and the point Q ₁ is expressed by Equations 4 to 6. Accordingly, the axis A4 of the torch 40 is XZ flat.
At position N ₁ paths outside parallel to the plane, the second arm 22
The position coordinates of the intersection P ₁ between the axes of the third arm 23 and
The position coordinates of the point Q of the torch 40 and the intersection angle δ are obtained in advance.
If so, these values will determine it.

[0065]

(Equation 4)

[0066]

(Equation 5)

[0067]

(Equation 6)

Here, a simplified form such as Equations 7 and 8 is used for describing the cosine and sine. These simplified forms are used in the following description.

[0069]

(Equation 7)

[0070]

(Equation 8)

Step S 64 The solution of the quartic equation of cos δ is derived. Such cosδ
Is easily derived from the positional relationship in FIG.

That is, the vector Q ₁ P ₁ and the vector MP
_Since it is orthogonal to ₁ ,

[0073]

(Equation 9)

And

[0075]

(Equation 10)

The following relational expression holds. Therefore, Equation 4 becomes Equation 9
When Equations 6 and 10 are substituted and x ₀ and y ₀ are normalized by d ₁ , Equation 9 becomes

[0077]

[Equation 11]

[0078]

(Equation 12)

[0079]

(Equation 13)

Is transformed into Further, both sides of Equation 11 are squared, and the relational expression after the square is

[0081]

[Equation 14]

By developing using the relationship, a quartic equation relating to Cδ as shown in Expression 15 is obtained.

[0083]

(Equation 15)

Here, a variable X _C , coefficients a, b, c and d
Respectively

[0085]

(Equation 16)

[0086]

[Equation 17]

[0087]

(Equation 18)

[0088]

[Equation 19]

[0089]

(Equation 20)

Is represented by

The quartic equation of Formula 15 can be analytically obtained by a well-known Cartesian method. That is, the solution X _C1
~ X _C4 is given by Equations 21 to 24 shown below. Therefore, if these relational expressions (Equations 21 to 24) are programmed in advance, the controller 5 shown in FIG.
By using 1, exact solutions X _{C1 to} X _C4 can be easily calculated.

[0092]

(Equation 21)

[0093]

(Equation 22)

[0094]

(Equation 23)

[0095]

(Equation 24)

As for the sign of the second term on the right side of Equations 21 to 24, the upper sign is adopted when a ≧ 0, and a <0
, It is promised that the lower sign will be adopted.
The constants U, V, and W are given by coefficients a to d given by Equations 17 to 20, and specific descriptions thereof are omitted here.

Steps S65 to S68 A sine Sδ is obtained based on the solutions X _{C1 to} X _C4 of the cosine Cδ obtained in step S64 (step S65). Then, the intersection angle δ is calculated from these values (Cδ, Sδ) (step S66).

Further, in steps S67 and S68, an angle Θ ′ (see FIG. 18) between the vector OP ₁ ′ and the X axis is obtained, and then the angle Θ ′ (virtual turning angle) is calculated from the angle Θ ′.
(Turning angle) is performed. That is, beyond the torch 40
The end position (theta, phi) required to move to point N ₁
The driving amount of the second arm 22 is the turning angle Θ ′, and Θ
= Θ '+ φ (= known)
In order to determine the angle 先 ず, it is necessary to first determine the turning angle Θ '
is there. By this conversion, the original turning angle の of the second arm 22 is obtained. In order to obtain the angle Θ ′, the point P ₁ ′ (x, y, 0), and therefore the point Q ₁ ′ (x ₀ , y ₀ ,
It is necessary to obtain the position coordinates of 0). The position coordinates of this point Q ₁ ′ are determined from the position of the tip of the torch 40 as described below. Therefore, in the following, step S
Explanation of the principle in 65 to S68 and derivation of various equations required in this step are performed.

Now, let us consider the forward conversion from the α system to the x system. That is, assuming that the position vector of the tip of the torch 40 is represented by the vector p and the posture is represented by (vector n, vector o, vector a), the coordinate transformation matrix T from the α-system to the x-system is represented by Equations 25 and 26. It is.

[0100]

(Equation 25)

[0101]

(Equation 26)

Here, each matrix AΘ, A _zx α, A
β and Aγ are coordinate transformation matrices associated with the degrees of freedom of the respective arms of the robot RB, and are represented by Equations 27 to 30.

[0103]

[Equation 27]

[0104]

[Equation 28]

[0105]

(Equation 29)

[0106]

[Equation 30]

Therefore, by expanding Expression 26 using Expressions 27 to 30, each element of the coordinate transformation matrix T can be obtained. Here, only each element of the vector a and the vector p is shown below.

[0108]

(Equation 31)

[0109]

(Equation 32)

[0110]

[Equation 33]

[0111]

(Equation 34)

[0112]

(Equation 35)

[0113]

[Equation 36]

The forward conversion equation (Equation 3) obtained as described above
From Equations 1 to 36, the solution of the inverse transformation (Θ, x _s , z _s , α,
β) is required.

Therefore, Equations 31 to 33 are replaced by Equations 34 to 36.
Substituting into

[0116]

(37)

[0117]

(38)

[0118]

[Equation 39]

Is obtained. Here, the position vector p
_m is

[0120]

(Equation 40)

[0121]

[Equation 41]

[0122]

(Equation 42)

Is defined. That is, the position vector p
The components p _xm , p _ym , and p _zm of _m give the position coordinates of the point Q in FIG.

Turning to FIG. 18 again, it is understood that the geometric relationship given by Equations 43 to 45 holds.

[0125]

[Equation 43]

[0126]

[Equation 44]

[0127]

[Equation 45]

Here, the angle ψ is an angle between the vector OQ ₁ ′ and the X axis. Further, the position coordinates of the point Q (p _xm ,
p _ym , p _zm )

[0129]

[Equation 46]

Is obtained. Therefore,

[0131]

[Equation 47]

[0132]

[Equation 48]

[0133]

[Equation 49]

[0134]

[Equation 50]

The relational expression given by the above holds, and the position coordinates of the angle ψ and the point Q ₁ (therefore, the point Q ₁ ′) are obtained from the position of the tip of the torch 40.

Here, atan2 ( _pym , _pxm ) is

[0137]

(Equation 51)

[0138]

(Equation 52)

This function uniquely gives the angle ξ.

On the other hand, the intersection angle δ is obtained by the following procedure. That is, the cosine Cδ has already been calculated in Step S64 (Equations 21 to 24), and the sine Sδ is calculated by Equation 11 (Step S65). Therefore, by using the values of x ₀ and y ₀ obtained by Equations 48 and 49, the intersection angle δ becomes

[0141]

(Equation 53)

Is determined (step S66).

Next, the position coordinates Q of the point Q ₁ obtained above
_The angle Θ ′ is determined based on ₁ (x ₀ , y ₀ , z ₀ ) and the intersection angle δ. That is, Equations 48 to 50 are replaced by Equations 4 to 6
Substituting into the position coordinates of the position coordinates P ₁ of the point P ₁ (x, y, z) or the point _{P 1 '(x, y,} 0) is determined. As a result, the angle Θ ′ is determined by Expression 54 (Step S67).

[0144]

(Equation 54)

Then, the vector MP ₁ is changed by the angle −φ to Z.
By turning around the axis, the angle Θ is determined (see FIG. 18). Therefore, the angle Θ

[0146]

[Equation 55]

Is determined (step S68).

(C-6-2-1) g-sin δ Method Step S62 When the parameter y ₀ is a value near 0, the turning angle Θ is obtained by applying the g-sin δ method in step S6A. This is for the following reason.

That is, in the g-cos δ method, first, the cosine C δ is obtained, then the sine S δ is obtained, and then the intersection angle δ is calculated. However, as is apparent from Equation 11, when the parameter y ₀ is a value extremely close to 0, the digit loss becomes large in the calculation process in the CPU 511, and there is a problem that the drive control of each arm becomes difficult. In fact, the value of the parameter y ₀ is 1
In the case of the order of 0 ^-3, the drive error due to the inverse transformation is a number
mm.

The g-sin δ method has been devised as a means for solving such a problem. In this method, the sine S δ is obtained first, and then the cosine C δ is obtained. The angle δ is to be obtained. According to this method, the above-described problem of the digit loss does not occur, and accurate drive control can be performed. That is, according to the g method, theoretically
Even if the parameter y ₀ is near 0, the intersection angle δ must be exactly
Despite trying to find a solution, it happens to be cosine Cδ
Earlier, it was said that data had been calculated electrically on a computer.
Calculation cannot be continued due to data signal processing.
Avoiding the situation and electrically calculating the exact solution that could be originally obtained
Can be obtained with high precision even in the process of (1). Hereinafter, the details of the g-sin δ method will be described based on the steps shown in FIG.

Steps S6A1 to 6A2 Step S6A1 is the same as step S63 in FIG. 7, and therefore detailed description of this step is omitted.

In step S6A2, the solution of the quartic equation relating to the sine Sδ is calculated. 4 for this sine Sδ
The following equation is derived based on the same considerations as in the case of the cosine Cδ. That is, if equation 11 is transformed using equation 14,

[0153]

[Equation 56]

Is obtained. Then, squaring both sides of Equation 56,

[0155]

[Equation 57]

Then, the following relational expression , that is, the point in FIG.
A quartic equation for the sine Sδ of the intersection angle δ at N ₁

[0157]

[Equation 58]

Is obtained. Here, each coefficient a ′, b ′,
c ′ and d ′ are respectively

[0159]

[Equation 59]

[0160]

[Equation 60]

[0161]

[Equation 61]

[0162]

(Equation 62)

Is represented by

The solutions X _{S1 to} X _S4 of Expression 58 are obtained by the Cartesian method, as in Expression 15. That is, Equations 21 to
Similarly, a program that gives the solutions X _{S1 to} X _S4 based on Equation 24 is created, and this program is stored in the memory 512, whereby the solutions X _{S1 to} X _S4 can be obtained by software processing. Moreover, since Equations 59 to 62 do not include the parameter y ₀ in its denominator, the parameter y _{0
Even when 0} is a value close to 0, sufficiently good calculation accuracy can be obtained when calculating the solutions X _{S1 to} X _S4 .

Step S6A3 The value of the cosine Cδ is calculated using the value of the sine Sδ calculated in the previous step. The derivation of the cosine Cδ is obtained from Expressions 11 and 14. That is, at point N ₁ in FIG.
The cosine Cδ of the intersection angle δ of the

[0166]

[Equation 63] And its denominator does not include the parameter y ₀ .
Therefore, it is possible to accurately calculate the cosine Cδ irrespective of the value of the parameter y ₀ . The expression 63 includes (x _o Cθ) in its denominator. However, it has been confirmed by simulation, experiments, and the like that the case where (x _o Cθ) is close to 0 does not occur in the range where the parameter y ₀ is close to ₀ (−Δy ₀ to + Δy ₀ ). Therefore, there is no need to consider (x _o Cθ).
This point holds true for the case where the parameter y ₀ to be described later is near 1 or -1 value near.

[0167] Step S66~S68 exactly electrostatic sine Sδ and cosine Cδ by step S6A
The torch at point N ₁ in FIG.
It is also possible to accurately and electrically calculate the intersection angle δ of 40 . Accordingly, in the g-sin δ method, similarly, after the intersection angle δ is electrically calculated based on Equation 47 (Step S66), the torch is performed in Steps S67 and S68 .
No. 40 required to move the tip of 40 to the point N in FIG.
The turning angle のof the second arm 22 is electrically calculated.

(C-6-3) Improved g Method Step S61 If it is determined that the zenith angle θ is within the specific area, the improved g method of Step S6B is applied. The reason is as follows.

That is, according to the g-cos δ method, the intersection angle δ is calculated by solving a quartic equation (equation 15) of the cosine Cδ.

However, each coefficient a, b,
c and d (the number 17 to number 20) includes a sine S theta in its denominator. Therefore, the zenith angle θ is within the specific area (0 ≦ θ ≦
Δθ, 180−Δθ ≦ θ ≦ 180), all of the coefficients a to d have large values. Moreover, when calculating the solution of Equation 15 (Equations 21 to 24), Equations 64 to 70 are used.
As shown in the square for each coefficient to d, are required complicated calculations cubed, etc. (a ² to d ^2, etc.). The quantities u, v, and w in equations 68 to 70 are the quantities u, v, and w shown in equations 21 to 24, respectively.

[0171]

[Equation 64]

[0172]

[Equation 65]

[0173]

[Equation 66]

[0174]

[Equation 67]

[0175]

[Equation 68]

[0176]

[Equation 69]

[0177]

[Equation 70]

As a result, the zenith angle θ falls within the above specific area.
While maintaining a certain posture, the tip of the torch 40 is moved to the point N in FIG.
The point N is used to determine the driving amount of each arm for moving.
The solution X _{c1 of} Equation 15 for the cosine Cδ of the intersection angle δ at ₁
When trying to find a to X _c4, it arises a problem that in operation overflows the CPU 511. In other words, so that the accuracy of the solution X _c1 to X _c4 is extremely lowered.
In this regard, the same applies to the g-sin δ method.

[0179] Of course, such a problem is because it is closely related to the calculation capacity of the CPU511, it is also possible to suppress the occurrence of 斯or that problem by increasing the calculation capacity. However, there is a need for a flexible solution that does not cause the problem even when the general-purpose CPU 511 is used.

[0180] Therefore, in the present invention, as 斯or that solution, improving g method is applied. Hereinafter, each step in the improved g method will be described with reference to FIGS.

In this embodiment, Δθ that defines the specific area
Is 3 deg. -10 deg. Is selected within the range. Of course, the settable range of Δθ is not limited to the above range. That is, the designer can use the CPU 511
Δθ while taking into account the calculation capacity and the values of the quantities x ₀ and y ₀ , etc.
Can be arbitrarily determined.

Step S6B1: It is determined whether or not the parameter y ₀ is near 0, near 1 or near −1. That is, when the parameter y ₀ is −
Δy ₀₁ ≦ y ₀ ≦ Δy ₀₁ , 1−Δy ₀₂ ≦ y ₀ ≦ 1 + Δy ₀₂
Or −1−Δy ₀₃ ≦ y ₀ ≦ −1 + Δy ₀₃ (Δy ₀₁ , Δy
₀₂ and Δy ₀₃ are values that are sufficiently smaller than 1). This step also uses the parameter y
Depending on the value of ₀ , the electrical calculation of the intersection angle δ
Have the meanings that selects a mode applicable improvements g method to avoid the drive amount of error occurrence that attributable to a decrease in calculation accuracy caused by are. Note that Δy ₀₁ , Δy ₀₂ , and Δy ₀₃ are also arbitrarily determined in consideration of the calculation capacity of the CPU 511.

That is, when it is determined that the parameter y ₀ does not exist within the above range, the improved g-cos δ method of step S6B2 is applied. However, if the parameter y ₀ is determined to be within the above range, the improved g-cos
In the δ method, since the calculation accuracy decreases due to the value of the parameter y ₀ , the improved g-sin δ in step S6B3 is applied. By applying the g-sin δ method, the zenith angle θ increases.
When specified in the value in the specific area, and g
18 is the y coordinate value of the virtual point N _{1 in} FIG.
Even if the value of the parameter y ₀ falls within the above range,
Without causing digit loss on the computer side,
Sufficient computational capacity, exactly and precisely, put the point N ₁
Can be obtained. Hereinafter, the modified g-cos δ method in step S6B2 will be described.

Improved g-cos δ Method FIG. 11 is a flowchart showing a main part of the improved g-cos δ method, which will be described with reference to FIG.

-1 Step S6B21 The first arm 21 is turned virtually by −φ around the Z axis. This step itself is the same as step S63.

-2 Step S6B22 The solution of the quartic equation of (cos δ) ^-1 is calculated. That is, in certain regions, to derive the quartic equation for the inverse of cosδ a variable, by solving the quartic equation, calculation processing over electronic computing machine if the value of the zenith angle θ is Ru Ah in a specific area
The above problems arising during the process are overcome. Such a quartic equation is

[0187]

[Equation 71] Is obtained by modifying equation 15 for a new variable X _c ′ represented by In other words, in the N ₁ point to be derived
The quartic equation for the intersection angle δ is

[0188]

[Equation 72] Given as Here, the coefficients A to D are respectively

[0189]

[Equation 73]

[0190]

[Equation 74]

[0191]

[Equation 75]

[0192]

[Equation 76] It is expressed as

As is clear from Equations 73 to 76, each coefficient A
The denominator of ＤD does not include the sine S θ . Therefore, even if the zenith angle θ is a value in the specific area, the operation of the CPU 511 does not overflow. That is, when the zenith angle θ is 0 deg. Or 180 deg. It is possible to calculate the solution X _c ′ and the solution X _c accurately even in the case where the values are as close as possible.

The same method (Cartesian method) as in Equation 15 is applied to the calculation of the solution X _c ′ in Equation 72.
That is, the solution X _c ′ is calculated by software processing in the CPU 511 determined based on the relational expressions of Expressions 21 to 24.

[0195] In -3 step S6B23 this step, based on the solution X _c 'obtained in the step S6B22, the value of <br/> cosine Cδ point in crossing angle N ₁ [delta] in Figure 18 are determined. Such a cosine Cδ is calculated by electrical processing based on Equation 77.

[0196]

[Equation 77]

-4 Step S6B24 Similarly, in the improved g method, using the value of cosine Cδ,
The sine Sδ is calculated from 1.

Improved g-sin δ Method When the parameter y ₀ is a value close to 0, the sine S δ cannot be calculated accurately as is clear from Equation 11.
Also, when the parameter y ₀ is a value close to 1 or a value close to −1, the cosine Cδ cannot be calculated accurately, as is clear from Equations 73 to 76. Therefore, in this case,
By applying an improved g- sin [delta method, zenith angle θ is especially
The above problem caused by being within the constant region and the parameter y
_The present invention solves the above-mentioned problem caused by ₀ being close to ₀ , 1 or -1 at the same time. Hereinafter, the main part of the improved g-sin δ method will be described with reference to FIG.

As shown in the figure, the solution of the quartic equation relating to the reciprocal of the sine Sδ is obtained based on the assumption of the virtual turning of the first arm (step S6B31) (step S6).
B32). This quartic equation is derived by modifying the quartic equation (Equation 58) for the sine Sδ using the relational expression expressed by Equation 78. That is, a quartic equation relating to the reciprocal of the sine Sδ is given by Expression 79.

[0200]

[Equation 78]

[0201]

[Expression 79]

Here, each of the coefficients A ′ to D ′ is given by
0 to the number 83.

[0203]

[Equation 80]

[0204]

[Equation 81]

[0205]

(Equation 82)

[0206]

[Equation 83]

The Cartesian method can be similarly applied to this quartic equation, and the solutions X _S1 ′ to X _S4 ′ can be electrically obtained by software processing. Moreover,
Since each of the coefficients A ′ to D ′ does not include the parameter y ₀ in its denominator, the software processing is not affected by the value of the parameter y ₀ .

Next, in step S6B33, the value of the sine Sδ to be originally obtained is electrically calculated based on the equation (84).

[0209]

[Equation 84]

[0210] Furthermore, in step S6B34, the value of the cosine Cδ based on the number 63 is electrically calculated, similarly, the step also is not affected by the value of the parameter y _0.

[0211] The step S66~S68 improved g- cos [delta] method or improved g- sin [delta method of FIG. 18
After the values of the cosine Cδ and the sine Sδ of the intersection angle δ of the torch 40 at the point N ₁ have been obtained by the electrical calculation process , the intersection angle δ is similarly calculated electrically (step S66).
Further, the turning angle Θ is electrically calculated (steps S67 to S67).
S68).

(C-6-4) Step S69 i) Since the exact solution of the turning angle Θ has been obtained, the exact solution is substituted into the coordinate transformation formulas (Formula 37 to Formula 39) to obtain
Translation amount in the XY plane of the second arm 22 x _s and axis A ₂
The surrounding rotation angle α is determined. These drive amounts x _s ,
The electrical calculation of α is performed as follows.

That is, since p _zm = z ₀ , from _Equations 39 and 50,

[0214]

[Equation 85]

And the expression 6 is

[0216]

[Equation 86]

Is represented by Therefore, Equation 85 and Equation 86
Than

[0218]

[Equation 87]

The following is obtained.

On the other hand, from Expressions 37 and 38, the degree of freedom of translation x
_s

[0221]

[Equation 88]

Is obtained by By substituting Equation 88 into Equation 37,

[0223]

[Equation 89]

Is obtained. Therefore, the angle α is determined by Expression 90 derived from Expression 87 and Expression 89.

[0225]

[Equation 90]

Ii) Next, the degree of freedom of translation z _{s of} the second arm 22 in the Z-axis direction and the rotation angle β about the axis A3 are determined. That is, the translational degree of freedom z _s is determined by Expression 50 and Expression 85, and the rotation angle β is determined by Expression 91 or Expression 92 derived from Expressions 31 to 33.

[0227]

[Equation 91]

[0228]

(Equation 92)

By the above steps, the solution of the inverse transformation, that is, the drive amounts Θ, x _s , z _s , α, and β of each arm are all strictly determined, and the step S6 by the g-cos δ method is finish.

(C-7) Step S7 The drive amounts Θ, x _s , and _{s of} each arm determined in step S6
z _s , α, β are driving signals D ₁ , D ₂ , D ₃ ,
It is converted to D ₄ and D ₅ . This conversion processing is performed by the CPU 51
1 is performed. Then, each of the drive signals D _{1 to} D ₅ is
The data is transmitted to each drive mechanism 531 to 535 via the data bus 57.

On the other hand, each of the drive mechanisms 531 to 535 drives each arm by an amount corresponding to each of the drive signals D _{1 to} D ₅ . As a result, the tip of the torch 40 moves to the interpolation point P _ij obtained by the calculation in step S5. Torch 40 at that time
Of course, the position and orientation of the tip of exactly match the interpolation data X _{ij at the} interpolation point P _ij . Therefore, the torch 40
Beam LB emitted toward the weld from the tip of
Moves accurately along the work line between the preceding interpolation point P _ij-1 and the target interpolation point P _ij .

(C-8) Step S8 In this step, it is determined whether or not the interpolation point _Pij is the last interpolation point. The determination is made by the controller 51 by software. When it is determined that the interpolation point P _ij is not the last interpolation point, the process returns to step S5,
The interpolation data X _{ij + 1 of the} next interpolation point P _{ij +1} is calculated, and a series of steps S6 to S8 are performed in the same manner.

On the other hand, when it is determined that the interpolation point P _ij is the last interpolation point, the process proceeds to step S9.

[0234] In (C-9) Step S9 present step, teaching points P _i whether the last teaching point is determined. This determination is also made by the controller 51. And the teaching point P _i
There when it is determined that not the last teaching point returns to step S4, the teaching data X _{i + 1} of the next teaching point P _{i + 1} is read, a series of steps S5~
S9 is performed again.

On the other hand, when it is determined that the teaching point P _i is the last teaching point, all the operations of the laser welding robot RB are completed.

As is clear from the above description, the points of this embodiment are understood as follows. That is, the concept of a geometrically assumed angle δ (intersection angle) is introduced, and the exact solution of the quartic equation of the cosine Cδ is processed by software outside the specific region. However, depending on the value of the parameter y _0, the cosine Cδ
, The sine Sδ is selected as a variable, and the exact solution of the quartic equation of the sine Sδ is processed by software. On the other hand, in the specific region, the exact solution of the quartic equation of the reciprocal of the cosine Cδ is processed by software. However, depending on the value of the parameter y ₀ , similarly, the reciprocal of the sine Sδ is selected as a variable, and the exact solution of the quartic equation of the reciprocal of the sine Sδ is processed by software. With these methods, the inverse transformation can be performed strictly and easily without approximation for all the moving points of the torch.

As a reference, FIGS. 20 (g method) and 21 (improved g method) show the flow of the inverse transformation algorithm based on the analysis described above. In both figures, the numbers in parentheses indicate the numbers of the mathematical expressions used in the above description. The mark (*) indicates the amount of the X system immediately obtained from the teaching data, and the mark (**) indicates the amount of the α system to be obtained by the inverse transformation.

D. Modifications (D-1) Part 1 In the above laser welding robot RB, basically, by calculating the solution of a quartic equation relating to the cosine Cδ outside the specific region (g-cosδ method), Cosine C
By calculating the solution of the quartic equation relating to the reciprocal of δ (improved g-cos δ method), the drive signals D _{1 to} D _{5 of} each arm are calculated.
Was decided. Where the parameter y ₀ is a value near 0,
In the case of a value near 1 or a value near −1, the g-sin δ method and the improved g-sin δ method have been selectively used.

However, it is also possible to develop the g method and the improved g method based on the sine Sδ. That is, outside the specific region, the drive signal D _{1 is obtained} from the solution of the quartic equation relating to the sine Sδ
Determines to D _5, it is possible to determine the driving signals D ₁ to D ₅ from the solution of 4-order equation for the inverse of the sine Sδ specific region. In this case, there is an advantage that it is not necessary to consider the value of the parameter y ₀ , and it is sufficient to consider whether the zenith angle θ is within the specific region. Note that the specific area in this case is also 0 deg. ≦ θ ≦ (0 deg. + Δθ), (180 deg.
−Δθ) ≦ θ ≦ 180 deg.

FIGS. 13 and 14 show a control method using the g method based on the sine Sδ and the improved g method.
However, the difference between this control method and the control methods shown in FIGS. 5 and 6 is that the step of inverse conversion from X system to α system (S
Only 6) is included. Therefore, FIG. 13 and FIG.
Shows only steps corresponding to step S6 in FIG.

The electrical and mechanical configuration of this modification is also the same as that of the laser welding robot RB.

Step S6C1 It is determined whether or not the zenith angle θ is within the specific area.

Steps S6C2 to S6C3 If it is determined that the zenith angle θ is within the specific area, g
The sin sigma method is applied and the sine S δ is first calculated.

Step S6C4 In the specific area, the improved g-sin δ method is applied. The details of the improved g-sin δ method are shown in FIG. 15, and the contents are as described above.

Steps S6C5 to S6C9 The value of the cosine Cδ is calculated from the value of the sine Sδ calculated by the g-sin δ method or the improved g-sin δ method. Furthermore,
The intersection angle δ is calculated. After that, each drive amount is calculated, including the turning angle Θ, and these software processes are also as described above.

As described above, in this modification, by calculating the reciprocal of the sine Sδ inside the specific area, and calculating the sine Sδ outside the specific area, all the points P _i ,
Each driving amount can be accurately determined at the point P _ij .

(D-2) Part 2 In each of the above-described embodiments, the improved g method is applied only when it is determined that the point to which the torch should be moved is within the specific area. However, the improved g method can be applied to a case where the torch should be moved not only within the specific area but also outside the specific area.

When Applying the Improved g-sin δ Method The improved g-sin δ method is applied over the entire range of the zenith angle θ. In this case, unless the zenith angle θ is an angle near to satisfy the case and Cθ ₃ ² = x ₀ ² becomes 90 ° vicinity, so that there is no concept of specific regions already described. In this regard, each of the coefficients A to
This can be understood by referring to the denominator of D.

An example of such step S6D is as follows.
It is a flowchart of FIG. FIG. 16 also shows only steps corresponding to step S6 in FIG. 5, and this modification is different from steps S6 to S61 and S61 in FIG.
This corresponds to a control method excluding 62. In this modification,
It is not necessary to selectively use the g method and the improved g method depending on the value of the zenith angle θ (except for the above exception), and the parameter y _0.
It is not necessary to consider the value of, so that there is an effect that the procedure for determining the drive amount can be simplified.

A case where the improved g-cos δ method is mainly applied A case where the improved g-cos δ method is mainly applied is shown in step S6E of FIG. Even in this modification, there is no concept of the specific region described above, and it is not necessary to use the g method and the improved g method depending on the value of the zenith angle θ.

However, the improved g by the value of the parameter y ₀
It is necessary to use the -cos δ method and the improved g-sin δ method properly, and this modification is a combined type of the improved g-sin δ method and the improved g-cos δ method. That is, if the parameter y ₀ is a value close to 0 or 1
If the value is a neighborhood value or -1 neighborhood value, the improved g-sin
The δ method is applied (step S6E2), and as long as the parameter y ₀ is out of the above range, the intersection angle δ is calculated by applying the improved g-cos δ method (step S6E2).
6E3).

(D-3) Part 3 In the specific region, the improved g-cos δ method is basically used.
Outside the specific region, the g-sin δ method can be basically used.

On the contrary, within the specific region, the improved g-sin δ
The g-cos δ method can be basically used outside the specific region.

FIG. 22 shows a combination of the improved g-methods and the combination of the improved g-methods used in the above-described control method. For any of these combinations,
For all postures and in all positions, the computer
Without any loss of digits in the electrical calculation process
Determine the intersection angle δ with high accuracy
The solution can be determined. In particular, in FIG. Second and third
No. 3 必 The method considers the value of the parameter y ₀
There is an advantage that it is unnecessary. 3 and No. 3 4
Method does not matter whether the zenith angle θ is in a specific area
The advantage that each arm can always be controlled using the improved g method
There is. Incidentally, in the figure 22, No. 1 is FIGS. 7 to 1
The method shown in FIG. No. 2 is a modified example (D-1). 3 and No. 3 4 are modifications (D-
2). No. 5-No. 6 shows a modified example (D-3).

(D-4) Part 4 In the above embodiment, the torch 40 on the XY plane is used.
Was assumed to be parallel to the X axis. However, when the X axis and the Y axis are interchanged and defined, a virtual state in which the projection is parallel to the Y axis may be assumed.

As understood from FIGS. 20 and 21, the virtual φ rotation only appears in the middle of the calculation process, and finally, the actual state in which the virtual φ rotation is not performed. Are obtained by the CPU in the controller. Therefore, by combining the above equations to form an equation in which the amount (for example, the virtual turning angle Θ ′) that appears corresponding to the virtual φ rotation is eliminated, the virtual φ rotation is included in the equation. The quantity for will not appear. That is, in this case, an arithmetic expression that does not perform the virtual φ rotation is obtained as an intermediate process, and it is possible to obtain each value of the α system based on such an arithmetic expression. That is, such modifications are also included in the scope of the present invention. Moreover, the angle δ is a scalar quantity and is not affected by the rotation of the coordinate axes. Therefore, even in such a case, the angle δ can be determined from the solution of the quartic equation for Cδ or Sδ, or the solution of the quartic equation for the reciprocal of Cδ or Sδ.

In the above embodiment, the value of the intersection angle δ is obtained after obtaining the cosine Cδ and the sine Sδ. However, the value of the intersection angle δ may be directly obtained from the cosine Cδ or the sine Sδ.

(D-5) No. 5 The present invention is not limited to the laser welding robot RB described above, and can be applied to any cylindrical coordinate robot having a configuration equivalent to the coordinate system shown in FIG. That is, the present control method can be applied to a cylindrical coordinate robot having a new XYZ coordinate system obtained by performing coordinate conversion on the XYZ coordinate system shown in FIG.

FIG. 23 and FIG. 24 show an example of such a laser welding robot RB1.
Here, FIG. 24 is a schematic diagram showing the α system of the laser welding robot RB1, and corresponds to a side view of the laser welding robot RB1 shown in FIG. 23 viewed from the direction of the arrow AR in the same figure. An overview of the configuration of the robot RB1 is as follows.

A first arm 21 'is installed vertically on the robot installation surface 10' parallel to the XY plane, and the first arm 21 'receives a driving force of a driving mechanism (not shown) of the first arm 21' itself. The one arm 21 ′ translates in the X direction along the guide 12 (first axis, drive amount x _s ). The bellows mechanism 13 is provided on the first arm 21 '.

The second arm 22 'attached to one surface of the first arm 21' receives a driving force of a driving mechanism (not shown) of the second arm 22 'and translates in the Y direction (third axis). , Drive amount y _s ), and YZ around the point M ′ as the turning center.
Swivels in a plane (second axis, swivel angle Θ). Therefore,
It can be said that the second arm 22 'has a degree of freedom of rotation in the YZ plane directly with respect to the first arm 21'.

Further, the third arm 23 'has a rotational motion about the fourth axis and the fifth axis (rotation angles α and β).

From the above configuration, it is understood that the robot RB1 corresponds to a configuration in which the robot RB in FIG. 1 is rotated by 90 degrees around the X axis in FIG. Such a coordinate transformation is shown in FIG. FIG. 25A shows the XYZ of FIG.
The rotation around the X axis of the coordinate system is shown, and FIG. 4B shows a case where the rotated coordinate system is applied as the Cartesian coordinate system of the robot RB1. As shown in this figure,
The X, Y, and Z axes of the robot RB1 are respectively shown in FIG.
Correspond to the Z axis, X axis and Y axis. Therefore, the turning angle の of the robot RB1 corresponds to the turning angle の in FIG.
When the g method or the improved g method is applied to the robot RB1, the second arm 22 'is virtually turned around the X axis. This is a point different from the robot RB in FIG.

FIGS. 27 and 28 show an example of an inverse conversion method in the robot RB1. This step S6F corresponds to step S6 in FIGS.
It should be noted that, by using the methods (programs) shown in FIGS. 7 and 8 as they are, first, drive amounts adapted to FIGS. 1 and 2 are determined, and then these drive amounts are determined according to the coordinate transformation shown in FIG. by converting the amount of drive adapted to the robot _{RB1 (x s, y s,} Θ, α, β) can also be determined. In this case, there is no need to construct a new program every time the axis configuration of the robot changes, and there is an advantage that it is sufficient to add a simple coordinate conversion program to one program.

Further, the torch 4 of the robot RB1
An attitude angle of 0 is shown in FIG. 26 for reference.

[0266]

【The invention's effect】

(1) According to the first aspect of the invention, the sine or the cosine of the angle δ is solved to obtain a quartic equation, and the turning angle Θ is strictly obtained from the positional relationship between the angle δ and the turning angle Θ. By using the value of Θ, the drive amount of each of the remaining arms can also be sequentially strictly obtained. That is, the present invention has an effect that the end effector tip can be accurately positioned without error.

According to the second aspect of the present invention, the turning angle Θ can be strictly determined according to the posture of the tip of the end effector, and the drive amounts of the remaining arms can also be strictly determined sequentially. Therefore, the present invention can also accurately position the end effector tip without errors.

(2) According to the first and second aspects of the present invention, since the form of the solution of the quartic equation that holds for the angle δ is determined, the steps for solving the quaternary equation are systematically performed. Can be processed. Therefore, there is also an effect that the drive control of each arm can be performed in a short time.

(3) There are the following effects unique to the embodiment. That is, a virtual state is introduced in which the first arm is turned around the Z axis by an angle φ so that the projection of the end effector on the XY plane is parallel to the X axis. Quadratic equation of sine or cosine and angle δ
Sine or cosine can be simplified into a simple form, and the exact value of the turning angle Θ can be easily determined.

[Brief description of the drawings]

FIG. 1 is a perspective view showing a mechanical configuration of a laser welding robot according to an embodiment of the present invention.

FIG. 2 is a schematic side view showing a mechanical configuration of a laser welding robot according to one embodiment of the present invention.

FIG. 3 is an explanatory diagram showing a position and a posture of a tip of a torch.

FIG. 4 is a block diagram illustrating an electrical configuration of a control system of the laser welding robot.

FIG. 5 is a flowchart showing steps for drive-controlling the laser welding robot.

FIG. 6 is a flowchart showing steps for controlling the drive of the laser welding robot.

FIG. 7 is a flowchart illustrating in detail a step of inverse conversion from an X system to an α system.

FIG. 8 is a flowchart illustrating in detail a step of inverse conversion from an X system to an α system.

FIG. 9 is a flowchart describing in detail the steps of the g-sin δ method in the inverse transform.

FIG. 10 is a flowchart illustrating steps of an improved g method in the inverse transform.

FIG. 11 is a flowchart describing in detail the steps of the improved g-cos δ method in the inverse transform.

FIG. 12 is a flowchart describing in detail the steps of the improved g-sin δ method in the inverse transform.

FIG. 13 is a flowchart for explaining in detail the steps of the inverse conversion in another embodiment of the present invention.

FIG. 14 is a flowchart illustrating in detail the steps of the inverse conversion according to another embodiment of the present invention.

FIG. 15 shows an improved g-sin according to another embodiment of the present invention.
6 is a flowchart illustrating the steps of the δ method in detail.

FIG. 16 is a flowchart illustrating in detail a step of inverse conversion according to still another embodiment of the present invention.

FIG. 17 is a flowchart illustrating in detail a step of inverse conversion according to still another embodiment of the present invention.

FIG. 18 is a three-dimensional view for clarifying a positional relationship.

FIG. 19 is an explanatory diagram showing projection onto an XZ plane.

FIG. 20 is an explanatory diagram showing a flow of an inverse transformation algorithm in the g method.

FIG. 21 is an explanatory diagram showing a flow of an inverse conversion algorithm in the improved g method.

FIG. 22 is an explanatory diagram showing a combination of the g method and the improved g method and a combination of the improved g methods.

FIG. 23 is a perspective view showing a mechanical configuration of a laser welding robot according to still another embodiment of the present invention.

FIG. 24 is an explanatory view showing an α system of a laser welding robot according to still another embodiment of the present invention.

FIG. 25 is an explanatory diagram showing coordinate conversion.

FIG. 26 is an explanatory diagram showing a posture angle of the torch 40.

FIG. 27 is a flowchart illustrating in detail the inverse conversion step of the laser welding robot according to still another embodiment of the present invention.

FIG. 28 is a flowchart illustrating in detail the inverse conversion step of the laser welding robot according to still another embodiment of the present invention.

[Explanation of symbols]

 RB laser welding robot 10 robot installation surface 21 first arm 22 second arm 23 third arm 31 tip of second arm 32 tip of third arm 40 torch A1 axis of first arm A2 A3 axis of the second arm A3 axis of the third arm A4 axis of the torch

Continued on the front page (72) Inventor Toshihide Kuroda 6-107, Takino-cho, Nishinomiya-shi, Hyogo Shinmei Wako Industry Co., Ltd. No. Shinmeiwa Industries Co., Ltd.Development Technology Division (72) Inventor Masuo Kasai No.6, 107, Takino-cho, Nishinomiya-shi, Hyogo Prefecture Shinmeiwa Industry Co., Ltd. 6-107 Takino-cho Shin-Meiwa Industrial Co., Ltd. Development Technology Division (72) Inventor Ryuhei Mitou 1-1, Shin-Meiwa-cho, Takarazuka-shi, Hyogo Shin-Meiwa Industrial Co., Ltd. Industrial Machinery Division (58) Field (Int.Cl. ⁶ , DB name) G05B 19/4093 B25J 9/04 B25J 9/10 G05B 19/18 G05B 19/4155

Claims (2)

(57) [Claims] A first arm installed substantially perpendicular to the robot installation surface; a first arm coupled to the first arm and having a degree of freedom of translation in a plane substantially parallel to the robot installation surface;
A second arm having a degree of freedom of rotation directly or indirectly in a predetermined plane defined with respect to the first arm;
A third arm coupled substantially at a right angle to the distal end of the arm and having a degree of freedom of rotation about the axis of the second arm and a degree of freedom of rotation about its own axis; A drive control method for a cylindrical coordinate robot having an end effector coupled at right angles, comprising: (a) designating a position and a posture of a tip of the end effector; and (b) including an axis of the end effector and The reciprocal of the cosine of the angle between the intersection line when the plane perpendicular to the predetermined plane intersects with the plane having the axial direction of the end effector as the normal direction and the axial direction of the third arm, or the sine of the angle (C) calculating a solution of a quartic equation with the reciprocal selected in step (b) as a variable; and (d) solving a solution of the quartic equation. Ri determining a value of said angle corresponding to the position and orientation, (e) the first using the value of angle, the second and third
Determining each drive amount of the arm; and (f) providing a drive output corresponding to the drive amount to each of the first, second, and third arm drive mechanisms, respectively. A driving method for a cylindrical coordinate robot, comprising the steps of driving three arms. A first arm installed substantially perpendicular to the robot installation surface; a first arm coupled to the first arm and having a degree of freedom of translation in a plane substantially parallel to the robot installation surface;
A second arm having a degree of freedom of rotation directly or indirectly in a predetermined plane defined with respect to the first arm;
A third arm coupled substantially at a right angle to the distal end of the arm and having a degree of freedom of rotation about the axis of the second arm and a degree of freedom of rotation about its own axis; A drive control method for a cylindrical coordinate robot having an end effector coupled at a right angle, comprising: (a) specifying a plurality of positions and postures of a tip of the end effector; and (b) specifying the posture in which the posture is predetermined. (C) when the posture is within the specific region, a plane that includes an axis of the end effector and is perpendicular to the predetermined plane is a plane of the end effector; A reciprocal of a cosine of an angle formed by an intersecting line and an axial direction of the third arm when intersecting with a plane having an axial direction as a normal direction, or a reciprocal of a sine of the angle, is selected. Choice Calculating a solution of a quartic equation using the obtained reciprocal as a variable, and obtaining values of the angles corresponding to the positions and postures from the solution of the quaternary equations; and (d) the posture is outside the specific area. In the case of, one of the cosine of the angle or the sine of the angle is selected as a variable, a solution of a quartic equation for the variable is calculated, and the position and the position are calculated from the solution of the quaternary equation. Obtaining the value of the angle corresponding to the posture; and (e) using the value of the angle, the first, second and third values.
Determining each drive amount of the arm; and (f) providing a drive output corresponding to the drive amount to each of the first, second, and third arm drive mechanisms, respectively. A driving method for a cylindrical coordinate robot, comprising the steps of driving three arms.