JPH11198072A - Minimum time speed controller for robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To shorten the acting time of a robot remarkably as compared with that by existent systems, in particular when the acting time is long, by maximizing the interpolation speed irrespective of the position and attitude of the robot. SOLUTION: The controller for an articulated robot provided with at least two arms has an interpolator means 1 for interpolating a target robot hand position with a straight line, circular arc or the like, an inverse coordinate means 2 for calculating the target value of each joint from the target robot hand value, a selector means 4 for selecting one that should have the maximum speed among the robot joint 3 axes, and (a differencing means 3, inverse Jacobian matrix calculator means 5 and minimum time rate calculator means 6) all for processing an inverse Jacobian matrix on the basis of a specified robot position to maximize the specified interpolation rate with the maximum possible speed of an actuator.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a minimum time speed control apparatus for controlling an articulated robot having at least two or more arms, which shortens the operation time of the robot during an interpolation operation of a straight line, a circular arc, or the like.

[0002]

2. Description of the Related Art Generally, the maximum interpolation speed of an industrial robot is designed so as to satisfy the application work. However, like handling and balletizing,
When evaluating not work speed but work time, extend the arm length so that the maximum speed can be increased as much as possible, use point-to-point control only when the trajectory does not matter, devise a teaching method, etc. In this way, the work time has been shortened. In the case of point-to-point control, there is no trajectory compensation, but acceleration / deceleration control and speed control are performed so that the speed of each axis can be as high as possible.
Normally, the operation time is shorter than that of the CP control that performs interpolation [this is referred to as “conventional example 1”].

Further, as a second conventional example, there is JP-A-8-263128. This while maintaining the path accuracy of the robot has to perform the high-speed acceleration and deceleration, so as to perform a linear operation between the points P ₁ to P _2, is moved to the point P ₁ a robot with linear motion,
The processing of the acceleration / deceleration control is started, a linear trajectory between points P _{1 and} P ₂ is generated, the attitude of each axis at point P ₁ is calculated, and the speed V that can be generated in the path direction from the maximum speed of each axis and the Jacobian. calculate the _real without exceeding the teachings speed, maximum torque of each axis at the point P _1, the moment of inertia, the external force is computed maximum acceleration of each axis, calculate the time T and the distance d required for acceleration also performs the point P ₂ of these processes, set the acceleration and deceleration intervals from the time T and the distance d, to determine the acceleration completion / deceleration start position, the start / deceleration start position and the acceleration end position / end between the equal It is described that interpolation is performed on a teaching trajectory so that acceleration movement is performed, and a movement command representing the created trajectory is output to a servo control system.

[0004]

However, when there is a restriction on the operation route in the conventional example 1, the CP control is performed to guarantee the trajectory, but there is a problem that the operation time becomes long. The reason for this is that in the case of CP control, it is necessary to set the magnitude of the interpolation speed, and it is fixed at a minimum minimum interpolation speed so that the set interpolation speed can be guaranteed at any position and orientation. It is because. That is, as shown in FIG. 5, comparing the case where the hand position is far from the rotation axis and the case where it is close to the rotation axis, the tangential velocity becomes larger at the position far from the rotation axis even at the same rotation angular velocity. In some cases, the maximum speed was reduced nearer to the rotation axis.

Further, the purpose of the prior art 2 is to determine the acceleration / deceleration time, and the method of calculating the hand speed of the robot uses the joint speeds that are reset so as not to exceed the teaching speed. From the scale down), the maximum speed of the hand is determined. At this time, the maximum speed does not exceed the teaching speed, but the control is to maximize the hand speed of the robot and minimize the operation time. This is a technical measure that is not appropriate as such, that is, not suitable for the shortest time control. The present invention provides an apparatus that eliminates all of the problems of Conventional Example 1 above.
When one of the joint velocities is maximized, the other joint velocity is calculated by using the inverse Jacobian matrix from the reset hand velocity so as not to deviate from the teaching path. Sometimes, the object is to provide a device in which the hand speed is increased ignoring the teaching speed and the robot is speed-controlled in the shortest time.

[0006]

According to a first aspect of the present invention, there is provided a control device for an articulated robot having at least two or more arms. Interpolation calculation means for interpolating with the robot, inverse coordinate means for calculating the target value of each joint from the hand target value of the robot, means for selecting the joint axis of the robot to be at the maximum speed, and inverse Jacobian matrix from the command position of the robot Means for performing calculations and maximizing the command interpolation speed by using the maximum speed that can be obtained by the actuator.

According to a second aspect of the present invention, the means for selecting a joint axis of the robot includes a product of a distance from each joint coordinate origin to a hand position, a current joint speed command value, and a maximum speed difference of the joint. The shortest time speed control device for a robot according to claim 1, wherein an axis that maximizes is selected.

Thus, according to the present invention, based on the speed command means described above, regardless of the position and posture of the robot,
Since the interpolation speed is maximized, particularly when the operation time is long, it is possible to obtain a robot shortest-time speed control device capable of greatly shortening the operation time as compared with the conventional example. .

[0009]

DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments of the present invention will be described below with reference to the drawings. In each drawing, the same reference numerals indicate the same or corresponding members. FIG. 1 is a block diagram showing a related circuit configuration of each means in one embodiment of the present invention [the invention of claim 2 of the present invention].

The definitions of the symbols used and described first are determined as follows. The number in parenthesis in the superscript indicates the number of differentiations. For example, r ⁽¹⁾ indicates the first derivative of the hand position vector r. i is the joint axis of the robot, k is the counter of the calculation clock, r is the hand position vector, r ⁽¹⁾ is the hand speed vector, r _s (k) is the hand target position coordinate sequence, R is the coordinate transformation matrix, and J is Jacobi. Matrix, q is the joint position vector, q ⁽¹⁾ is the joint velocity vector, q _ri
⁽¹⁾ the target speed of the i-th joint, q _mri ⁽¹⁾ the maximum speed of the i-th joint, d _i is the distance to the hand position from the coordinate origin of the i-th joint, V _r is the hand target speed vector, p _s (ξ) is an approximate trajectory of the hand target trajectory.

In one embodiment of the minimum speed control device for a robot, first, a predetermined position of the robot is calculated by an interpolation calculating means 1 from a hand position coordinate sequence 11 given in a joint coordinate system defined for each joint of the robot. A robot hand target position coordinate sequence 12 given in the orthogonal coordinate system at the origin is obtained as a data sequence for each predetermined calculation clock. Next, the robot hand target coordinate sequence 12 is used by the inverse coordinate conversion means 2 to obtain the target position 13 of each joint of the robot. The target speed 14 is obtained from the target position 13 by using the difference means 3. Target position 1 of each joint
Using the target speed 3 and the target speed 14, the joint axis selecting means 4 and the inverse Jacobian matrix calculating means 5 of the robot to be set to the maximum speed obtain the joint axis 15 and the inverse Jacobian matrix 16 to be set to the maximum speed, respectively. Finally, using the joint axis 15 and the inverse Jacobian matrix 16 to be the maximum speed, the shortest time speed calculating means 6 obtains the corrected target position 17 of each joint of the robot and the corrected target speed 18 of each joint. I have.

The above is specifically described. FIG. 2 shows position vectors of a start point, a pass point, and an end point of the robot operation,
It is a figure showing a posture frame. In other words, the hand position coordinate sequence 11 represents FIG. 2 itself. These data are
For example, when the robot operator gives the information by remote teaching using a programming pendant or the like, the information is stored in the control device of the robot with the joint angles of the joint axes and the like. The interpolation calculation means 1 performs a geometric calculation so that a desired trajectory passing through the hand position coordinate sequence 11 is obtained. To do this, first, using a transformation matrix R indicating the relationship between the joint angle and the hand position uniquely determined by the structure of the robot, a hand position coordinate sequence 11 expressed by the joint angle is defined by the orthogonality defined by the robot origin. Convert to position / posture vector in coordinate system.

This is shown in equation (1). X in the equation (1)
Y and Z are obtained by dividing a 3 × 3 rotation matrix into 3 × 1 sub-matrices, and P is a transposed vector indicating a coordinate origin position at each joint. The suffix 0 in the upper left indicates the coordinate system defined at the origin of the robot, and the suffix n in the lower right indicates that it is the n-th joint coordinate counting from the origin.
At the same time, equation (1) indicates that the robot is configured with n axes.

[0014]

(Equation 1) Next, coordinate transformation is performed on the three hand position coordinate sequences shown in FIG. 2 using equation (1), and the respective position and orientation vectors are represented by r ₁ , r ₂ , and r ₃ , and are represented by equation (2). . Equations (2) and thereafter are assumed to have no change in posture for easy understanding.

[0015]

(Equation 2)

Here, assuming that the robot is operated at a constant speed without considering the acceleration / deceleration process, the coordinates of the robot hand target position at each predetermined calculation clock are as shown in FIG.
FIG. 3 [(a) shows the case of circular interpolation and (b) shows the case of linear interpolation] is a diagram showing the coordinates of the robot hand target position for each calculation clock. That is, the hand target position coordinate sequence 12 [12 _a1 , 12 _a2 , 1
2 _b1 , 12 _b2 ]. Next, to obtain the target position and target speed for each joint of the robot,
The inverse transformation of the hand target position coordinate sequence 12 is performed. In particular,
By calculating the inverse function of equation (1), a target position 13 for each joint at each predetermined calculation clock is obtained.

When the target position 13 for each joint is determined by the difference means 3, the calculation clock is ΔT, the target position is q _r (k), and the target speed is q _r (k) ⁽¹⁾ . It becomes like (3). Note that k represents a calculation clock counter.

(Equation 3)

Next, regarding the joint axis selecting means 4 and the inverse Jacobian matrix calculating means 5 of the robot to be set to the maximum speed, in this embodiment, since the inverse Jacobian matrix calculating process can be applied to the joint axis selecting means. , The inverse Jacobi matrix calculation means 5 will be described. First, the inverse Jacobian matrix J indicating the relationship between the hand speed and the joint speed of the robot can be obtained as follows in the case of a six-axis robot. Z and P in the formula (4) are the same as those used in the formula (1).

[0019]

(Equation 4)

The inverse Jacobian matrix calculating means 5 calculates the inverse matrix of the equation (4) analytically, or solves the equation (4) numerically at every predetermined calculation clock, and sequentially obtains the inverse matrix. Thus, an inverse Jacobian matrix 16 is obtained. Using the inverse Jacobi matrix J ⁻¹ , the joint velocity q
⁽¹⁾ can be seen as a function of the hand speed r ⁽¹⁾ .

[0021]

(Equation 5)

Further, the joint axis selection means 4 of the robot should be the maximum speed in the present embodiment, as shown in equation (6), the distance d _i from the joint coordinate origin to the hand position, the current The joint speed command value, that is, the axis that maximizes the product of the difference _{Δq i} ⁽¹⁾ between the target speed q _ri ⁽¹⁾ and the maximum speed q _mri ⁽¹⁾ of the joint is determined, and the joint whose maximum speed is to be obtained is obtained. The axis is the joint axis 15.

[0023]

(Equation 6)

Next, the shortest time speed calculating means 6 for obtaining the corrected target position 17 of each joint of the robot and the corrected speed 18 of each joint by using the joint axis 15 and the inverse Jacobian matrix 16 to be the maximum speed will be described. I do. Taking the example of a six-axis robot as an example, Equation (5) can be rewritten as a relational expression between the target speed q _r ⁽¹⁾ to each joint and the hand target vector V _r as follows. However, as in Expression (2), it is assumed that the posture does not change.

[0025]

(Equation 7)

Here, it is considered that the original trajectory is expressed using the shape parameter ξ and the hand target position coordinate sequence 12. Hand target position coordinate sequence as shown in Figure 3 r _s (
k) as shown in equation (8).

[0027]

(Equation 8)

Next, the original trajectory is represented by a quadratic curve as shown in FIG. 4, which is a diagram showing an approximate curve of the target trajectory, using three consecutive hand target position coordinate data strings. Although the operation is fast, it can be reproduced without a problem of approximation accuracy by shortening the calculation clock. The quadratic curve is represented by equation (9) as p _s (ξ). This is a linear expression of the parameter ξ even in the case of a straight line, and can be treated uniformly. _{p s (ξ) = 2 (} ξ-0.5) (ξ-1) · r s (k) -4ξ (ξ-1) · r s (k + 1) + 2ξ (ξ-0.5) · r s (k + 2) ...... ……………… (9) However, 0.0 ≦ （≦ 1.0

[0029] As shown in FIG. 4, the distance from the formula (9) r _s (k) in up to _{r s (k + 2) δ} , when the time is for t,
The hand target speed vector _Vr and the parameter ξ are given by the following equation (1)
0). _{ξ = {| V r | /} δ} t also ................................. (10), the hand target speed vector _{V r, V r = dp s} (ξ) / dt = {dp s ( ξ) / dξ｝ · dξ / dt ………………… (11) By differentiating equation (9) in the parameter _{ξ, dp s (ξ) /} dξ = (4ξ-3) · r s (k) - (8ξ-4) · r s (k + 1) + (4ξ-1) · R _s (k + 2) ···································· (12) From the equation (10), dξ / dt = | V _r | / δ ……………………… ... (13) is obtained. By substituting the equations (12) and (13) into the equation (11) and using the equation (8), the hand target speed vector V
_r is as shown in equation (14).

[0030]

(Equation 9)

The correction target position 17 of each joint of the robot and the correction target speed 18 of each joint are determined by the joint axis 15 to be set to the maximum speed.
_Where i is the maximum velocity of the joint and q _mri ⁽¹⁾ , q _mri ⁽¹⁾ is substituted into the left side of equation (7), and equation (14) is substituted into the velocity vector on the right side of equation (7). Ask. That is, the i-th equation of the equation (7) after the substitution is:

[0032]

(Equation 10) By substituting equation (14) into equation (15), the parameter ξ becomes

[0033]

[Equation 11] And you can ask.

The shortest time speed calculating means 6 calculates the equation (1)
By substituting the parameter た obtained in 6) into equation (9), a corrected position target expressed in the orthogonal coordinate system at the robot origin is obtained, and based on this, the inverse function of equation (1) is calculated. And the correction target position 17 of each joint of the robot. further,
The correction target position 17 is substituted into the equation (3) to obtain the correction target speed 18 of each joint of the robot. Alternatively, by substituting the parameter に into Equation (14), a hand velocity vector is obtained, and the obtained value is substituted into Equation (7), and the corrected target velocity of each joint of the robot other than the joint axis i to be the maximum velocity is obtained. 18 can also be obtained.

So far, attention has been paid only to the hand target position of the robot. However, it is understood that the posture can be easily realized by changing the posture to a function of the parameter ξ.

[0036]

As described above, according to the present invention, in a control device of an articulated robot having at least two or more arms, an interpolation calculating means for interpolating a hand target position by a straight line, a circular arc or the like, a robot hand target Inverse coordinate transformation means for calculating the target value of each joint from the values, means for selecting the joint axis of the robot to be set to the maximum speed, and calculation of the inverse Jacobian matrix from the command position of the robot to determine the maximum speed that the actuator can output And means for maximizing the command interpolation speed by using the robot.More preferably, the means for selecting a joint axis of the robot further includes a distance from each joint coordinate to the hand position,
By selecting the axis that maximizes the product of the current joint speed command value and the maximum speed difference of that joint, the interpolation speed is maximized regardless of the robot's position and posture. In addition, it is possible to obtain a special effect that a shortest-time speed control device for a robot capable of greatly shortening the operation time as compared with the conventional example can be obtained.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a related circuit configuration of each means in an embodiment of the present invention [invention 2 of the present invention].

FIG. 2 is a diagram showing a position vector and a posture frame of a start point, a pass point, and an end point of the operation of the robot according to the present invention.

FIGS. 3A and 3B are diagrams showing robot hand targets for each calculation clock in the present invention, wherein FIG. 3A shows the case of circular interpolation, and FIG. 3B shows the case of linear interpolation.

FIG. 4 is a diagram showing an approximate curve of a target trajectory of a robot according to the present invention.

FIG. 5 is an explanatory diagram showing that the tangential speed changes between large and small even at the same rotation speed when the hand position is far or near from the rotation axis.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 Interpolation calculation means 2 Inverse coordinate conversion means 3 Difference means 4 Joint axis selection means 5 Inverse Jacobi matrix calculation means 6 Minimum time speed calculation means 11 Hand position coordinate sequence 12, 12 _a1 , 12 _a2 , 12 _b1 , 12 _b2 Hand target position Coordinate example 13 Target position of each robot joint 14 Target speed of each robot joint 15 Joint axis to be maximized 16 Inverse Jacobian matrix 17 Corrected target position of each robot joint 18 Corrected target speed of each robot joint

Claims (2)

[Claims] 1. A control device for an articulated robot having at least two or more arms, an interpolation calculating means for interpolating a target position of a hand with a straight line or an arc, and calculating a target value of each joint from a target value of the robot. Means for selecting the joint axis of the robot to be set to the maximum speed, means for calculating the inverse Jacobian matrix from the command position of the robot, and means for maximizing the command interpolation speed using the maximum speed that can be obtained by the actuator A minimum speed control device for a robot, comprising: 2. The joint axis selecting means of the robot selects an axis that maximizes a product of a distance from each joint coordinate origin to a hand position, a current joint speed command value, and a maximum speed difference of the joint. 2. The minimum time speed control device for a robot according to claim 1, wherein: