WO2019215980A1

WO2019215980A1 - Robot control device

Info

Publication number: WO2019215980A1
Application number: PCT/JP2019/005302
Authority: WO
Inventors: 敦実橋本; 中田　広之; 正義岩谷; 良祐山本; 紘義上田
Original assignee: パナソニックＩｐマネジメント株式会社
Priority date: 2018-05-10
Filing date: 2019-02-14
Publication date: 2019-11-14
Also published as: JPWO2019215980A1; JP7126222B2

Abstract

A robot control device 10 for controlling actions of a multi-articulated robot arm 6, is provided with: a kinetic torque calculation unit 21 that calculates kinetic torque of each joint shaft; and a storage unit 20 having a table 24 in which an angle movement amount Δθap, a rotational speed ωap, and a rotational acceleration αap obtained at a time point ta when the rotational acceleration of each joint shaft becomes maximum, are stored in association with an acceleration time Ta. The kinetic torque calculation unit 21 calculates a residual torque τcap and an inertial term torque τap corresponding to the rotational acceleration αap, and corrects the acceleration time Ta when an inertial term torque τapj is larger than an available torque which is the difference between the maximum allowable torque τjmax and the residual torque τcap with respect to a j-axis.

Description

Robot controller

The present invention relates to a robot control apparatus related to robot operation control.

In an articulated robot equipped with multiple sets of joint axes that are rotated by a motor, when the robot arm is moved at high speed between teaching points, an excessive load may be applied to the motor that drives the joint axes due to rapid acceleration / deceleration. is there.

In Patent Document 1, the maximum or minimum acceleration of the reference joint axis is corrected so that the torque obtained using the dynamic model becomes the target torque while keeping the maximum or minimum acceleration arrival time of each joint axis constant. Techniques to do this are disclosed. By doing in this way, vibration of the robot arm at the time of acceleration / deceleration can be suppressed, and a smooth acceleration / deceleration operation can be performed.

JP 2010-269385 A

Incidentally, the torque generated in the joint shaft depends on the output torque of the motor connected to the joint shaft, and is limited by the allowable maximum torque of the motor. That is, when a plurality of joint axes are operated simultaneously, the torque required to operate the joint axes may be insufficient.

For example, if the joint axis is rotated with insufficient torque during acceleration, only the joint axis will be delayed, and the trajectory of the arm tip may follow an unexpected trajectory. That is, the robot may come into contact with surrounding objects. In addition, if the torque required for deceleration is insufficient, an overshoot may occur at the target teaching point. Even in this case, there is a risk of contact with surrounding objects.

Usually, in such cases, the robot arm operation program is corrected. Specifically, it responds by adding a command for adjusting the acceleration / deceleration time or changing the target operation speed.

However, this correction work has to be done by actually operating the operation program and performing cut-and-try, which requires man-hours.

In the configuration shown in Patent Document 1, the maximum or minimum reference joint axis is set so that the torque obtained using the dynamic model becomes the target torque while keeping the maximum or minimum acceleration arrival time of each joint axis constant. The minimum acceleration has been corrected.

However, in this case, although the operation of the robot arm is stable, the tact, which is the operation time of the robot arm, increases.

The present invention has been made in view of such points, and the acceleration / deceleration time is set so that the torque required to operate the joint shaft does not exceed the allowable maximum torque of the motor while suppressing an increase in the tact time of the robot. It is to provide a robot control device having a mechanism for automatically adjusting the angle.

To achieve the above object, a robot control apparatus according to the present invention is a robot control apparatus that controls the operation of a robot arm having a plurality of sets of motors and joint axes that are rotationally driven by the motors. Based on the rotational speed, rotational acceleration, and joint angle, a dynamic torque calculating unit that calculates and outputs the dynamic torque in each joint axis, and each joint at the time when the rotational acceleration of each joint axis becomes maximum and minimum A storage unit having at least a table in which the movement amount and rotation speed of the joint angle on the shaft and the maximum and minimum values of the rotation acceleration are associated with acceleration / deceleration time for accelerating / decelerating each joint axis; The dynamic torque calculation unit calculates dynamic torque corresponding to the maximum value and the minimum value of the rotational acceleration based on the values in the table, and An inertial term torque corresponding to the maximum and minimum values of the rolling acceleration and a residual torque obtained by subtracting the inertial term torque from the dynamic torque are calculated, and at least one of the joint axes, the inertial term When the torque is larger than the available torque that can be used for accelerating / decelerating each joint axis at a predetermined acceleration, the common acceleration / deceleration time in the rotation operation of each joint axis is corrected.

According to this configuration, it is possible to detect in advance the shortage of torque necessary for the operation of each joint axis, correct the acceleration / deceleration time or movement time with a value corresponding to the shortage of torque, and initially set one of these values. By making the value larger than this value, it is possible to control the operation of the robot arm by rotationally driving the joint shaft with a torque within a range that can be output. As a result, the trouble of correcting the teaching of the robot operation program can be saved. In addition, the robot arm is stabilized in order to correct the acceleration / deceleration time based on the dynamic torque at the time when the actual acceleration reaches the maximum or minimum, that is, the time at which the acceleration / deceleration operation on the joint axis is performed most rapidly. Thus, it is possible to suppress an increase in tact, which is the operation time of the robot arm.

As described above, according to the robot control device of the present invention, the acceleration / deceleration time is automatically adjusted so that the torque required for each joint axis when operating the robot arm does not exceed the outputable torque. Therefore, the trouble of correcting the teaching of the robot operation program can be saved. Further, an increase in the tact time of the robot can be suppressed.

FIG. 1 is a functional block diagram of a robot control apparatus according to an embodiment of the present invention. FIG. 2 is a diagram illustrating temporal changes in joint angles, speeds, and accelerations at different joint axes. FIG. 3 is a schematic diagram showing the configuration of the two-axis robot. FIG. 4 is a diagram showing temporal changes in speed, acceleration, and dynamic torque during acceleration on the j-axis. FIG. 5 is a flowchart showing a procedure for correcting the acceleration / deceleration time according to the embodiment of the present invention. FIG. 6A is a diagram showing temporal changes in acceleration, speed, and angular movement when the acceleration / deceleration time is 100 msec. FIG. 6B is a diagram showing temporal changes in acceleration, speed, and angular movement when the acceleration / deceleration time is 150 msec. FIG. 6C is a diagram illustrating temporal changes in acceleration, speed, and angular movement when the acceleration / deceleration time is 200 msec. FIG. 7 is a diagram showing the configuration of the table. FIG. 8 is a block diagram for deriving the amount of angular movement, speed, and acceleration. FIG. 9 is a diagram illustrating temporal changes in joint angle, speed, acceleration, and dynamic torque during acceleration / deceleration on the j-axis.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. The following description of the preferred embodiments is merely exemplary in nature and is in no way intended to limit the invention, its application, or its application.

(Embodiment)
[Function block configuration of robot controller]
FIG. 1 shows a functional block configuration of a robot control apparatus according to the present embodiment, FIG. 2 shows temporal changes in velocity and acceleration on different i-axis and j-axis, and FIG. 3 shows a biaxial robot. The schematic diagram of is shown. In the following description, subscripts i and j are integers, 1 ≦ i, j ≦ n, and i ≠ j, and n is the number of joint axes included in the robot arm 6. In the following description, the rotational speed or rotational acceleration of the i-axis or j-axis that is the rotational axis may be simply referred to as the i-axis or j-axis speed or acceleration.

The robot controller 10 includes an operation program 1, a processing unit 2, a movement command generation unit 3, an acceleration / deceleration processing unit 4, a servo 5, a movement time calculation unit 7, each axis speed calculation unit 8, A deceleration time calculation unit 9, a recording unit 20, a dynamic torque calculation unit 21, a correction processing unit 22, and a calculation unit 23 are provided as functional blocks. The function block shown in FIG. 1 executes software stored in the recording unit 20 or read from the outside on an arithmetic device such as a CPU (Central Processing Unit) (not shown) provided in the robot control device 10. This is realized. The operation program 1 may be included in this software. Further, for convenience of explanation, illustration of a plurality of sets including a joint axis incorporated in the robot arm 6 and a motor connected to the joint axis and rotating the joint axis is omitted.

In the operation program 1, the teaching point position of the robot arm 6, the interpolation form corresponding to the shape of the locus connecting the teaching points, and the target moving speed are recorded. The teaching point position is a work point position in the orthogonal space or a joint angle of the robot arm 6. The target moving speed is a target speed when moving between teaching points in the instructed interpolation form.

As the interpolation mode, CP (continuous path) interpolation that interpolates the work point on the orthogonal space so as to draw a specific shape such as a straight line or an arc, and moves each joint axis at a constant speed, There is PTP (point to point) interpolation regardless of the shape, but hereinafter, a case where PTP interpolation is performed will be described. PTP interpolation is an interpolation mode applied when moving between teaching points in the shortest time. In many cases, one or a plurality of joint axes of the robot arm 6 is operated so as to rotate at the maximum speed.

When the teaching point position recorded in the operation program 1 is a value in the orthogonal space, the processing unit 2 converts the teaching point position into a joint angle of each joint axis of the robot arm 6, and the teaching point position is determined by the robot. In the case of the joint angle of the arm 6, it is converted into a value on the orthogonal space. In the following description, an arbitrary teaching point recorded on the operation program 1 is Pa, and the next teaching point is Pb.

The movement command generation unit 3 generates and outputs a movement command based on the movement amount between adjacent teaching points and the target movement speed. This movement command is a joint angle movement amount per unit time (hereinafter, simply referred to as an angle movement amount). The amount of angular movement of each joint is collectively set as one set, and this is set as one movement command. Therefore, the movement command is a vector quantity. The movement command generation unit 3 outputs a plurality of sets of movement commands corresponding to the movement time described later in time series. A plurality of sets of movement commands output in time series is called a movement command sequence. The movement command sequence output by the movement command generation unit 3 is not subjected to acceleration / deceleration processing described later.

The acceleration / deceleration processing unit 4 performs an acceleration / deceleration process on the movement command sequence from the movement command generation unit 3 and outputs an accelerated / decelerated movement command. The acceleration / deceleration processing unit 4 performs acceleration / deceleration processing on the movement command sequence from the movement command generation unit 3 so that the target movement speed is reached in an acceleration time output by an acceleration / deceleration time calculation unit 9 described later. Specifically, as shown in FIG. 2, the acceleration at each joint axis is changed with time so as to reach a predetermined target moving speed. In addition, the movement command sequence from the movement command generation unit 3 is set so that the deceleration starts from the target movement speed before the teaching point Pb and reaches the teaching point Pb after the deceleration time output by the acceleration / deceleration time calculation unit 9 has elapsed. Decelerate. In the following description, the time-varying waveform of acceleration may be referred to as an acceleration profile, a deceleration profile, or an acceleration / deceleration profile. As shown in FIG. 2, the acceleration / deceleration profile has a shape in which the speed changes in an S shape with respect to time so as not to induce vibration of the robot arm 6. The acceleration / deceleration processing is performed based on the corrected acceleration time or deceleration time output from the calculation unit 23 described later, and a specific acceleration / deceleration processing procedure will be described later.

The servo 5 controls the operation of the robot arm 6 by rotationally driving the motor connected to each joint axis based on the movement command sequence processed by the acceleration / deceleration processing unit 4.

The movement time calculation unit 7 calculates and outputs the time required for the tip of the robot arm 6 to move at the target movement speed without acceleration / deceleration from the teaching point Pa to the teaching point Pb. The required time corresponds to the travel time.

Each axis speed calculation unit 8 calculates and outputs a target speed of each joint axis at the time of PTP interpolation.

The acceleration / deceleration time calculation unit 9 obtains and outputs an acceleration time when accelerating from the teaching point Pa to the target speed and a deceleration time when decelerating from the target speed and reaching the teaching point Pb before the teaching point Pb.

The storage unit 20 stores a later-described table 24 (see FIG. 7) and the operation program 1, and transmits the stored data to the corresponding block in accordance with requests from each unit of the robot control apparatus 10.

The dynamic torque calculation unit 21 calculates and outputs the dynamic torque of each joint axis at a predetermined time based on the data in the table 24.

The correction processing unit 22 uses first and second correction coefficients γa, which will be described later, based on the dynamic torque, the inertia term torque, and the torque other than the inertia term torque at the predetermined time calculated by the dynamic torque calculation unit 21. γb is calculated and output.

The calculation unit 23 multiplies the acceleration time Ta or the deceleration time Tb calculated by the acceleration / deceleration time calculation unit 9 by the first or second correction coefficient γa, γb calculated by the correction processing unit 22, and after correction. Acceleration time Ta ′ or deceleration time Tb ′ is obtained and output to the acceleration / deceleration processing unit 4.

The table 24 is a table in which the actual acceleration that is the maximum or the minimum when each joint axis of the robot arm 6 is driven and the speed and the amount of angular movement at that time are arranged in association with the acceleration / deceleration time.

The acceleration time is, for example, the time from the acceleration at the teaching point Pa until reaching the target movement speed, and the deceleration time is, for example, the time from the start of deceleration before the teaching point Pb until the movement speed becomes zero. Time is referred to as acceleration / deceleration time. Also, for each joint axis, the acceleration time when accelerating to the target speed with the maximum allowable acceleration of each joint axis (sometimes called the shortest acceleration time of each axis) is obtained, Select the one with the largest value and set this as the acceleration time common to all axes. As shown in FIG. 2, all joint axes are accelerated with an acceleration time common to all the axes. At the time of deceleration, the shortest deceleration time for each joint axis is selected with the maximum value, and this is set as the deceleration time common to all axes. Similar to the acceleration, all the joint axes decelerate with the deceleration time common to all the axes (see FIG. 2).

Therefore, the acceleration / deceleration time in the table 24 is an acceleration / deceleration time common to all axes. The configuration and creation procedure of the table 24 will be described later.

[Acceleration / deceleration time correction procedure]
FIG. 4 shows temporal changes in speed, acceleration, and dynamic torque during acceleration on the j-axis. FIG. 4 shows acceleration and torque calculated based on the movement command output from the movement command generation unit 3 corresponding to the j-axis speed. In FIG. 4, the solid line indicates the dynamic torque calculated based on the movement command, and the broken line indicates the dynamic torque actually applied to the j-axis.

As shown in FIG. 4, the dynamic torque increases as the acceleration increases, reaches a maximum value at time t2, and then decreases as the acceleration decreases. Here, assuming that the maximum torque that can be output by the j-axis is the allowable maximum torque τjmax related to the j-axis, the dynamic torque calculated based on the movement command exceeds the allowable maximum torque τjmax in the period from time t1 to time t3. Yes. This means that the torque for driving the joint shaft is insufficient. In such a case, for example, it is necessary to perform adjustment to increase the acceleration time so that the dynamic torque becomes equal to or less than the allowable maximum torque τjmax.

However, in the actual robot control apparatus 10 to which the robot arm 6 is connected, due to a delay in the servo control system or the like, the actual speed change over time (hereinafter referred to as a speed profile) may be referred to rather than a movement command during acceleration / deceleration. ) Often decreases (see the broken line in FIG. 4), and accordingly, the actual dynamic torque indicated by the broken line becomes smaller than the dynamic torque calculated based on the command speed.

Therefore, as shown in FIG. 4, although the dynamic torque does not actually exceed the allowable maximum torque τjmax, it may be determined that correction for increasing the acceleration time is necessary. In this case, since the tact time increases, there is a problem that productivity of work using the robot arm 6 is lowered.

Therefore, in the present embodiment, the dynamic torque is calculated using the angular movement amount and the actual speed when the acceleration is maximized, and the acceleration / deceleration time is corrected based on the dynamic torque. Has been eliminated.

<STEP1: Calculation of acceleration / deceleration time>
FIG. 5 shows a procedure for correcting the acceleration / deceleration time according to this embodiment, and the acceleration / deceleration time is corrected by sequentially executing the five steps described below.

First, the acceleration time Ta and the deceleration time Tb are calculated in the target system (STEP 1).

In the case of PTP interpolation, when the joint angles at teaching points Pa and Pb are θa and θb, respectively, the movement time between work points corresponding to the teaching points is Tm, and the unit time of interpolation is Th, the movement command Δθ is Is represented by Formula (1). The travel time Tm is calculated by the travel time calculation unit 7.

Δθ = (θb−θa) · Th / Tm (1)

Here, it is a vector quantity having the joint angle of each joint axis of θa and θb as elements. Further, Δθ is a vector amount having the angle movement amount of each joint axis as an element.

When the moving distance between the work points corresponding to the teaching points Pa and Pb is L and the target moving speed is V, the moving time Tm is expressed by the equation (2a).

Tm = L / V (2a)

In the case of PTP interpolation, the movement distance L corresponds to a linear distance between work points. Further, when the robot arm 6 is operated by accelerating or decelerating the joint axis and there is a joint axis that exceeds the permissible maximum speed, the equation (2a) is set so that the speed of the joint axis falls within the permissible maximum speed. The travel time Tm calculated from (1) is corrected.

Further, in the PTP interpolation, when moving between work points corresponding to the teaching points Pa and Pb in the shortest time, a value obtained by dividing the movement angle of each joint axis by the maximum speed (the shortest movement time of the axis) The maximum value among them (which may be called) is defined as a movement time Tm.

That is, when the movement angle of the j-axis among the plurality of joint axes is Δθj and the maximum speed is ωjmax, the movement time Tm is expressed by the equation (2b).

Tm = max {Δθj / ωjmax} (2b)

Here, max {} means that the largest one of the elements (one or more) in parentheses is selected.

Further, the target speed ωjc related to the j-axis calculated by each axis speed calculation unit 8 is expressed by Expression (3).

Ωjc = (θbj−θaj) / Tm (3)

Here, θaj and θbj are j-axis joint angles at adjacent teaching points Pa and Pb, respectively, and Tm is the above-described movement time. Therefore, the target speed ωc is a vector quantity having the target speed of each joint axis as an element. Note that ωjc can be obtained in advance before PTP interpolation.

In the case of PTP interpolation, the shortest acceleration / deceleration time is calculated and output based on the target speed of each joint axis from each axis speed calculation unit 8.

First, the shortest acceleration time Taj about the j-axis is obtained as follows.

Taj = | ωjc | / αajmax (4)

Here, ωjc is the j-axis target speed calculated by each axis speed calculator 8, and αajmax is the allowable maximum acceleration of the j-axis when accelerating only the j-axis alone at the teaching point Pa.

Acceleration time Ta when moving the robot arm 6 from the teaching point Pa is expressed by equation (5).

Ta = max {Taj} (5)

Here, max {} means that the largest value is selected from a plurality of elements in parentheses.

The shortest deceleration time Tbj for the j axis is similarly expressed by the equation (6).

Tbj = | ωjc | / αbjmax (6)

Here, αbjmax is the allowable maximum acceleration of the j axis when only the j axis is decelerated alone at the teaching point Pb.

Therefore, the deceleration time Tb when moving the robot arm 6 to the teaching point Pb is expressed by Expression (7).

Tb = max {Tbj} (7)

<STEP2: Create table>
Next, the amount of angular movement and speed at which the actual acceleration is maximized are obtained, and the table 24 is created based on these values (STEP 2).

6A shows the time change of acceleration, speed, and angular movement when acceleration / deceleration time is 100 msec, FIG. 6B shows the case where acceleration / deceleration time is 150 msec, and FIG. Show. 6A to 6C, the movement command and the acceleration based on the movement command are indicated by a broken line, and the actual value is indicated by a solid line. FIG. 6A shows the acceleration, speed, and angular movement when the movement command is increased from 0 to 1 during 100 msec. Further, the amount of angular movement when the movement command is decreased from 1 to 0 during 100 msec is also shown. Similarly, in FIG. 6B, the acceleration, speed, and angular movement amount when the movement command is increased from 0 to 1 during 150 msec, and in FIG. 6C, the movement command is increased from 0 to 1 during 200 msec. The acceleration, speed, and amount of angular movement are shown.

As shown in FIG. 6A, the speed and acceleration of the rotating shaft during acceleration are smaller than the movement command and acceleration based thereon. This is based on the response delay of the servo control system as described above. Therefore, the actual acceleration / deceleration profile does not have a trapezoidal shape but a shape having a maximum value at time ta. The actual acceleration α100, speed ω100, and angular movement amount Δθa100 at time ta are arranged in association with the acceleration / deceleration time 100 msec. Although not shown, when the movement command is decreased from 1 to 0 during 100 msec, the time at which the actual acceleration becomes maximum is time tb (not shown), and the angle up to the joint angle θb at time tb. The movement amount Δθb100 is also arranged in the same manner as the acceleration α100 and the like in association with the acceleration / deceleration time 100 msec.

As shown in FIGS. 6B and 6C, the acceleration / deceleration time is changed to obtain values α150, α200, etc. corresponding to the acceleration α100, respectively, and are arranged in association with the acceleration / deceleration time in the same manner as described above. In this way, the table 24 shown in FIG. 7 is created, and the table 24 is stored in the storage unit 20.

The acceleration α100, speed ω100, angular movement amount Δθa100, and the like may be obtained by actually measuring various sensors attached to the rotation axis of the robot arm 6 or may be obtained by other methods. For example, these values may be derived by simulation based on the block diagram shown in FIG. 8 using a servo control system model composed of the robot arm 6 and the robot controller 10 and each parameter. Good. In other words, after setting the acceleration / deceleration time, the temporal change of the speed and acceleration is derived by simulation, and the value at which the derived acceleration is maximized and the speed and angular movement at that time are obtained and incorporated in the table 24. To do. In addition, since the control constant used for driving each joint axis is common to all axes, only one type of table 24 is prepared. However, a 6-axis robot or the like may use different control constants for the basic axis and the wrist axis. In such a case, although not shown in FIG. 7, the acceleration / deceleration time is made common for each joint axis of the robot arm 6, and each data is acquired to create the table 24. Further, the number of data included in the table 24 is determined as appropriate according to the interval of acceleration / deceleration time.

<STEP3: Calculation of Dynamic Torque and Inertia Term Torque>
Based on the table 24 created in STEP 2, the dynamic torque calculation unit 21 calculates the dynamic torque and inertia term torque of each joint axis (STEP 3).

Specifically, the following parameters in the table 24, the acceleration αap, the speed ωap, the joint angle θap at the time ta when the acceleration is maximum, the acceleration αbp, the speed ωbp, the joint angle θbp at the time tb when the acceleration is minimum, Then, the dynamic torque at the time ta and the time tb is calculated from constants (see FIG. 3) such as the link length, the position of the center of gravity of the robot arm 6, the position of the center of gravity, the moment of inertia of the link, and the inertia of the motor.

This dynamic torque is the torque of each joint axis required to operate at the speed ω and acceleration α in the case of the joint angle θ, and the joint angle θ, speed ω, and acceleration α are obtained from the components of each joint axis. Vector quantity.

Also, the dynamic torque at time t is expressed by equation (15).

Τ (t) = H (θ (t)) · α (t) + D · ω (t) + b (ω (t)) + c (θ (t)) (15)

Here, H (θ (t)) is an inertia matrix at the joint angle θ (t), and D is a viscosity matrix composed of viscosity coefficients. Further, b (ω (t)) is the sum of the centrifugal force and the Coriolis force due to the velocity ω (t) at the joint angle θ (t) at time t, and c (ω (t)) is the joint angle θ (t ) Gravity torque. In addition, Formula (15) can be derived | led-out by standing and solving the equation of motion of the robot arm 6 by the Lagrange method or the Newton Euler method.

Here, the right side of Expression (15) is divided into the first term and the other parts, and the form is expressed by Expression (16). The first term on the right side of the equations (15) and (16) is a component that depends only on acceleration (hereinafter referred to as inertia term torque). The second term of equation (16) is a component that depends on the speed ω (t) (hereinafter referred to as speed-dependent term torque), and the second term is a component that depends on gravity (hereinafter referred to as gravity term torque). is there. In the following description, τc (t) may be referred to as residual torque.

Τ (t) = τa (t) + τd (t) + τe (t) = τa (t) + τc (t) (16)

However,
τa (t) = H (θ (t)) · α (t) (17)
τd (t) = D · ω (t) + b (ω (t)) (18)
τe (t) = c ((ω (t)) (19)
τc (t) = τd (t) + τe (t) (20)

The dynamic torque calculation unit 21 substitutes the acceleration αap, the speed ωap, the joint angle θap, the acceleration αbp, the speed ωbp, and the joint angle θbp at the time ta described above into the equations (16) to (19) to obtain the time Inertia term torque τap, velocity dependent term torque τdap, gravity term torque τeap at ta, inertia term torque τbp at time tb, velocity dependent term torque τdbp, and gravity term torque τebp are calculated.

For example, in the biaxial robot shown in FIG. 3, the dynamic torque τ1 (t) of the first axis and the dynamic torque τ2 (t) of the second axis are expressed by equations (21) and (22), respectively.

τ1 (t) = (H11 · α1 + H12 · α2) + (D11 · ω1 + b1) + c1 (21)
τ2 (t) = (H21 · α1 + H22 · α2) + (D22 · ω2 + b2) + c2 (22)

Note that the inertia matrices H11 to H22 are expressed by the following equations (23) to (26).

H11 = m1 · lg1 ² + m2 · L1 ² + m2 · lg2 ² + I1 + I2 + 2 · m2 · L1 · lg2 · cos θ2 + Jm1 (23)
H12 = m ² · lg ^{2 2} + I 2 + m ² · L 1 · lg 2 · cos θ ² (24)
H21 = m2 · lg2 ² + I2 + m2 · L1 · lg2 · cos θ2 (25)
H22 = m2 · lg2 ² + I2 + Jm2 (26)

Also, the viscosity matrices D11 and D22 are expressed by the equations (27) and (28).

D11 = d1 (27)
D22 = d2 (28)

Further, b1 and b2 which are speed-dependent term torques are expressed by equations (29) and (30), respectively.

b1 = −m2 · L1 · lg2 · (2 · ω1 + ω2) · ω2 · sinθ2 (29)
b2 = m2 · L1 · lg2 · ω1 ² · sin θ2 (30)

Further, c1 and c2 corresponding to the gravity term torque are expressed by the equations (31) and (32), respectively.

c1 = (m1 · g · lg1 + m2 · g · L1) cos θ1 + m2 · g · lg2 · cos (θ1 + θ2) (31)
c2 = m2 · g · lg2 · cos (θ1 + θ2) (32)

Here, the contents of each symbol in the equations (21) to (32) are as follows.

θ1: The rotation angle of the first axis at time t (the counterclockwise direction on the paper is the positive direction)
θ2: rotation angle of the second axis at time t (the counterclockwise direction on the paper is defined as the positive direction)
ω1: rotation speed of the first axis at time t ω2: rotation speed of the second axis at time t α1: rotation acceleration of the first axis at time t α2: rotation acceleration of the second axis at time t m1: rotation of the first axis The center-of-gravity mass of the link m2: The center-of-gravity mass of the link of the second axis, and m2 includes the mass of the load at the end of the link.

lg1: distance from the center of rotation of the first axis to the center of gravity position lg2: distance from the center of rotation of the second axis to the position of the center of gravity L1: distance from the center of rotation of the first axis to the center of rotation of the second axis I1: first Main inertia moment around the rotation axis at the center of gravity of the shaft I2: Main inertia moment around the rotation axis at the center of gravity of the second axis Jm1: Converted value of the arm rotation axis of the motor inertia of the first axis Jm2: Second The value converted into the arm rotation axis of the motor inertia of the shaft d1: The viscous friction coefficient of the first axis d2: The viscous friction coefficient of the second axis g: The gravitational acceleration.

<STEP4: Calculation of correction coefficient>
FIG. 9 shows temporal changes in joint angle, speed, acceleration, and dynamic torque during acceleration / deceleration on the j-axis. FIG. 9 is also a schematic diagram showing a concept for deriving the correction coefficient γ.

As described above, the correction processing unit 22 calculates the dynamic torque, inertia term torque, and torque other than the inertia term torque at a predetermined time calculated by the dynamic torque calculation unit 21 (speed-dependent term torque, gravity term torque). The first and second correction coefficients γa and γb are calculated and output based on the above.

Specific procedures for calculating the first and second correction coefficients γa and γb will be described below. Note that the maximum torque that can be output on the j-axis is the allowable maximum torque τjmax. In the present embodiment, the amount of change in the gravity term torque and the speed-dependent term torque before and after the times ta and tb are sufficiently smaller than the amount of change in the inertia term torque.

First, using the allowable maximum torque τjmax related to the j-axis and the residual torque τcapj of the j-axis at time ta, a torque τarj shown in Expression (33) is calculated.

Τarj = τjmax-τcapj (33)

Note that the sign of the allowable maximum torque τjmax is matched with the sign of the dynamic torque τj (ta) at time ta. *

Next, using the j-axis inertia term torque τapj and torque τarj at time ta calculated by the dynamic torque calculation unit 21, a correction coefficient γaj for the j-axis is calculated.

Γaj = τapj / τarj (34)

Similarly, torque τbrj shown in Expression (35) is calculated using allowable maximum torque τjmax related to j-axis and residual torque τcbpj of j-axis at time tb.

Τbrj = τjmax−τcbpj (35)

The sign of the allowable maximum torque τjmax is set to the same sign as the dynamic torque τj (tb) at time tb. *

Next, using the j-axis inertia term torque τbpj and torque τbrj at time tb calculated by the dynamic torque calculator 21, a correction coefficient γbj for the j-axis is calculated.

Γbj = τbpj / τbrj (36)

Here, the torque τarj shown in the equation (33) and the torque τbrj in the equation (35) can be used among the allowable maximum torque τjmax of the j-axis when performing the acceleration / deceleration rotation operation at the maximum acceleration on the j-axis. This corresponds to torque (hereinafter, sometimes referred to as available torque). Further, the inertia term torques τapj and τbj are torques necessary for realizing the acceleration / deceleration rotation operation at the maximum acceleration on the j-axis.

Therefore, if γaj and γbj shown in equations (34) and (36) are 1 or less, the j-axis can be accelerated and decelerated and rotated at a predetermined maximum acceleration. On the other hand, if γaj and γbj are larger than 1, the j-axis means that there is not enough torque necessary to realize a desired operation.

Next, for all joint axes of the robot arm 6, the correction coefficients are calculated by the equations (33) to (36), respectively, and the maximum correction coefficient obtained during the acceleration operation is selected to obtain the first correction. The coefficient is γa. In addition, the maximum correction coefficient obtained during the deceleration operation is selected as the second correction coefficient γb. In the following description, γa and γb may be collectively referred to as correction coefficient γ.

γa = max {γaj} (37)
γb = max {γbj} (38)

<STEP5: Correction of acceleration / deceleration time>
The calculation unit 23 multiplies the acceleration time Ta calculated by the acceleration / deceleration time calculation unit 9 by the first correction coefficient γa calculated by the correction processing unit 22 to obtain a corrected acceleration time Ta ′. Output to the deceleration processing unit 4. Further, the calculation unit 23 multiplies the deceleration time Tb calculated by the acceleration / deceleration time calculation unit 9 by the second correction coefficient γb calculated by the correction processing unit 22 to obtain a corrected deceleration time Tb ′. And output to the acceleration / deceleration processing unit 4. Note that Ta ′ and Tb ′ are expressed by equations (39) and (40), respectively.

Ta ′ = Ta · γa (39)
Tb ′ = Tb · γb (40)

As is clear from the equations (33) to (40), when γa and γb ≦ 1, it means that there is no shortage of torque in any joint axis. In this case, the corrected acceleration time Ta ′ and deceleration time Tb ′ obtained by multiplying the first and second correction coefficients γa and γb are the same as the initial acceleration time Ta and deceleration time Tb.

On the other hand, in the case of γa, γb> 1, it means that torque shortage has occurred in any of the joint axes. Therefore, the joint axis is rotated at a new acceleration time Ta ′ (> Ta) obtained by multiplying the initial acceleration time Ta by γa (> 1). This means that the initial acceleration of the joint axis is reduced to 1 / γa times, that is, the inertia term torque is reduced to 1 / γa times. Therefore, the j-axis dynamic torque τj corrected by the first correction coefficient γa does not exceed the allowable maximum torque τjmax. Similarly, the j-axis dynamic torque τj corrected by the second correction coefficient γb does not exceed the allowable maximum torque τjmax.

The acceleration / deceleration processing unit 4 performs the above-described acceleration / deceleration processing on the movement command sequence output from the movement command generation unit 3 using the corrected acceleration time Ta ′ or deceleration time Tb ′.

[Effects]
As described above, the robot control apparatus 10 according to the present embodiment controls the operation of the robot arm 6 having a plurality of sets of motors and joint axes that are rotationally driven by the motors.

The robot control device 10 calculates a dynamic torque in each joint axis based on the rotational speed, rotational acceleration, and joint angle of each joint axis, and outputs the dynamic torque in each joint axis. The movement amounts Δθap, Δθbp of the joint angles, the rotational speeds ωap, ωbp, the maximum values αap of the rotational acceleration, and the minimum values αbp of the joint axes at the time points ta, tb at which the rotational acceleration becomes maximum and minimum are added to each joint axis. It includes at least a table 24 stored in association with acceleration / deceleration times Ta and Tb for decelerating operation, and a storage unit 20 in which the table 24 is stored.

Based on the values in the table 24, the dynamic torque calculation unit 21 calculates the dynamic torque corresponding to the maximum value αap and the minimum value αbp of the rotational acceleration, and also corresponds to the maximum value αap and the minimum value αbp of the rotational acceleration. Inertia torques τap, τbp to be calculated and residual torques τcap, τcbp obtained by subtracting the inertial term torque from the dynamic torque, and at least one of the joint axes, the inertia term torque τap is applied to each joint axis. The available torque (τmax−τcap) that can be used for the acceleration / deceleration operation at a predetermined acceleration or the inertial term torque τbp that can be used for the acceleration / deceleration operation of each joint axis at a predetermined acceleration ( When larger than (τmax−τcbp), the common acceleration / deceleration time Ta or Tb in the rotation operation of each joint axis is corrected.

According to the present embodiment, in an articulated robot, it can be detected in advance whether or not the torque necessary for acceleration / deceleration at each joint axis is insufficient. If the torque is insufficient, the acceleration time Ta or the deceleration time Tb is detected. And the rotation of the joint shaft can be accelerated with a torque within a range that can be output, and the operation of the robot arm 6 can be controlled. Thus, the trouble of correcting the teaching of the robot operation program 1 can be saved.

Further, the robot control device 10 generates a movement command Δθ for each joint axis based on the distance L between adjacent work points in the robot arm 6 and the target movement speed V, and outputs the movement command Δθ. The torque ratio between the available torque, which is a component obtained by subtracting the residual torque τcap or τcbp from the allowable maximum torque τjmax that can be output by the j-axis that is one joint axis, and the inertial term torques τap, τbp is calculated. And a correction processing unit 22 that calculates the torque ratio in each shaft and outputs the maximum value of the torque ratios in each joint shaft as a correction coefficient γ. Further, when the correction coefficient γ is 1 or less, the robot control apparatus 10 performs acceleration / deceleration processing on the movement command Δθ based on a preset acceleration time Ta or deceleration time Tb, and when the correction coefficient γ exceeds 1. And an acceleration / deceleration processing unit 4 that performs acceleration / deceleration processing of the movement command Δθ based on the acceleration time Ta ′ or the deceleration time Tb ′ corrected by multiplying the acceleration time Ta or the deceleration time Tb by the correction coefficient γ.

The operation of the robot arm 6 is controlled by rotationally driving each joint axis based on the movement command Δθ subjected to acceleration / deceleration processing by the acceleration / deceleration processing unit 4.

According to the present embodiment, in an articulated robot, it is possible to detect in advance whether or not the torque necessary for acceleration at each joint axis is insufficient. If the torque is insufficient, the value is in accordance with the shortage. By correcting the acceleration time Ta with the first correction coefficient γa (> 1) and setting the acceleration time Ta to a time Ta ′ larger than the initial value, the rotational movement of the joint shaft can be performed with a torque within the output range. By accelerating, the operation of the robot arm 6 can be controlled. Thus, the trouble of correcting the teaching of the robot operation program 1 can be saved. Similarly, the deceleration time Tb is corrected by the second correction coefficient γb (> 1), which is a value corresponding to the shortage of torque, and the deceleration time Tb is set to a time Tb ′ larger than the initial value. It is possible to control the operation of the robot arm 6 by decelerating the rotation of the joint shaft with a torque within a possible range. Thus, the trouble of correcting the teaching of the robot operation program 1 can be saved.

Further, the acceleration time Ta and / or the deceleration time Tb is corrected based on the dynamic torque at the time when the actual acceleration is maximized or minimized, that is, the time when the acceleration / deceleration operation is most rapidly performed on the joint axis. Therefore, it is possible to suppress an increase in tact, which is the operation time of the robot arm 6, while stably operating the robot arm 6.

The value stored in the table 24 may be calculated based on the movement command.

In this way, the creation work of the table 24 can be simplified and the creation time can be shortened.

In this embodiment, the acceleration / deceleration profile is trapezoidal, but it goes without saying that the present invention can be applied to other profiles.

The robot controller of the present invention can automatically adjust the acceleration / deceleration time so that the torque required for each joint axis does not exceed the torque that can be output when the robot arm is moved while suppressing an increase in tact. Thus, teaching correction of the robot operation program can be reduced, teaching work can be performed efficiently, and it is extremely useful when applied to a robot having a plurality of axes.

DESCRIPTION OF SYMBOLS 1 Operation program 2 Processing part 3 Movement command generation part 4 Acceleration / deceleration processing part 5 Servo 6 Robot arm 7 Movement time calculation part 8 Each axis speed calculation part 9 Acceleration / deceleration time calculation part 10 Robot controller 20 Storage part 21 Dynamic torque calculation Unit 22 Correction processing unit 23 Calculation unit 24 Table

Claims

A robot control device for controlling the operation of a robot arm having a plurality of sets of motors and joint axes that are rotationally driven by the motors,
A dynamic torque calculator that calculates and outputs dynamic torque in each joint axis based on the rotational speed, rotational acceleration, and joint angle of each joint axis;
The amount of movement and rotational speed of the joint angle at each joint axis at the time when the rotational acceleration of each joint axis becomes maximum and minimum, and the maximum and minimum values of the rotation acceleration are added to cause each joint axis to perform acceleration / deceleration operations. A storage unit having a table associated with the deceleration time,
The dynamic torque calculation unit calculates dynamic torque corresponding to the maximum value and minimum value of the rotational acceleration based on the values in the table, and inertia corresponding to the maximum value and minimum value of the rotational acceleration. A term torque and a residual torque obtained by subtracting the inertia term torque from the dynamic torque,
When the inertia term torque is larger than the available torque that can be used for accelerating / decelerating each joint axis at a predetermined acceleration in at least one of the joint axes, it is common in the rotational operation of each joint axis. A robot controller that corrects the acceleration / deceleration time of the robot.
The robot control device according to claim 1,
A movement command generator for generating and outputting a movement command for each joint axis based on a distance between adjacent work points and a target movement speed in the robot arm;
Calculating a torque ratio between the available torque, which is a component obtained by subtracting the residual torque from an allowable maximum torque that can be output by one joint shaft, and the inertia term torque, and further calculating the torque ratio for each joint shaft. A correction processing unit that calculates and outputs the maximum value among the torque ratios in each joint axis as a correction coefficient;
When the correction coefficient is 1 or less, the movement command is accelerated / decelerated based on the preset acceleration / deceleration time. When the correction coefficient exceeds 1, the acceleration / deceleration time is multiplied by the correction coefficient. An acceleration / deceleration processing unit that accelerates / decelerates the movement command based on the corrected acceleration / deceleration time,
A robot control device that controls the operation of the robot arm by rotationally driving each joint axis based on the movement command subjected to acceleration / deceleration processing by the acceleration / deceleration processing unit.
The robot control device according to claim 1 or 2,
The value stored in the table is calculated based on a movement command.