WO2019215980A1 - Robot control device - Google Patents
Robot control device Download PDFInfo
- Publication number
- WO2019215980A1 WO2019215980A1 PCT/JP2019/005302 JP2019005302W WO2019215980A1 WO 2019215980 A1 WO2019215980 A1 WO 2019215980A1 JP 2019005302 W JP2019005302 W JP 2019005302W WO 2019215980 A1 WO2019215980 A1 WO 2019215980A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- acceleration
- torque
- time
- joint
- axis
- Prior art date
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
Definitions
- the present invention relates to a robot control apparatus related to robot operation control.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- a robot control apparatus 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.
- 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
- 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.
- 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
- FIG. 3 shows a biaxial robot.
- the schematic diagram of is shown.
- subscripts i and j are integers, 1 ⁇ i, j ⁇ n, and i ⁇ j
- n is the number of joint axes included in the robot arm 6.
- 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.
- 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.
- 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.
- 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.
- 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.
- 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.
- the time-varying waveform of acceleration may be referred to as an acceleration profile, a deceleration profile, or an acceleration / deceleration profile.
- 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
- 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.
- 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.
- 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).
- 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.
- 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.
- 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.
- the dynamic torque increases as the acceleration increases, reaches a maximum value at time t2, and then decreases as the acceleration decreases.
- 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.
- 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.
- 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.
- 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.
- the acceleration time Ta and the deceleration time Tb are calculated in the target system (STEP 1).
- ⁇ is a vector quantity having the joint angle of each joint axis of ⁇ a and ⁇ b as elements.
- ⁇ is a vector amount having the angle movement amount of each joint axis as an element.
- the movement distance L corresponds to a linear distance between work points.
- 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.
- 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.
- Tm max ⁇ j / ⁇ jmax ⁇ (2b)
- max ⁇ means that the largest one of the elements (one or more) in parentheses is selected.
- ⁇ 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.
- 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.
- the shortest acceleration time Tom about the j-axis is obtained as follows.
- ⁇ jc is the j-axis target speed calculated by each axis speed calculator 8
- ⁇ 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)
- 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).
- ⁇ bjmax is the allowable maximum acceleration of the j axis when only the j axis is decelerated alone at the teaching point Pb.
- 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).
- FIG. 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
- 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.
- 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.
- 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.
- 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.
- 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.
- control constant used for driving each joint axis is common to all axes, only one type of table 24 is prepared.
- a 6-axis robot or the like may use different control constants for the basic axis and the wrist axis.
- 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).
- 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 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.
- ⁇ (t) H ( ⁇ (t)) ⁇ ⁇ (t) + D ⁇ ⁇ (t) + b ( ⁇ (t)) + c ( ⁇ (t)) (15)
- H ( ⁇ (t)) is an inertia matrix at the joint angle ⁇ (t)
- D is a viscosity matrix composed of viscosity coefficients
- 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
- c ( ⁇ (t)) is the joint angle ⁇ (t ) Gravity torque.
- Formula (15) can be derived
- Expression (16) 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.
- ⁇ c (t) may be referred to as residual torque.
- 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.
- 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)
- H11 m1 ⁇ lg1 2 + m2 ⁇ L1 2 + m2 ⁇ lg2 2 + I1 + I2 + 2 ⁇ m2 ⁇ L1 ⁇ lg2 ⁇ cos ⁇ 2 + Jm1 (23)
- H12 m 2 ⁇ lg 2 2 + I 2 + m 2 ⁇ L 1 ⁇ lg 2 ⁇ cos ⁇ 2 (24)
- H21 m2 ⁇ lg2 2 + I2 + m2 ⁇ L1 ⁇ lg2 ⁇ cos ⁇ 2 (25)
- H22 m2 ⁇ lg2 2 + I2 + Jm2 (26)
- ⁇ 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.
- 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 ⁇ .
- 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.
- the maximum torque that can be output on the j-axis is the allowable maximum torque ⁇ jmax.
- 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.
- 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.
- the sign of the allowable maximum torque ⁇ jmax is set to the same sign as the dynamic torque ⁇ j (tb) at time tb.
- 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).
- 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.
- ⁇ 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.
- ⁇ aj and ⁇ bj are larger than 1, the j-axis means that there is not enough torque necessary to realize a desired operation.
- 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.
- the maximum correction coefficient obtained during the deceleration operation is selected as the second correction coefficient ⁇ b.
- ⁇ a and ⁇ b may be collectively referred to as correction coefficient ⁇ .
- ⁇ 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.
- Ta ′ and Tb ′ are expressed by equations (39) and (40), respectively.
- Ta ′ Ta ⁇ ⁇ a (39)
- Tb ′ Tb ⁇ ⁇ b (40)
- 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.
- the joint axis is rotated at a new acceleration time Ta ′ (> Ta) obtained by multiplying the initial acceleration time Ta by ⁇ a (> 1).
- Ta initial acceleration time
- 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.
- the j-axis dynamic torque ⁇ j corrected by the first correction coefficient ⁇ a does not exceed the allowable maximum torque ⁇ jmax.
- 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 ′.
- the robot control apparatus 10 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.
- 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 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.
- 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.
- 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 ⁇ .
- 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.
- 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.
- 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.
- 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.
- the trouble of correcting the teaching of the robot operation program 1 can be saved.
- 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.
- 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.
- 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.
- 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.
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
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
本発明は、ロボットの動作制御に係るロボット制御装置に関する。
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.
特許文献1には、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正する技術が開示されている。このようにすることで、加減速時のロボットアームの振動を抑えて、スムーズな加減速動作を行うことができる。
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.
ところで、関節軸に発生するトルクは、当該関節軸に連結されたモータの出力トルクに依存し、当該モータの許容最大トルクによって制限される。すなわち、複数の関節軸が同時に動作する場合に、関節軸を動作させるのに必要なトルクが不足する場合がある。
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.
また、特許文献1に示す構成では、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正している。
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.
上記目的を達成するため、本発明に係るロボット制御装置は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、各関節軸のうち少なくとも1つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とする。
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.
以下、本発明の実施形態を図面に基づいて詳細に説明する。以下の好ましい実施形態の説明は、本質的に例示に過ぎず、本発明、その適用物或いはその用途を制限することを意図するものでは全くない。
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.
(実施形態)
[ロボット制御装置の機能ブロック構成]
図1は、本実施形態に係るロボット制御装置の機能ブロック構成を示し、図2は、異なる関節軸であるi軸及びj軸における速度及び加速度の時間変化を示し、図3は、2軸ロボットの模式図を示す。なお、以降の説明において示す添字i,jは整数で1≦i,j≦nかつi≠jであり、nはロボットアーム6に含まれる関節軸の個数である。なお、以降の説明において、回転軸であるi軸やj軸の回転速度や回転加速度を単にi軸やj軸の速度や加速度と呼ぶことがある。 (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 therobot 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.
[ロボット制御装置の機能ブロック構成]
図1は、本実施形態に係るロボット制御装置の機能ブロック構成を示し、図2は、異なる関節軸であるi軸及びj軸における速度及び加速度の時間変化を示し、図3は、2軸ロボットの模式図を示す。なお、以降の説明において示す添字i,jは整数で1≦i,j≦nかつi≠jであり、nはロボットアーム6に含まれる関節軸の個数である。なお、以降の説明において、回転軸であるi軸やj軸の回転速度や回転加速度を単にi軸やj軸の速度や加速度と呼ぶことがある。 (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
ロボット制御装置10は、動作プログラム1と、処理部2と、移動指令生成部3と、加減速処理部4と、サーボ5と、移動時間算出部7と、各軸速度算出部8と、加減速時間算出部9と、記録部20と、動力学トルク算出部21と、補正処理部22と、演算部23とを機能ブロックとして備えている。なお、図1に示す機能ブロックは、記録部20に保存された、または外部から読み込まれたソフトウェアをロボット制御装置10に設けられた図示しないCPU(Central Processing Unit)等の演算装置上で実行することで実現される。このソフトウェアに動作プログラム1が含まれていてもよい。また、説明の便宜上、ロボットアーム6に組み込まれた関節軸と、当該関節軸に連結されて関節軸を回転駆動するモータとで構成される複数の組については図示を省略している。
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.
動作プログラム1には、ロボットアーム6の教示点位置と、教示点間を結ぶ軌跡の形状にあたる補間形態と、目標移動速度とが記録されている。教示点位置は、直交空間上の作業点位置またはロボットアーム6の関節角度である。目標移動速度は、教示点間を指示された補間形態で移動するときの目標速度である。
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.
補間形態としては、直交空間上の作業点を、直線や円弧等の特定の形状を描くように補間するCP(continuas path)補間と、各関節軸を一定の速度で動かし、作業点の軌跡の形状は問わないPTP(point to point)補間とがあるが、以降は、PTP補間を行う場合について説明する。PTP補間は教示点間を最短時間で移動する場合に適用する補間形態であり、ロボットアーム6の関節軸のいずれかもしくは複数が、その最大速度で回転するように動作させることが多い。
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.
処理部2は、動作プログラム1に記録されている教示点位置が直交空間上の値である場合に、それをロボットアーム6の各関節軸の関節角度に変換し、また、教示点位置がロボットアーム6の関節角度の場合は直交空間上の値に変換する。なお、以降の説明において、動作プログラム1上に記録された任意の教示点をPaとし、その次の教示点をPbとする。
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.
移動指令生成部3は、隣り合う教示点間の移動量と目標移動速度とに基づいて移動指令を生成して出力する。この移動指令は単位時間当たりの関節角移動量(以下、単に角度移動量と呼ぶことがある。)である。各関節の角度移動量をまとめて1組とし、これを1つの移動指令とする。よって、移動指令はベクトル量である。移動指令生成部3は、後述する移動時間に対応する複数組の移動指令を時系列的に出力する。この時系列的に出力される複数組の移動指令を移動指令列と言う。なお、移動指令生成部3が出力する移動指令列には後述する加減速処理はなされていない。
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.
加減速処理部4は、移動指令生成部3からの移動指令列に対して加減速処理を行って、加減速した移動指令を出力する。加減速処理部4は、後述する加減速時間算出部9が出力する加速時間で目標移動速度に達するように、移動指令生成部3からの移動指令列を加減速処理する。具体的には、図2に示すように、各関節軸における加速度を時間変化させて所定の目標移動速度に到達するようにする。また、教示点Pb手前で目標移動速度から減速を始めて、加減速時間算出部9が出力する減速時間が経過した後に教示点Pbに到達するように、移動指令生成部3からの移動指令列を減速処理する。なお、以降の説明において、加速度の時間変化波形を加速プロファイルまたは減速プロファイルあるいは加減速プロファイルと呼ぶことがある。なお、図2に示すように、加減速プロファイルは、ロボットアーム6の振動を誘発しないように時間に対して速度がS字状に変化する形状とする。なお、加減速処理は後述する演算部23が出力する補正後の加速時間または減速時間に基づいて行い、具体的な加減速処理の手順については後述する。
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.
サーボ5は、加減速処理部4で処理された移動指令列に基づいて、各関節軸に連結されたモータを回転駆動させてロボットアーム6の動作を制御する。
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.
移動時間算出部7は、ロボットアーム6の先端が教示点Paから教示点Pbへ加減速せずに目標移動速度で移動するのに要する所要時間を算出して出力する。当該所要時間が移動時間に相当する。
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.
各軸速度算出部8は、PTP補間時の各関節軸の目標速度を算出し出力する。
Each axis speed calculation unit 8 calculates and outputs a target speed of each joint axis at the time of PTP interpolation.
加減速時間算出部9は、教示点Paから目標速度に加速するときの加速時間、教示点Pb手前で目標速度から減速して教示点Pbに到達するときの減速時間を求め出力する。
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.
記憶部20は、後述するテーブル24(図7参照)や動作プログラム1を保存しており、ロボット制御装置10の各部からの要求に従って保存されたデータを対応するブロックに送信する。
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.
動力学トルク算出部21は、テーブル24内のデータに基づいて、所定の時刻における各関節軸の動力学トルクをそれぞれ算出し出力する。
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.
補正処理部22は、動力学トルク算出部21で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルクに基づいて、後述する第1及び第2補正係数γa,γbを算出し出力する。
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.
演算部23は、加減速時間算出部9で算出された加速時間Taまたは減速時間Tbに対して、補正処理部22で算出された第1または第2補正係数γa,γbを乗じて、補正後の加速時間Ta’または減速時間Tb’を求め、加減速処理部4に出力する。
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.
テーブル24は、ロボットアーム6の各関節軸が駆動するときに、極大または極小となる実加速度及びそのときの速度と角度移動量が加減速時間に関連付けて配列されたテーブルである。
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.
なお、加速時間は、例えば、教示点Paから加速を始めて目標移動速度に達するまでの時間をいい、減速時間とは、例えば、教示点Pbの手前で減速を始めて移動速度が零になるまでの時間をいい、加減速時間とは、これら両者を合わせた時間をいう。また、各関節軸について、各関節軸の許容最大加速度で目標速度まで加速したときの加速時間(各軸の最短加速時間と呼ぶことがある)を求め、これら各関節軸における最短加速時間の中で値が最大のものを選び、これを全軸共通の加速時間とする。図2に示すように、全ての関節軸はこの全軸共通の加速時間で加速する。また、減速時には、各関節軸における最短減速時間の中で値が最大のものを選び、これを全軸共通の減速時間とする。加速時と同様に、全ての関節軸はこの全軸共通の減速時間で減速する(図2参照)。
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).
従って、テーブル24における加減速時間は全軸共通の加減速時間である。なお、テーブル24の構成及び作成手順については後述する。
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.
[加減速時間の補正手順]
図4は、j軸における加速時の速度、加速度及び動力学トルクの時間変化を示す。なお、図4では、j軸の速度に相当する移動指令生成部3から出力された移動指令に基づいて算出された加速度やトルクを示している。また、図4において、実線は、移動指令に基づいて算出された動力学トルクを示し、破線は、実際にj軸に加わる動力学トルクを示す。 [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 movementcommand 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.
図4は、j軸における加速時の速度、加速度及び動力学トルクの時間変化を示す。なお、図4では、j軸の速度に相当する移動指令生成部3から出力された移動指令に基づいて算出された加速度やトルクを示している。また、図4において、実線は、移動指令に基づいて算出された動力学トルクを示し、破線は、実際にj軸に加わる動力学トルクを示す。 [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
図4に示すように、動力学トルクは、加速度の上昇に伴い増加し、時刻t2で最大値になった後、加速度の低下に伴い減少する。ここで、j軸が出力可能な最大トルクをj軸に関する許容最大トルクτjmaxとすると、時刻t1からt3までの期間で、移動指令に基づいて算出された動力学トルクが許容最大トルクτjmaxを上回っている。これは、関節軸を駆動させるトルクが足らないことを意味する。このような場合には、例えば、加速時間を増加させる補正を行って、動力学トルクが許容最大トルクτjmax以下になるように調整する必要がある。
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.
しかし、ロボットアーム6が接続された実際のロボット制御装置10では、サーボ制御系の遅れ等により、加減速時の移動指令よりも実際の速度の時間変化(以下、速度プロファイルと呼ぶことがある。)が小さくなることが多く(図4中の破線参照)、これに伴い、破線で示す実際の動力学トルクが指令速度に基づいて算出された動力学トルクよりも小さくなる。
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.
よって、図4に示すように、実際には動力学トルクが許容最大トルクτjmaxを超えていないにも関わらず、加速時間を増加させる補正が必要と判断されてしまうことがある。この場合、タクトが増加するため、ロボットアーム6を用いた作業の生産性が低下するという問題があった。
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:加減速時間の算出>
図5は、本実施形態に係る加減速時間の補正手順を示し、以降で説明する5つのステップを順次実行して加減速時間の補正を行う。 <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.
図5は、本実施形態に係る加減速時間の補正手順を示し、以降で説明する5つのステップを順次実行して加減速時間の補正を行う。 <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.
まず、対象となる系において、加速時間Ta及び減速時間Tbを算出する(STEP1)。
First, the acceleration time Ta and the deceleration time Tb are calculated in the target system (STEP 1).
PTP補間の場合、教示点Pa、Pbでの関節角度をそれぞれθa、θbとし、教示点間に対応する作業点間の移動時間をTm、補間の単位時間をThとするとき、移動指令Δθは、式(1)で表わされる。なお、移動時間Tmは移動時間算出部7で算出される。
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)
Δθ = (θb−θa) · Th / Tm (1)
ここで、θa、θb各関節軸の関節角度を要素とするベクトル量である。また、Δθは各関節軸の角度移動量を要素とするベクトル量である。
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.
教示点Pa,Pbに対応する作業点間の移動距離をL、目標移動速度をVとするとき、移動時間Tmは、式(2a)で表わされる。
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)
Tm = L / V (2a)
なお、PTP補間の場合、移動距離Lは、作業点間の直線距離に相当する。また、関節軸を加速または減速回転させてロボットアーム6を動作させる際に、自身の許容最大速度を超える関節軸がある場合は、当該関節軸の速度が許容最大速度に収まるように式(2a)から算出された移動時間Tmは修正される。
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.
また、PTP補間において、教示点Pa,Pbに対応する作業点間を最短時間で移動する場合は、各関節軸の移動角度をその最大速度で除して得る値(当該軸の最短移動時間と呼ぶことがある)の中の最大値を移動時間Tmとする。
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.
すなわち、複数の関節軸のうちj軸の移動角度をΔθj、最大速度をωjmaxとするとき、移動時間Tmは式(2b)で表わされる。
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)
Tm = max {Δθj / ωjmax} (2b)
ここで、max{}は括弧内の要素(1個もしくは複数個)のうちの最大のものを選択することを意味する。
Here, max {} means that the largest one of the elements (one or more) in parentheses is selected.
また、各軸速度算出部8で算出されたj軸に関する目標速度ωjcは、式(3)で表わされる。
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)
Ωjc = (θbj−θaj) / Tm (3)
ここで、θaj、θbjはそれぞれ、隣り合う教示点Pa、Pbでのj軸の関節角度であり、Tmは上述した移動時間である。よって、目標速度ωcは各関節軸の目標速度を要素とするベクトル量である。なお、ωjcはPTP補間前に予め値を求めることができる。
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.
PTP補間の場合は、各軸速度算出部8からの各関節軸の目標速度に基づいて、最短の加減速時間を算出し出力する。
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.
まず、j軸についての最短加速時間Tajを次のように求める。
First, the shortest acceleration time Taj about the j-axis is obtained as follows.
Taj=|ωjc|/αajmax ・・・(4)
Taj = | ωjc | / αajmax (4)
ここで、ωjcは各軸速度算出部8で算出されたj軸の目標速度、αajmaxは、教示点Paにおいてj軸だけを単独で加速する場合のj軸の許容最大加速度である。
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.
教示点Paからロボットアーム6を移動させるときの加速時間Taは、式(5)で表わされる。
Acceleration time Ta when moving the robot arm 6 from the teaching point Pa is expressed by equation (5).
Ta=max{Taj} ・・・(5)
Ta = max {Taj} (5)
ここで、max{}は括弧内の複数の要素から値が最大のものを選択することを意味する。
Here, max {} means that the largest value is selected from a plurality of elements in parentheses.
j軸についての最短減速時間Tbjについても同様に式(6)で表わされる。
The shortest deceleration time Tbj for the j axis is similarly expressed by the equation (6).
Tbj=|ωjc|/αbjmax ・・・(6)
Tbj = | ωjc | / αbjmax (6)
ここで、αbjmaxは、教示点Pbにおいてj軸だけを単独で減速する場合のj軸の許容最大加速度である。
Here, αbjmax is the allowable maximum acceleration of the j axis when only the j axis is decelerated alone at the teaching point Pb.
従って、教示点Pbへロボットアーム6を移動させるときの減速時間Tbは、式(7)で表わされる。
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)
Tb = max {Tbj} (7)
<STEP2:テーブル作成>
次に、実際の加速度が極大となるときの角度移動量や速度を求め、これらに基づいてテーブル24を作成する(STEP2)。 <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).
次に、実際の加速度が極大となるときの角度移動量や速度を求め、これらに基づいてテーブル24を作成する(STEP2)。 <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は、加減速時間が100msecの場合の、図6Bは、加減速時間が150msecの場合の、図6Cは、加減速時間が200msecの場合の加速度、速度、角度移動量の時間変化をそれぞれ示す。図6A~6Cにおいて、移動指令及びそれに基づいた加速度を破線で示し、実際の値を実線で示している。なお、図6Aでは、100msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量を示している。また、100msecの間に移動指令を1から0まで減少させたときの角度移動量もあわせて図示している。同様に、図6Bでは、150msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量を、図6Cでは、200msecの間に移動指令を0から1まで増加させたときの加速度、速度、角度移動量をそれぞれ示している。
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.
図6Aに示すように、加速中の回転軸の速度及び加速度は、移動指令及びそれに基づく加速度よりも小さくなっている。これは、前述したように、サーボ制御系の応答遅れ等に基づく。従って、実際の加減速プロファイルは台形形状とならずに、時刻taで極大値を有する形状となる。時刻taにおける実際の加速度α100,速度ω100、角度移動量Δθa100を加減速時間100msecに関連付けて配列する。また、図示しないが、100msecの間に移動指令を1から0まで減少させたときに、実際の加速度が極大となる時刻を時刻tb(図示せず)とし、時刻tbにおける関節角度θbまでの角度移動量Δθb100も加減速時間100msecに関連付けて加速度α100等と同様に配列する。
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.
図6B,6Cに示すように、加減速時間を変化させて、それぞれ、加速度α100に対応する値α150,α200等を求めて、上記と同様に加減速時間に関連付けて配列する。このようにして図7に示すテーブル24を作成し、テーブル24は記憶部20に保存される。
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.
なお、加速度α100,速度ω100、角度移動量Δθa100等は、実際のロボットアーム6の回転軸に各種センサを取付けて実測して求めてもよいし、他の方法で求めてもよい。例えば、ロボットアーム6とロボット制御装置10とで構成されるサーボ制御系のモデル及び各パラメータを用いて、図8に示すブロック線図に基づいて、シミュレーションによりこれらの値を導出するようにしてもよい。つまり、加減速時間を設定した後に、シミュレーションにより速度、加速度の時間変化を導出し、導出された加速度が極大となる値及びその時刻での速度、角度移動量を求めてテーブル24に組み込むようにする。なお、各関節軸の駆動に用いられる制御定数は、全軸共通であるため、テーブル24は1種類だけ準備される。ただし、6軸ロボット等では基本軸と手首軸とで異なる制御定数を用いる場合もある。このような場合は、図7には図示していないが、ロボットアーム6の関節軸のそれぞれについて加減速時間を共通とした上で、各データが取得されてテーブル24が作成される。また、テーブル24に含まれるデータの個数は、加減速時間の間隔により適宜決められる。
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:動力学トルク及び慣性項トルクの算出>
STEP2で作成したテーブル24に基づいて、動力学トルク算出部21で各関節軸の動力学トルク及び慣性項トルクをそれぞれ算出する(STEP3)。 <STEP3: Calculation of Dynamic Torque and Inertia Term Torque>
Based on the table 24 created in STEP 2, the dynamictorque calculation unit 21 calculates the dynamic torque and inertia term torque of each joint axis (STEP 3).
STEP2で作成したテーブル24に基づいて、動力学トルク算出部21で各関節軸の動力学トルク及び慣性項トルクをそれぞれ算出する(STEP3)。 <STEP3: Calculation of Dynamic Torque and Inertia Term Torque>
Based on the table 24 created in STEP 2, the dynamic
具体的には、テーブル24内の以下のパラメータ、加速度が極大となる時刻taにおける加速度αap、速度ωap、関節角度θap、加速度が極小となる時刻tbにおける加速度αbp、速度ωbp、関節角度θbp、また、ロボットアーム6のリンク長、重心位置、重心質量、リンクの慣性モーメント、モータのイナーシャ等の定数(図3参照)から、時刻ta及び時刻tbでの動力学トルクを算出する。
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.
また、時刻tにおける動力学トルクは式(15)で表わされる。
Also, the dynamic torque at time t is expressed by equation (15).
τ(t)=H(θ(t))・α(t)+D・ω(t)+b(ω(t))+c(θ(t)) ・・・(15)
Τ (t) = H (θ (t)) · α (t) + D · ω (t) + b (ω (t)) + c (θ (t)) (15)
ここで、H(θ(t))は関節角度θ(t)における慣性行列であり、Dは粘性係数からなる粘性行列である。さらに、b(ω(t))は、時刻tでの関節角度θ(t)における速度ω(t)による遠心力、コリオリ力の和、c(ω(t))は、関節角度θ(t)における重力トルクである。また、なお、式(15)は、ラグランジュ法またはニュートンオイラー法によりロボットアーム6の運動方程式を立てて解くことで導出できる。
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.
ここで、式(15)の右辺を、第1項とそれ以外の部分とに分けて、式(16)で表わされる形式とする。なお、式(15)、(16)の右辺の第1項は加速度のみに依存する成分(以下、慣性項トルクという。)である。また、式(16)の第2項は速度ω(t)に依存する成分(以下、速度依存項トルクという。)、第2項は重力に依存する成分(以下、重力項トルクという。)である。なお、以降の説明において、τc(t)を残余トルクと呼ぶことがある。
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)
Τ (t) = τa (t) + τd (t) + τe (t) = τa (t) + τc (t) (16)
ただし、
τ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) 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)
τ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) 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)
動力学トルク算出部21では、上述した時刻taにおける加速度αap、速度ωap、関節角度θap、時刻tbにおける加速度αbp、速度ωbp、関節角度θbpを式(16)~(19)に代入して、時刻taでの慣性項トルクτap、速度依存項トルクτdap、重力項トルクτeap及び時刻tbでの慣性項トルクτbp、速度依存項トルクτdbp、重力項トルクτebpをそれぞれ算出する。
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.
例えば、図3に示す2軸ロボットにおいて、第1軸の動力学トルクτ1(t)及び第2軸の動力学トルクτ2(t)は、それぞれ式(21)、(22)で表わされる。
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) τ1 (t) = (H11 · α1 + H12 · α2) + (D11 · ω1 + b1) + c1 (21)
τ2 (t) = (H21 · α1 + H22 · α2) + (D22 · ω2 + b2) + c2 (22)
τ2(t)=(H21・α1+H22・α2)+(D22・ω2+b2)+c2 ・・・(22) τ1 (t) = (H11 · α1 + H12 · α2) + (D11 · ω1 + b1) + c1 (21)
τ2 (t) = (H21 · α1 + H22 · α2) + (D22 · ω2 + b2) + c2 (22)
なお、慣性行列H11~H22は以下の式(23)~(26)で表わされる。
Note that the inertia matrices H11 to H22 are expressed by the following equations (23) to (26).
H11=m1・lg12+m2・L12+m2・lg22+I1+I2+2・m2・L1・lg2・cosθ2+Jm1 ・・・(23)
H12=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(24)
H21=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(25)
H22=m2・lg22+I2+Jm2 ・・・(26) H11 = m1 · lg1 2 + m2 · L1 2 + m2 · lg2 2 + I1 + I2 + 2 · m2 · L1 · lg2 · cos θ2 + Jm1 (23)
H12 = m 2 · lg 2 2 + I 2 + m 2 ·L 1 · lg 2 · cos θ 2 (24)
H21 = m2 · lg2 2 + I2 + m2 · L1 · lg2 · cos θ2 (25)
H22 = m2 · lg2 2 + I2 + Jm2 (26)
H12=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(24)
H21=m2・lg22+I2+m2・L1・lg2・cosθ2 ・・・(25)
H22=m2・lg22+I2+Jm2 ・・・(26) H11 = m1 · lg1 2 + m2 · L1 2 + m2 · lg2 2 + I1 + I2 + 2 · m2 · L1 · lg2 · cos θ2 + Jm1 (23)
H12 = m 2 · lg 2 2 + I 2 + m 2 ·
H21 = m2 · lg2 2 + I2 + m2 · L1 · lg2 · cos θ2 (25)
H22 = m2 · lg2 2 + I2 + Jm2 (26)
また、粘性行列D11,D22は式(27)、(28)で表わされる。
Also, the viscosity matrices D11 and D22 are expressed by the equations (27) and (28).
D11=d1 ・・・(27)
D22=d2 ・・・(28) D11 = d1 (27)
D22 = d2 (28)
D22=d2 ・・・(28) D11 = d1 (27)
D22 = d2 (28)
また、速度依存項トルクであるb1,b2はそれぞれ式(29)、(30)で表わされる。
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・ω12・sinθ2 ・・・(30) b1 = −m2 · L1 · lg2 · (2 · ω1 + ω2) · ω2 · sinθ2 (29)
b2 = m2 · L1 · lg2 · ω1 2 · sin θ2 (30)
b2=m2・L1・lg2・ω12・sinθ2 ・・・(30) b1 = −m2 · L1 · lg2 · (2 · ω1 + ω2) · ω2 · sinθ2 (29)
b2 = m2 · L1 · lg2 · ω1 2 · sin θ2 (30)
また、重力項トルクに対応するc1,c2はそれぞれ式(31)、(32)で表わされる。
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) c1 = (m1 · g · lg1 + m2 · g · L1) cos θ1 + m2 · g · lg2 · cos (θ1 + θ2) (31)
c2 = m2 · g · lg2 · cos (θ1 + θ2) (32)
c2=m2・g・lg2・cos(θ1+θ2) ・・・(32) c1 = (m1 · g · lg1 + m2 · g · L1) cos θ1 + m2 · g · lg2 · cos (θ1 + θ2) (31)
c2 = m2 · g · lg2 · cos (θ1 + θ2) (32)
ここで、式(21)~(32)における各符号の内容は以下の通りである。
Here, the contents of each symbol in the equations (21) to (32) are as follows.
θ1:時刻tにおける第1軸の回転角(紙面における左回りを正方向とする。)
θ2:時刻tにおける第2軸の回転角(紙面における左回りを正方向とする。)
ω1:時刻tにおける第1軸の回転速度
ω2:時刻tにおける第2軸の回転速度
α1:時刻tにおける第1軸の回転加速度
α2:時刻tにおける第2軸の回転加速度
m1:第1軸のリンクの重心質量
m2:第2軸のリンクの重心質量、なお、m2にはリンク先端の負荷の質量が含まれる。 θ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.
θ2:時刻tにおける第2軸の回転角(紙面における左回りを正方向とする。)
ω1:時刻tにおける第1軸の回転速度
ω2:時刻tにおける第2軸の回転速度
α1:時刻tにおける第1軸の回転加速度
α2:時刻tにおける第2軸の回転加速度
m1:第1軸のリンクの重心質量
m2:第2軸のリンクの重心質量、なお、m2にはリンク先端の負荷の質量が含まれる。 θ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:第1軸の回転中心から重心位置までの距離
lg2:第2軸の回転中心から重心位置までの距離
L1:第1軸の回転中心から第2軸の回転中心までの距離
I1:第1軸の、重心位置での回転軸回りの主慣性モーメント
I2:第2軸の、重心位置での回転軸回りの主慣性モーメント
Jm1:第1軸のモータイナーシャのアーム回転軸換算値
Jm2:第2軸のモータイナーシャのアーム回転軸換算値
d1:第1軸の粘性摩擦係数
d2:第2軸の粘性摩擦係数
g:重力加速度
である。 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.
lg2:第2軸の回転中心から重心位置までの距離
L1:第1軸の回転中心から第2軸の回転中心までの距離
I1:第1軸の、重心位置での回転軸回りの主慣性モーメント
I2:第2軸の、重心位置での回転軸回りの主慣性モーメント
Jm1:第1軸のモータイナーシャのアーム回転軸換算値
Jm2:第2軸のモータイナーシャのアーム回転軸換算値
d1:第1軸の粘性摩擦係数
d2:第2軸の粘性摩擦係数
g:重力加速度
である。 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:補正係数の算出>
図9は、j軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す。また、図9は、補正係数γを導出するための考え方を示す模式図でもある。 <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 γ.
図9は、j軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す。また、図9は、補正係数γを導出するための考え方を示す模式図でもある。 <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 γ.
前述したように、補正処理部22は、動力学トルク算出部21で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルク(速度依存項トルク、重力項トルク)に基づいて第1及び第2補正係数γa,γbを算出し出力する。
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.
具体的な第1及び第2補正係数γa,γbの算出手順について以下に説明する。なお、j軸の出力可能な最大トルクを許容最大トルクτjmaxとする。また、本実施形態では、時刻ta,tbの前後における重力項トルクと速度依存項トルクの変化量は、慣性項トルクの変化量に比べて十分に小さいものとする。
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.
まず、j軸に関する許容最大トルクτjmaxと、時刻taにおけるj軸の残余トルクτcapjとを用いて、式(33)に示すトルクτarjを算出する。
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)
Τarj = τjmax-τcapj (33)
なお、許容最大トルτjmaxの符号は時刻taにおける動力学トルクτj(ta)と同符号に合わせる。
Note that the sign of the allowable maximum torque τjmax is matched with the sign of the dynamic torque τj (ta) at time ta. *
次に、動力学トルク算出部21で算出された時刻taにおけるj軸の慣性項トルクτapjとトルクτarjを用いて、j軸に関する補正係数γajを算出する。
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)
Γaj = τapj / τarj (34)
同様に、j軸に関する許容最大トルクτjmaxと、時刻tbにおけるj軸の残余トルクτcbpjとを用いて、式(35)に示すトルクτbrjを算出する。
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)
Τbrj = τjmax−τcbpj (35)
なお、許容最大トルτjmaxの符号は時刻tbにおける動力学トルクτj(tb)と同符号に合わせる。
The sign of the allowable maximum torque τjmax is set to the same sign as the dynamic torque τj (tb) at time tb. *
次に、動力学トルク算出部21で算出された時刻tbにおけるj軸の慣性項トルクτbpjとトルクτbrjを用いて、j軸に関する補正係数γbjを計算する。
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)
Γbj = τbpj / τbrj (36)
ここで、式(33)に示すトルクτarj及び式(35)中のトルクτbrjは、j軸における最大加速度での加減速回転動作を行う場合に、j軸の許容最大トルクτjmaxのうち利用可能なトルク(以下、利用可能トルクと呼ぶことがある。)に相当する。また、慣性項トルクτapj及びτbjは、j軸における最大加速度での加減速回転動作を実現するのに必要なトルクである。
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.
従って、式(34),(36)に示すγaj,γbjが1以下であれば、j軸は所定の最大加速度で加減速回転動作が可能となる。一方、γaj,γbjが1よりも大きければ、j軸は所望の動作を実現するのに必要なトルクが足らないことを意味する。
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.
次に、ロボットアーム6の全関節軸について、式(33)~(36)により補正係数をそれぞれ算出し、加速動作中に得られた補正係数の中の最大のものを選んで、第1補正係数γaとする。また、減速動作中に得られた補正係数の中の最大のものを選んで、第2補正係数γbとする。なお、以降の説明において、γa,γbを総称して補正係数γと呼ぶことがある。
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) γa = max {γaj} (37)
γb = max {γbj} (38)
γb=max{γbj} ・・・(38) γa = max {γaj} (37)
γb = max {γbj} (38)
<STEP5:加減速時間の補正>
演算部23は、加減速時間算出部9で算出された加速時間Taに対して、補正処理部22で算出された第1補正係数γaを乗じて、補正後の加速時間Ta’を求め、加減速処理部4に出力する。また、演算部23は、加減速時間算出部9で算出された減速時間Tbに対して、補正処理部22で算出された第2補正係数γbを乗じて、補正後の減速時間Tb’を求め、加減速処理部4に出力する。なお、Ta’、Tb’はそれぞれ式(39)、(40)で表わされる。 <STEP5: Correction of acceleration / deceleration time>
Thecalculation 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.
演算部23は、加減速時間算出部9で算出された加速時間Taに対して、補正処理部22で算出された第1補正係数γaを乗じて、補正後の加速時間Ta’を求め、加減速処理部4に出力する。また、演算部23は、加減速時間算出部9で算出された減速時間Tbに対して、補正処理部22で算出された第2補正係数γbを乗じて、補正後の減速時間Tb’を求め、加減速処理部4に出力する。なお、Ta’、Tb’はそれぞれ式(39)、(40)で表わされる。 <STEP5: Correction of acceleration / deceleration time>
The
Ta’=Ta・γa ・・・(39)
Tb’=Tb・γb ・・・(40) Ta ′ = Ta · γa (39)
Tb ′ = Tb · γb (40)
Tb’=Tb・γb ・・・(40) Ta ′ = Ta · γa (39)
Tb ′ = Tb · γb (40)
式(33)~(40)より明らかなように、γa,γb≦1の場合は、いずれの関節軸においてもトルク不足は生じていないことを意味する。この場合は、第1及び第2補正係数γa,γbを乗じて得る補正後の加速時間Ta’及び減速時間Tb’は当初の加速時間Ta及び減速時間Tbと同じである。
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.
一方、γa,γb>1の場合は、の場合は、いずれかの関節軸においてトルク不足が生じていることを意味する。そこで、当初の加速時間Taをγa(>1)倍した新たな加速時間Ta’(>Ta)で関節軸を回転させる。これは、関節軸の当初の加速度を1/γa倍に下げる、すなわち、慣性項トルクを1/γa倍に下げるということである。従って、第1補正係数γaにより補正されたj軸の動力学トルクτjは、その許容最大トルクτjmaxを超えることはない。同様に、第2補正係数γbにより補正されたj軸の動力学トルクτjは、その許容最大トルクτjmaxを超えることはない。
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.
加減速処理部4は、この補正後の加速時間Ta’または減速時間Tb’を用いて、移動指令生成部3から出力された移動指令列に対して上述した加減速処理を行う。
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 ′.
[効果等]
以上説明したように、本実施形態におけるロボット制御装置10は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアーム6の動作を制御する。 [Effects]
As described above, therobot 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.
以上説明したように、本実施形態におけるロボット制御装置10は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアーム6の動作を制御する。 [Effects]
As described above, the
また、ロボット制御装置10は、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部21と、各関節軸の回転加速度が極大及び極小となる時点ta,tbでの各関節軸における関節角度の移動量Δθap,Δθbpと回転速度ωap,ωbpと回転加速度の極大値αap及び極小値αbpとが各関節軸を加減速動作させるための加減速時間Ta,Tbに関連付けられて保存されたテーブル24と、これが格納された記憶部20と、を少なくとも備えている。
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.
動力学トルク算出部21は、テーブル24内の値に基づいて、回転加速度の極大値αap及び極小値αbpに対応する動力学トルクを算出するとともに、回転加速度の極大値αap及び極小値αbpに対応する慣性項トルクτap,τbpと、動力学トルクから慣性項トルクを差し引いた残余トルクτcap,τcbpとを算出し、各関節軸のうち少なくとも1つの関節軸において、慣性項トルクτapが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク(τmax-τcap)よりも大きいか、慣性項トルクτbpが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク(τmax-τcbp)よりも大きい場合は、各関節軸の回転動作における共通の加減速時間TaまたはTbを補正する。
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.
本実施形態によれば、多関節ロボットにおいて、各関節軸での加減速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、加速時間Taまたは減速時間Tbを補正して、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。
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.
また、ロボット制御装置10は、ロボットアーム6における隣り合う作業点間距離Lと目標移動速度Vとに基づいて、各関節軸に対する移動指令Δθをそれぞれ生成して出力する移動指令生成部3と、一の関節軸であるj軸が出力可能な許容最大トルクτjmaxから残余トルクτcapまたはτcbpを差し引いた成分である利用可能トルクと、慣性項トルクτap,τbpとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸におけるトルク比の中の最大値を補正係数γとして出力する補正処理部22と、を備えている。また、ロボット制御装置10は、補正係数γが1以下の場合は、予め設定された加速時間Taまたは減速時間Tbに基づいて移動指令Δθを加減速処理し、補正係数γが1を超える場合は、加速時間Taまたは減速時間Tbに補正係数γを乗じて補正された加速時間Ta’または減速時間Tb’に基づいて移動指令Δθを加減速処理する加減速処理部4と、を備えている。
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 γ.
加減速処理部4で加減速処理された移動指令Δθに基づいて各関節軸を回転駆動させてロボットアーム6の動作を制御する。
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.
本実施形態によれば、多関節ロボットにおいて、各関節軸での加速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、不足分に応じた値である第1補正係数γa(>1)で加速時間Taを補正して、加速時間Taを当初の値よりも大きな時間Ta’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。同様に、トルクの不足分に応じた値である第2補正係数γb(>1)で減速時間Tbを補正して、減速時間Tbを当初の値よりも大きな時間Tb’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を減速して、ロボットアーム6の動作を制御することができる。このことにより、ロボットの動作プログラム1の教示修正の手間を省くことができる。
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.
また、実際の加速度が極大または極小となる時刻、つまり、関節軸での加減速動作が最も急激に行われる時刻での動力学トルクに基づいて、加速時間Ta及び/または減速時間Tbを補正するため、ロボットアーム6を安定して動作させつつ、ロボットアーム6の動作時間であるタクトが増加するのを抑えることができる。
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.
テーブル24に保存された値は、移動指令に基づいて算出されるようにしてもよい。
The value stored in the table 24 may be calculated based on the movement command.
このようにすることで、テーブル24の作成作業が簡素化されるとともに、作成時間を短縮することができる。
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.
1 動作プログラム
2 処理部
3 移動指令生成部
4 加減速処理部
5 サーボ
6 ロボットアーム
7 移動時間算出部
8 各軸速度算出部
9 加減速時間算出部
10 ロボット制御装置
20 記憶部
21 動力学トルク算出部
22 補正処理部
23 演算部
24 テーブル DESCRIPTION OFSYMBOLS 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
2 処理部
3 移動指令生成部
4 加減速処理部
5 サーボ
6 ロボットアーム
7 移動時間算出部
8 各軸速度算出部
9 加減速時間算出部
10 ロボット制御装置
20 記憶部
21 動力学トルク算出部
22 補正処理部
23 演算部
24 テーブル DESCRIPTION OF
Claims (3)
- モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、
各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、
各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、
前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、
各関節軸のうち少なくとも1つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とするロボット制御装置。 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. - 請求項1に記載のロボット制御装置において、
前記ロボットアームにおける隣り合う作業点間距離と目標移動速度とに基づいて、各関節軸に対する移動指令をそれぞれ生成して出力する移動指令生成部と、
一の前記関節軸が出力可能な許容最大トルクから前記残余トルクを差し引いた成分である前記利用可能トルクと、前記慣性項トルクとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸における前記トルク比の中の最大値を補正係数として出力する補正処理部と、
前記補正係数が1以下の場合は、予め設定された前記加減速時間に基づいて前記移動指令を加減速処理し、前記補正係数が1を超える場合は、前記加減速時間に前記補正係数を乗じて補正された加減速時間に基づいて前記移動指令を加減速処理する加減速処理部と、をさらに備え、
前記加減速処理部で加減速処理された前記移動指令に基づいて各関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。 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. - 請求項1または2に記載のロボット制御装置において、
前記テーブルに保存された値は、移動指令に基づいて算出されることを特徴とするロボット制御装置。 The robot control device according to claim 1 or 2,
The value stored in the table is calculated based on a movement command.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020518145A JP7126222B2 (en) | 2018-05-10 | 2019-02-14 | robot controller |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018-091668 | 2018-05-10 | ||
JP2018091668 | 2018-05-10 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2019215980A1 true WO2019215980A1 (en) | 2019-11-14 |
Family
ID=68467986
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2019/005302 WO2019215980A1 (en) | 2018-05-10 | 2019-02-14 | Robot control device |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP7126222B2 (en) |
WO (1) | WO2019215980A1 (en) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0580833A (en) * | 1991-09-18 | 1993-04-02 | Mitsubishi Electric Corp | Controller for industrial robot |
JPH07200033A (en) * | 1993-12-28 | 1995-08-04 | Mitsubishi Electric Corp | Robot controller |
JPH11259108A (en) * | 1998-03-13 | 1999-09-24 | Mitsubishi Electric Corp | Robot controller |
JP2007279899A (en) * | 2006-04-04 | 2007-10-25 | Mitsubishi Electric Corp | Numerical control apparatus |
JP2009028851A (en) * | 2007-07-27 | 2009-02-12 | Nachi Fujikoshi Corp | Robot control device |
-
2019
- 2019-02-14 JP JP2020518145A patent/JP7126222B2/en active Active
- 2019-02-14 WO PCT/JP2019/005302 patent/WO2019215980A1/en active Application Filing
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0580833A (en) * | 1991-09-18 | 1993-04-02 | Mitsubishi Electric Corp | Controller for industrial robot |
JPH07200033A (en) * | 1993-12-28 | 1995-08-04 | Mitsubishi Electric Corp | Robot controller |
JPH11259108A (en) * | 1998-03-13 | 1999-09-24 | Mitsubishi Electric Corp | Robot controller |
JP2007279899A (en) * | 2006-04-04 | 2007-10-25 | Mitsubishi Electric Corp | Numerical control apparatus |
JP2009028851A (en) * | 2007-07-27 | 2009-02-12 | Nachi Fujikoshi Corp | Robot control device |
Also Published As
Publication number | Publication date |
---|---|
JPWO2019215980A1 (en) | 2021-05-20 |
JP7126222B2 (en) | 2022-08-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7199178B2 (en) | ROBOT CONTROL DEVICE, ROBOT DEVICE, ROBOT CONTROL PARAMETER ADJUSTMENT METHOD, AND PROGRAM | |
JP4281696B2 (en) | Acceleration / deceleration control method and apparatus, and acceleration / deceleration control method program | |
JP3900789B2 (en) | Motor speed / acceleration determination method, acceleration / deceleration generation method, acceleration / deceleration control method, acceleration / deceleration control device, and motor control device | |
US9764471B2 (en) | Trajectory generation apparatus for robot to generate trajectory including curved portion | |
CN105619424B (en) | Control device and method, the control equipment of multi-axle motor of motor driving apparatus | |
US9505074B2 (en) | Weaving control device for articulated robot | |
US20150051735A1 (en) | Control apparatus of robot, robot, and program thereof | |
JP7170181B2 (en) | robot controller | |
JPH10329063A (en) | Robot control device | |
JP4192780B2 (en) | Robot control device | |
JP4498061B2 (en) | Welding robot controller | |
WO2019215980A1 (en) | Robot control device | |
JP2005052913A (en) | Robot controller | |
JP2020015124A (en) | Robot control method, article manufacturing method, robot control device, robot, program and recording medium | |
JPWO2018061283A1 (en) | Robot control method and welding method | |
WO2019012942A1 (en) | Robot control device | |
JP2016005296A (en) | Control method of motor drive device, motor drive device and robot device | |
JP5436930B2 (en) | S-curve acceleration / deceleration trajectory generation method and articulated robot system | |
JP2016177513A (en) | Tandem position control device | |
JPH0276692A (en) | Robot adaptation control method | |
JP6915470B2 (en) | Robot control device | |
JP2012061560A (en) | Robot control device | |
JP3194829B2 (en) | Robot motion program generation device | |
JP6926882B2 (en) | Robot control device | |
JPH07295651A (en) | Acceleration and deceleration controller for robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 19799997 Country of ref document: EP Kind code of ref document: A1 |
|
ENP | Entry into the national phase |
Ref document number: 2020518145 Country of ref document: JP Kind code of ref document: A |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 19799997 Country of ref document: EP Kind code of ref document: A1 |