JPWO2019215980A1 - Robot control device - Google Patents

Abstract

The robot control device 10 that controls the operation of the articulated robot arm 6 includes a dynamic torque calculation unit 21 that calculates the dynamic torque of each joint axis, and a time point ta when the rotational acceleration of each joint axis becomes maximum. A storage unit 20 having a table 24 in which an angular movement amount Δθap, a rotation speed ωap, and a rotation acceleration αap are stored in association with an acceleration time Ta is provided. The dynamic torque calculation unit 21 calculates the inertial term torque τap and the residual torque τcap corresponding to the rotational acceleration αap, and uses the inertial term torque τappj as the difference between the allowable maximum torque τjmax and the residual torque τcap on the j-axis. If it is larger than the possible torque, the acceleration time Ta is corrected.

Description

The present invention relates to a robot control device related to robot motion control.

In an articulated robot equipped with a plurality of sets of joint shafts rotationally driven 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 shafts due to sudden acceleration / deceleration. is there.

In Patent Document 1, the maximum or minimum acceleration of the reference joint axis is modified so that the torque obtained by using the dynamic model becomes the target torque while keeping the maximum or minimum acceleration arrival time of each joint axis constant. The technology to be used is disclosed. By doing so, it is possible to suppress the vibration of the robot arm during acceleration / deceleration and perform smooth acceleration / deceleration operation.

Japanese Unexamined Patent Publication No. 2010-269385

By the way, 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 maximum allowable torque of the motor. That is, when a plurality of joint axes operate at the same time, the torque required to operate the joint axes may be insufficient.

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

Usually, in such a case, the operation program of the robot arm is modified. Specifically, it responds by adding a command to adjust the acceleration / deceleration time or changing the target operating speed.

However, this correction work had to be performed by actually driving the operation program and performing cut and try, which required man-hours.

Further, in the configuration shown in Patent Document 1, the maximum or minimum acceleration of the reference joint axis is maintained so that the torque obtained by 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 is 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 this point, and the acceleration / deceleration time is set so that the torque required to operate the joint axis does not exceed the maximum allowable torque of the motor while suppressing the increase in the tact of the robot. The purpose is to provide a robot control device equipped with a mechanism for automatically adjusting the torque.

In order to achieve the above object, the robot control device according to the present invention is a robot control device that controls the operation of a robot arm having a plurality of sets of a motor and joint shafts rotationally driven by the motor, and is a robot control device of each joint shaft. A dynamic torque calculation unit that calculates and outputs the dynamic torque in each joint axis based on the rotational speed, rotational acceleration, and joint angle, and each joint at the time when the rotational acceleration of each joint axis becomes maximum and minimum. At least a storage unit having a table in which the movement amount and rotation speed of the joint angle on the shaft and the maximum value and the minimum value of the rotation acceleration are associated with the acceleration / deceleration time for accelerating / decelerating each joint axis is provided. The kinematic torque calculation unit calculates the kinetic torque corresponding to the maximum value and the minimum value of the rotational acceleration based on the values in the table, and the inertia corresponding to the maximum value and the minimum value of the rotational acceleration. The term torque and the residual torque obtained by subtracting the inertia term torque from the kinematic torque are calculated, and the inertia term torque accelerates / decelerates each joint axis at a predetermined acceleration in at least one of the joint axes. When it is larger than the available torque available for operation, it is characterized by correcting a common acceleration / deceleration time in the rotational operation of each joint axis.

According to this configuration, the shortage of torque required for the operation of each joint axis can be detected in advance, the acceleration / deceleration time or the movement time is corrected by the value corresponding to the shortage of torque, and one of these values is initially set. By making it larger than the value of, the joint shaft can be rotationally driven with a torque within the output range, and the operation of the robot arm can be controlled. As a result, it is possible to save the trouble of teaching and modifying the operation program of the robot. In addition, the robot arm is stabilized because the acceleration / deceleration time is corrected based on the dynamic torque at the time when the actual acceleration becomes maximum or minimum, that is, the time when the acceleration / deceleration operation on the joint axis is performed most rapidly. It is possible to suppress an increase in tact, which is the operating time of the robot arm, while operating the robot arm.

As described above, according to the robot control device according to 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. This makes it possible to save the trouble of teaching and modifying the robot operation program. In addition, it is possible to suppress an increase in the tact of the robot.

FIG. 1 is a functional block diagram of a robot control device according to an embodiment of the present invention. FIG. 2 is a diagram showing changes in joint angles, velocities, and accelerations over time on different joint axes. FIG. 3 is a schematic view showing the configuration of a two-axis robot. FIG. 4 is a diagram showing time changes in speed, acceleration, and kinetic 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 time changes in acceleration, speed, and angular movement amount when the acceleration / deceleration time is 100 msec. FIG. 6B is a diagram showing time changes in acceleration, speed, and angular movement amount when the acceleration / deceleration time is 150 msec. FIG. 6C is a diagram showing time changes in acceleration, speed, and angular movement amount when the acceleration / deceleration time is 200 msec. FIG. 7 is a diagram showing the structure of the table. FIG. 8 is a block diagram for deriving the amount of angular movement, velocity, and acceleration. FIG. 9 is a diagram showing changes over time in joint angles, velocities, accelerations, and kinetic torques 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 preferred embodiments is merely exemplary and is not intended to limit the present invention, its applications or its uses.

(Embodiment)
[Functional block configuration of robot control device]
FIG. 1 shows a functional block configuration of a robot control device according to the present embodiment, FIG. 2 shows time changes in speed and acceleration on i-axis and j-axis, which are different joint axes, and FIG. 3 shows a two-axis robot. The schematic diagram of is shown. The subscripts i and j shown in the following description 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 rotation speed and acceleration of the i-axis and j-axis, which are the rotation axes, may be simply referred to as the speed and acceleration of the i-axis and j-axis.

The robot control device 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, and 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 functional block shown in FIG. 1 executes software stored in the recording unit 20 or read from the outside on an arithmetic unit such as a CPU (Central Processing Unit) (not shown) provided in the robot control device 10. It will be realized by. The operation program 1 may be included in this software. Further, for convenience of explanation, illustration of a plurality of sets including a joint shaft incorporated in the robot arm 6 and a motor connected to the joint shaft and rotationally driving the joint shaft is omitted.

In the operation program 1, the position of the teaching point 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 the working point position in the orthogonal space or the joint angle of the robot arm 6. The target moving speed is the target speed when moving between the teaching points in the instructed interpolation form.

Interpolation forms include CP (continuus pass) interpolation, which interpolates work points in orthogonal space so as to draw a specific shape such as a straight line or arc, and movement of each joint axis at a constant speed to create a locus of work points. There is PTP (point to point) interpolation regardless of the shape, but the case where PTP interpolation is performed will be described below. PTP interpolation is an interpolation form applied when moving between teaching points in the shortest time, and is often operated so that any or a plurality of joint axes of the robot arm 6 rotate at the maximum speed thereof.

When the teaching point position recorded in the operation program 1 is a value in the orthogonal space, the processing unit 2 converts it into the joint angle of each joint axis of the robot arm 6, and the teaching point position is 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, any teaching point recorded on the operation program 1 will be referred to as Pa, and the next teaching point will be referred to as 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 the amount of joint angle movement per unit time (hereinafter, may be simply referred to as the amount of angular movement). The amount of angular movement of each joint is collectively made into one set, and this is used 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 chronological order. A plurality of sets of movement commands output in chronological order are called a movement command sequence. The movement command sequence output by the movement command generation unit 3 is not subjected to the acceleration / deceleration process described later.

The acceleration / deceleration processing unit 4 performs acceleration / deceleration processing on the movement command sequence from the movement command generation unit 3 and outputs the accelerated / decelerated movement command. The acceleration / deceleration processing unit 4 accelerates / decelerates the movement command sequence from the movement command generation unit 3 so that the target movement speed is reached in the acceleration time output by the 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. Further, the movement command sequence from the movement command generation unit 3 is arranged 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 elapses. 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 time so as not to induce vibration of the robot arm 6. The acceleration / deceleration process is performed based on the corrected acceleration time or deceleration time output by the calculation unit 23, which will be described later, and the specific procedure of the acceleration / deceleration process will be described later.

The servo 5 controls the operation of the robot arm 6 by rotationally driving a 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 from the teaching point Pa to the teaching point Pb at the target moving speed without accelerating or decelerating. The required time corresponds to the travel time.

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

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

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

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

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

The calculation unit 23 multiplies the acceleration time Ta or deceleration time Tb calculated by the acceleration / deceleration time calculation unit 9 by the first or second correction coefficients γa and γ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 becomes 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 relation to the acceleration / deceleration time.

The acceleration time is, for example, the time from the teaching point Pa to the start of acceleration to reach the target moving speed, and the deceleration time is, for example, the time from the start of deceleration before the teaching point Pb until the moving speed becomes zero. The time is used, and the acceleration / deceleration time is the total time of both. In addition, for each joint axis, the acceleration time when accelerating to the target speed with the allowable maximum acceleration of each joint axis (sometimes called the shortest acceleration time of each axis) is obtained, and among the shortest acceleration times in each of these joint axes. Select the one with the largest value in, and use this as the acceleration time common to all axes. As shown in FIG. 2, all joint axes accelerate with an acceleration time common to all axes. At the time of deceleration, the one having the maximum value among the shortest deceleration times in each joint axis is selected, and this is set as the deceleration time common to all axes. As in the case of acceleration, all joint axes decelerate in the deceleration time common to all axes (see FIG. 2).

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

[Procedure for correcting acceleration / deceleration time]
FIG. 4 shows the time variation of velocity, acceleration and kinetic torque during acceleration on the j-axis. Note that FIG. 4 shows the acceleration and torque calculated based on the movement command output from the movement command generation unit 3 corresponding to the speed of the j-axis. Further, in FIG. 4, the solid line shows the dynamic torque calculated based on the movement command, and the broken line shows the dynamic torque actually applied to the j-axis.

As shown in FIG. 4, the kinetic 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 with respect to the j-axis, the kinetic torque calculated based on the movement command exceeds the allowable maximum torque τjmax in the period from time t1 to t3. There is. This means that there is not enough torque to drive the joint shaft. In such a case, for example, it is necessary to make a correction 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 device 10 to which the robot arm 6 is connected, the actual speed changes with time (hereinafter, referred to as a speed profile) rather than the movement command at the time of acceleration / deceleration due to the delay of the servo control system or the like. ) Is often smaller (see the broken line in FIG. 4), and accordingly, the actual dynamic torque shown by the broken line becomes smaller than the dynamic torque calculated based on the command speed.

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

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

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

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

In the case of PTP interpolation, when the joint angles at the teaching points Pa and Pb are θa and θb, the movement time between the working points corresponding to the teaching points is Tm, and the unit time of interpolation is Th, the movement command Δθ is , Expressed by equation (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 of the joint axes of θa and θb as an element. Further, Δθ is a vector quantity having the amount of angular movement of each joint axis as an element.

When the moving distance between the working 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 moving distance L corresponds to a straight line distance between working points. Further, when the robot arm 6 is operated by accelerating or decelerating the joint axis, if there is a joint axis exceeding its own allowable maximum speed, the equation (2a) is set so that the speed of the joint axis falls within the allowable maximum speed. The travel time Tm calculated from) is corrected.

Further, in PTP interpolation, when moving between working points corresponding to teaching points Pa and Pb in the shortest time, a value obtained by dividing the moving angle of each joint axis by its maximum speed (the shortest moving time of the axis). The maximum value in (sometimes called) is the travel 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 to select the maximum of the elements (one or more) in parentheses.

Further, the target velocity ωjc for the j-axis calculated by each axis velocity calculation unit 8 is represented by the equation (3).

ωjc = (θbj−θaj) / Tm ・ ・ ・ (3)

Here, θaj and θbj are joint angles of the j-axis at adjacent teaching points Pa and Pb, respectively, and Tm is the above-mentioned movement time. Therefore, the target velocity ωc is a vector quantity having the target velocity of each joint axis as an element. The value of ω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 for the j-axis is obtained as follows.

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

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

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

Ta = max {Taj} ・ ・ ・ (5)

Here, max {} means to select the one having the maximum value from a plurality of elements in parentheses.

Similarly, the shortest deceleration time Tbj for the j-axis is also expressed by the equation (6).

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

Here, αbjmax is the maximum permissible acceleration of the j-axis when only the j-axis is decelerated independently at the teaching point Pb.

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

Tb = max {Tbj} ... (7)

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

6A shows the time change of acceleration, speed, and angular movement amount when the acceleration / deceleration time is 100 msec, FIG. 6B shows the acceleration / deceleration time of 150 msec, and FIG. 6C shows the acceleration / deceleration time of 200 msec. Shown. In FIGS. 6A to 6C, the movement command and the acceleration based on the movement command are shown by broken lines, and the actual values are shown by solid lines. Note that FIG. 6A shows the acceleration, velocity, and angular movement amount when the movement command is increased from 0 to 1 during 100 msec. In addition, the amount of angular movement when the movement command is reduced from 1 to 0 during 100 msec is also shown in the figure. Similarly, in FIG. 6B, the acceleration, velocity, and angular movement amount when the movement command was increased from 0 to 1 during 150 msec, and in FIG. 6C, the movement command was increased from 0 to 1 during 200 msec. The acceleration, velocity, and angular movement amount at that time are shown respectively.

As shown in FIG. 6A, the velocity and acceleration of the rotating shaft during acceleration are smaller than the movement command and the acceleration based on it. This is based on the response delay of the servo control system and the like as described above. Therefore, the actual acceleration / deceleration profile does not have a trapezoidal shape, but has a shape having a maximum value at time ta. The actual acceleration α100, velocity ω100, and angular movement amount Δθa100 at time ta are arranged in association with the acceleration / deceleration time of 100 msec. Although not shown, the time at which the actual acceleration becomes maximum when the movement command is reduced from 1 to 0 during 100 msec is set as the time tb (not shown), and the angle to the joint angle θb at the 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 of 100 msec.

As shown in FIGS. 6B and 6C, the acceleration / deceleration times are changed to obtain the values α150, α200, etc. corresponding to the acceleration α100, respectively, and 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, the velocity ω100, the angular movement amount Δθa100, and the like may be obtained by actually mounting various sensors on the rotation axis of the robot arm 6 and actually measuring them, or by other methods. For example, using a model of a servo control system composed of a robot arm 6 and a robot control device 10 and each parameter, these values may be derived by simulation based on the block diagram shown in FIG. Good. That is, after setting the acceleration / deceleration time, the time change of the velocity and the acceleration is derived by the simulation, and the value at which the derived acceleration is maximized and the velocity and the angular movement amount at that time are obtained and incorporated into the table 24. To do. Since the control constants used to drive each joint axis are common to all axes, only one type of table 24 is prepared. However, in a 6-axis robot or the like, different control constants may be used for the basic axis and the wrist axis. In such a case, although not shown in FIG. 7, each data is acquired and the table 24 is created after sharing the acceleration / deceleration time for each of the joint axes of the robot arm 6. Further, the number of data included in the table 24 is appropriately determined depending on the interval of acceleration / deceleration time.

<STEP3: Calculation of kinetic torque and inertial term torque>
Based on the table 24 created in STEP 2, the dynamic torque calculation unit 21 calculates the dynamic torque and the inertial term torque of each joint shaft, respectively (STEP 3).

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

This kinetic torque is the torque of each joint axis required to operate at the velocity ω and the acceleration α in the case of the joint angle θ, and the joint angle θ, the velocity ω, and the acceleration α are derived from the components of each joint axis. Is a vector quantity.

The kinetic torque at time t is expressed by the equation (15).

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

Here, H (θ (t)) is an inertial 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). ) Is the gravitational torque. Further, the equation (15) can be derived by formulating and solving the equation of motion of the robot arm 6 by the Lagrange method or the Newton Euler method.

Here, the right-hand side of the equation (15) is divided into a first term and other parts, and the form represented by the equation (16) is used. 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 inertial term torque). The second term of the equation (16) is a component that depends on the velocity ω (t) (hereinafter, referred to as a velocity-dependent term torque), and the second term is a component that depends on gravity (hereinafter, referred to as a 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)

In the dynamic torque calculation unit 21, the acceleration αap, the velocity ωap, the joint angle θap at the time ta, the acceleration αbp at the time tb, the velocity ωbp, and the joint angle θbp are substituted into the equations (16) to (19), and the time is changed. The inertial term torque τap at ta, the speed-dependent term torque τdap, the gravity term torque τeap, and the inertial term torque τbp at time tb, the speed-dependent term torque τdbp, and the gravity term torque τebp are calculated, respectively.

For example, in the two-axis robot shown in FIG. 3, the first-axis dynamic torque τ1 (t) and the second-axis dynamic torque τ2 (t) are represented by the 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)

The inertial matrices H11 to H22 are represented 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 = m2 ・ lg2 ² + I2 + m2 ・ L1 ・ lg2 ・ cosθ2 ・ ・ ・ (24)
H21 = m2 ・ lg2 ² + I2 + m2 ・ L1 ・ lg2 ・ cosθ2 ・ ・ ・ (25)
H22 = m2 ・ lg2 ² + I2 + Jm2 ・ ・ ・ (26)

The viscosity matrices D11 and D22 are represented by the equations (27) and (28).

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

The speed-dependent term torques b1 and b2 are represented by the 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 gravitational term torque are represented 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 reference numeral in the equations (21) to (32) are as follows.

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

lg1: Distance from the center of rotation of the first axis to the position of the center of gravity 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 of the axis at the center of gravity I2: Main inertia moment around the rotation axis of the second axis at the center of gravity Jm1: Converted value of the arm rotation axis of the motor inertia of the first axis Jm2: Second Shaft motor inertia arm rotation axis conversion value d1: First axis viscous friction coefficient d2: Second axis viscous friction coefficient g: Gravity acceleration.

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

As described above, the correction processing unit 22 has a torque other than the dynamic torque, the inertial term torque, and the inertial term torque 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.

The specific procedure for calculating the first and second correction coefficients γa and γb will be described below. The maximum torque that can be output on the j-axis is defined as the allowable maximum torque τjmax. Further, in the present embodiment, the amount of change in the gravity term torque and the speed-dependent term torque before and after the time ta and tb is assumed to be sufficiently smaller than the amount of change in the inertial term torque.

First, the torque τarj represented by the equation (33) is calculated by using the allowable maximum torque τjmax with respect to the j-axis and the residual torque τcapj of the j-axis at time ta.

τarj = τjmax-τcapj ・ ・ ・ (33)

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

Next, the correction coefficient γaj with respect to the j-axis is calculated using the inertial term torque τapj and torque τarj of the j-axis at the time ta calculated by the dynamics torque calculation unit 21.

γaj = τapj / τarj ・ ・ ・ (34)

Similarly, the torque τbrj represented by the equation (35) is calculated by using the maximum allowable torque τjmax with respect to the j-axis and the residual torque τcbpj of the j-axis at time tb.

τbrj = τjmax-τcbpj ... (35)

The sign of the allowable maximum torque τjmax is matched with the sign of the dynamic torque τj (tb) at time tb.

Next, the correction coefficient γbj for the j-axis is calculated using the inertial term torque τbpj and the torque τbrj of the j-axis at the time tb calculated by the dynamics torque calculation unit 21.

γ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 maximum allowable torque τjmax of the j-axis when the acceleration / deceleration rotation operation at the maximum acceleration in the j-axis is performed. It corresponds to torque (hereinafter, may be referred to as available torque). Further, the inertial term torques τapj and τbj are torques required to realize the acceleration / deceleration rotation operation at the maximum acceleration on the j-axis.

Therefore, if γaj and γbj shown in the equations (34) and (36) are 1 or less, the j-axis can perform acceleration / deceleration rotation operation at a predetermined maximum acceleration. On the other hand, if γaj and γbj are larger than 1, it means that the j-axis does not have enough torque to realize the desired operation.

Next, for all the joint axes of the robot arm 6, the correction coefficients are calculated by the equations (33) to (36), and the largest correction coefficient obtained during the acceleration operation is selected to make the first correction. Let the coefficient be γa. Further, the maximum correction coefficient obtained during the deceleration operation is selected and used as the second correction coefficient γb. In the following description, γa and γb may be collectively referred to as a 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 the corrected acceleration time Ta'and adds it. 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 the corrected deceleration time Tb'. , Output to the acceleration / deceleration processing unit 4. In addition, Ta'and Tb'are represented by equations (39) and (40), respectively.

Ta'= Ta ・ γa ・ ・ ・ (39)
Tb'= Tb ・ γb ・ ・ ・ (40)

As is clear from the formulas (33) to (40), when γa and γb ≦ 1, it means that the torque shortage does not occur in any of the joint axes. 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, when γa and γb> 1, it means that torque is insufficient 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 by 1 / γa times, that is, the inertial term torque is reduced by 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-mentioned acceleration / deceleration processing on the movement command sequence output from the movement command generation unit 3 by using the corrected acceleration time Ta'or deceleration time Tb'.

[Effects, etc.]
As described above, the robot control device 10 in the present embodiment controls the operation of the robot arm 6 having a plurality of sets of a motor and a plurality of joint shafts rotationally driven by the motor.

Further, the robot control device 10 has a dynamic torque calculation unit 21 that calculates and outputs dynamic torque in each joint axis based on the rotational speed, rotational acceleration, and joint angle of each joint axis, and a dynamic torque calculation unit 21 of each joint axis. The amount of movement of the joint angle at each joint axis at the time points ta and tb when the rotational acceleration becomes maximum and minimum Δθap, Δθbp, the rotational speed ωap, ωbp, and the maximum value αap and minimum value αbp of the rotational acceleration add each joint axis. It includes at least a table 24 stored in association with acceleration / deceleration times Ta and Tb for deceleration operation, and a storage unit 20 in which the tables 24 are stored.

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 based on the values in the table 24, and also corresponds to the maximum value αap and the minimum value αbp of the rotational acceleration. The inertial term torques τap and τbp to be applied and the residual torques τcap and τcbp obtained by subtracting the inertial term torque from the dynamic torque are calculated, and the inertial term torque τap determines each joint axis at at least one of the joint axes. Available torque that is greater than the available torque (τmax-τcap) available for accelerating or decelerating at a given acceleration, or the inertial term torque τbp is available for accelerating or decelerating each joint axis at a given acceleration (τmax-τcap). If it is larger than τmax−τcbp), the common acceleration / deceleration time Ta or Tb in the rotational movement of each joint axis is corrected.

According to the present embodiment, in the articulated robot, it is possible to detect in advance whether or not the torque required for acceleration / deceleration at each joint axis is insufficient, and if the torque is insufficient, the acceleration time Ta or the deceleration time Tb Can be corrected to accelerate the rotational movement of the joint shaft with a torque within the output range, and the movement of the robot arm 6 can be controlled. As a result, it is possible to save the trouble of teaching and modifying the robot operation program 1.

Further, the robot control device 10 includes a movement command generation unit 3 that generates and outputs 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. The torque ratio between the available torque, which is the component obtained by subtracting the residual torque τcap or τcbp from the maximum allowable torque τjmax that can be output by the j-axis, which is one joint axis, and the inertial term torques τap and τbp, is calculated, and further, each joint is calculated. It is provided with a correction processing unit 22 that calculates the torque ratio on each shaft and outputs the maximum value in the torque ratio on each joint shaft as a correction coefficient γ. Further, the robot control device 10 accelerates / decelerates the movement command Δθ based on the preset acceleration time Ta or deceleration time Tb when the correction coefficient γ is 1 or less, and when the correction coefficient γ exceeds 1. The acceleration / deceleration processing unit 4 that accelerates / decelerates 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 γ is provided.

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

According to the present embodiment, in the articulated robot, it is possible to detect in advance whether or not the torque required for acceleration at each joint axis is insufficient, and if the torque is insufficient, it is a value according to 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. The operation of the robot arm 6 can be controlled by accelerating. As a result, it is possible to save the trouble of teaching and modifying the robot operation program 1. Similarly, the deceleration time Tb is corrected by the second correction coefficient γb (> 1), which is a value corresponding to the insufficient torque, and the deceleration time Tb is set to a time Tb'larger than the initial value to output. The operation of the robot arm 6 can be controlled by decelerating the rotational operation of the joint shaft with a torque within a possible range. As a result, it is possible to save the trouble of teaching and modifying the robot operation program 1.

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 becomes the maximum or the minimum, that is, the time when the acceleration / deceleration operation on the joint axis is performed most rapidly. Therefore, while the robot arm 6 is operated stably, it is possible to suppress an increase in tact, which is the operating time of the robot arm 6.

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

By doing so, the work of creating the table 24 can be simplified and the creation time can be shortened.

In this embodiment, the acceleration / deceleration profile has a trapezoidal shape, but it goes without saying that it can be applied to other profiles.

The robot control device of the present invention can automatically adjust the acceleration / deceleration time so that the torque required for each joint axis does not exceed the outputable torque when moving the robot arm while suppressing the increase in tact. It is possible to reduce the number of teaching corrections of the robot operation program, and the teaching work can be performed efficiently, which is extremely useful for application to a robot composed of a plurality of axes.

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

Claims (3)

A robot control device that controls the operation of a robot arm having a plurality of sets of a motor and joint shafts rotationally driven by the motor.
A dynamic torque calculation unit that calculates and outputs the dynamic torque of 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 rotational acceleration are added to accelerate and decelerate each joint axis. With at least a storage unit having a table associated with the deceleration time,
The kinetic torque calculation unit calculates the kinetic torque corresponding to the maximum value and the minimum value of the rotational acceleration based on the values in the table, and the inertia corresponding to the maximum value and the minimum value of the rotational acceleration. The term torque and the residual torque obtained by subtracting the inertial term torque from the kinetic torque are calculated.
In at least one joint axis of each joint axis, if the inertial term torque is greater than the available torque available for accelerating or decelerating each joint axis at a predetermined acceleration, it is common in the rotational movement of each joint axis. A robot control device characterized by correcting the acceleration / deceleration time of. In the robot control device according to claim 1,
A movement command generator that generates and outputs a movement command for each joint axis based on the distance between adjacent work points and the target movement speed in the robot arm.
The torque ratio between the available torque, which is a component obtained by subtracting the residual torque from the maximum allowable torque that can be output by one joint shaft, and the inertial term torque is calculated, and the torque ratio at each joint shaft is calculated. A correction processing unit that calculates and outputs the maximum value in the torque ratio of 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, and when the correction coefficient exceeds 1, the acceleration / deceleration time is multiplied by the correction coefficient. Further, an acceleration / deceleration processing unit that performs acceleration / deceleration processing of the movement command based on the corrected acceleration / deceleration time is further provided.
A robot control device characterized by controlling the operation of the robot arm by rotationally driving each joint axis based on the movement command processed by the acceleration / deceleration processing unit. In the robot control device according to claim 1 or 2.
A robot control device characterized in that the values stored in the table are calculated based on a movement command.

Images