Robot control method and robot control device

Abstract

Stop and start detection block (63) determines whether or not a joint portion is in a stopped state before a rotation direction of the joint portion is inverted based on stop flag signal (Stop_Flg). When it is determined that the joint portion is in the stopped state, filter processing block (65) changes a frequency component of a correction amount for correcting backlash to a low frequency lower than a predetermined threshold value.

Claims

1. A robot control device for controlling an operation of a robot arm having a plurality of joint portions, the device comprising: a determination unit that receives a stop flag signal and determines whether or not a joint portion to be corrected among the plurality of joint portions is in a stopped state before a rotation direction of the joint portion to be corrected is inverted based on the stop flag signal; and a frequency changing unit that changes a frequency component of a correction amount for correcting backlash at the joint portion to be corrected to a low frequency lower than a predetermined threshold value when it is determined that the joint portion to be corrected is in the stopped state.

2. The robot control device according to claim 1, wherein the determination unit determines whether or not the joint portion to be corrected among the plurality of joint portions is in a middle of an operation before the rotation direction of the joint portion to be corrected is inverted, and the frequency changing unit changes the frequency component of the correction amount to a high frequency higher than the low frequency when it is determined that the joint portion to be corrected is in the middle of the operation.

3. A robot control method, of a robot control device that comprises a processor, for controlling an operation of a robot arm having a plurality of joint portions, the method comprising; receiving, by the processor, a stop flag signal; determining, by the processor, based on the stop flag signal, that a joint portion to be corrected among the plurality of joint portions is in a stopped state before a rotation direction of the joint portion be corrected is inverted; changing, by the processor, a frequency component of a correction amount for correcting backlash at the joint portion to be corrected to a low frequency lower than a predetermined threshold value based on the determination.

4. The robot control method of claim 3, further comprising: determining, by the processor, that the joint portion to be corrected among the plurality of joint portions is in a middle of an operation before the rotation direction of the joint portion to be corrected is inverted; and changing, by the processor, the frequency component of the correction amount to a high frequency higher than the low frequency when it is determined that the joint portion to be corrected is in the middle of the operation.

5. The robot control method of claim 4, further comprising: inverting, by the processor, the rotation direction of the joint portion based on the correction amount.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a diagram illustrating a configuration of a robot according to the present exemplary embodiment.

(2) FIG. 2 is a diagram illustrating a trajectory error.

(3) FIG. 3 is a diagram when the trajectory error is corrected by a backlash correction amount.

(4) FIG. 4 is a diagram illustrating an actual trajectory after correction.

(5) FIG. 5 is a graph showing a change in the backlash correction amount.

(6) FIG. 6 is a functional block diagram related to position control of the robot.

(7) FIG. 7 is a diagram illustrating torsion of a reduction gear in a robot arm.

(8) FIG. 8 is a block diagram of a robot control device.

(9) FIG. 9 is a block diagram of a backlash correction block.

(10) FIG. 10A is a diagram illustrating an actual trajectory in a case where backlash correction is not performed when a joint portion is inverted from a stopped state.

(11) FIG. 10B is a graph showing a change in a turning shaft speed.

(12) FIG. 10C is a graph showing the change in the backlash correction amount.

(13) FIG. 11A is a diagram illustrating the actual trajectory in a case where the backlash correction amount is steeply changed when the joint portion is inverted from the stopped state.

(14) FIG. 11B is a graph showing the change in the turning shaft speed.

(15) FIG. 11C is a graph showing the change in the backlash correction amount.

(16) FIG. 12A is a diagram illustrating the actual trajectory in a case where the backlash correction amount is gently changed when the joint portion is inverted from the stopped state.

(17) FIG. 12B is a graph showing the change in the turning shaft speed.

(18) FIG. 12C is a graph showing the change in the backlash correction amount.

(19) FIG. 13A is a timing chart illustrating switching between an inversion timing, a stop flag signal, a start edge signal, and a filter switching signal.

(20) FIG. 13B is a graph showing a change in a speed command component.

(21) FIG. 13C is a graph showing the change in the backlash correction amount.

(22) FIG. 14A is a timing chart illustrating switching between the inversion timing, the stop flag signal, the start edge signal, and the filter switching signal.

(23) FIG. 14B is a graph showing the change in the speed command component.

(24) FIG. 14C is a graph showing the change in the backlash correction amount.

DESCRIPTION OF EMBODIMENT

(25) Hereinafter, an exemplary embodiment of the present disclosure is described with reference to the drawings. Note that the following description of the preferred exemplary embodiment is merely exemplary in nature, and is not intended to limit the present disclosure, its application, or its use.

(26) As illustrated in FIG. 1, vertical articulated robot 1 includes robot arm 2 and a plurality of joint portions J1 to J6. Robot control device 20 is connected to robot 1.

(27) Robot arm 2 is divided into a plurality of portions, and joint portions J1 to J6 are provided at coupling portions of the portions. Servomotors 12 are coupled to joint portions J1 to J6 via reduction gears 13 (see FIG. 6).

(28) Servomotor 12 is driven based on position command c from robot control device 20 to rotate each of joint portions J1 to J6 by a desired amount, and an operation and a posture of robot arm 2 are controlled.

(29) Here, joint portions J1 to J3 are three main shafts that determine the overall posture of robot arm 2, and joint portions J4 to J6 are three wrist shafts that determine a direction of a distal end of robot arm 2. Note that joint portion J1 is a turning shaft for turning robot arm 2.

(30) Hereinafter, a case where a laser output device (not illustrated) is attached to the distal end of robot arm 2 and a work of cutting a workpiece is performed by irradiation with laser light will be described.

(31) In the example illustrated in FIG. 2, laser cutting is performed in a circular orbit in a counterclockwise direction from a position of 0. Here, as will be described later in FIG. 6, upon receiving an instruction from operation and teaching unit 21, a rotation direction of motor 12 of joint portion J1 is inverted at a timing when the distal end of robot arm 2 reaches positions of 90 and 270 in FIG. 2. Thus, backlash errors occur at the positions of 90 and 270, respectively.

(32) Specifically, an actual trajectory is a trajectory in which a portion on a right side of an inversion position in FIG. 2, that is, a semicircular portion from the position of 270 to the position of 90 is shifted downward without changing a diameter itself.

(33) A portion on a left side of the inversion position in FIG. 2, that is, a semicircular portion from the position of 90 to the position of 270 is a trajectory shifted upward without changing the diameter itself.

(34) As described above, the actual trajectory is a trajectory in which the right side of the reverse position is shifted downward and the left side of the reverse position is shifted upward in FIG. 2.

(35) Thus, as illustrated in FIG. 3, correction is performed such that the actual trajectory approaches a target trajectory by adding or subtracting a backlash correction amount to or from the position command. Specifically, the position command is corrected to a position command shifted upward in FIG. 3 by adding the backlash correction amount to the position command from the position of 0 to the position of 90 and from the position of 270 to the position of 360 (0).

(36) On the other hand, the position command is corrected to a position command shifted downward in FIG. 3 by subtracting a correction amount (1BL) from the position command from the position 90 to the position of 270.

(37) As described above, it is possible to perform correction such that the actual trajectory of robot arm 2 becomes a circular orbit by providing a correction addition command as illustrated in FIG. 4.

(38) Here, the backlash correction amount is a value determined in advance by an experiment or the like. For example, a waveform illustrated in FIG. 5 is output as the backlash correction amount. In the example shown in FIG. 5, while the distal end of robot arm 2 is moved in a circular orbit, as will be described later in FIG. 6, upon receiving an instruction from operation and teaching unit 21, servomotor 12 of joint portion J1 is inverted without stopping. Thus, when servomotor 12 is inverted from a non-stopped state, the correction amount is steeply changed such that a timing of the backlash correction is not delayed.

(39) <Configuration of Articulated Robot and Control System Thereof>

(40) FIG. 6 is a functional block diagram related to position control of the robot, and schematically illustrates internal configurations of robot mechanism 11 and robot control device 20.

(41) As illustrated in FIG. 6, robot mechanism 11 is a mechanical drive unit of robot 1, and includes servomotors 12 (hereinafter, simply referred to as motors), reduction gears 13, and encoders 14. Although not illustrated, robot arm 2 is included.

(42) Motors 12 are coupled to joint portions J1 to J6 of robot 1 via reduction gear 13, and drives joint portions J1 to J6 by control signals sent from servo controllers 23 of robot control device 20 to control the operation and posture of robot arm 2. In robot 1, joint portions J1 to J6 can be operated to draw a desired orbit such as a circle or an ellipse at the distal end of robot arm 2.

(43) Encoder 14 is connected to motor 12, detects a rotation amount and a rotational speed thereof, and sends, as a feedback signal, a detection signal to servo controller 23.

(44) Note that, in the following description, motor 12, reduction gear 13, and encoder 14 coupled to joint portion J1 may be referred to as a first motor, a first reduction gear, and a first encoder, respectively, and the motors and the like coupled to joint portions J2 to J6 may be referred to as second to sixth motors and the like, respectively.

(45) Servo controller 23 and backlash correction block 25 connected to the first motor may be referred to as a first servo controller and a first backlash correction block, respectively, and servo controllers 23 connected to second to sixth motors 12 and backlash correction blocks 25 connected to second to sixth servo controllers 23 may be referred to as second to sixth servo controllers and second to sixth backlash correction blocks, respectively.

(46) The position commands and position command correction values sent to the shafts may be referred to as position commands 1c to 6c and position command correction values 1BL to 6BL.

(47) Robot control device 20 includes operation and teaching unit 21, main controller 22, servo controllers 23, and backlash correction blocks 25.

(48) Operation and teaching unit 21 stores, for example, the trajectory of robot arm 2 acquired at the time of teaching, a rotational operation of each motor 12 for drawing the trajectory, and the like.

(49) In response to an instruction from operation and teaching unit 21, main controller 22 outputs position commands 1c to 6c of shafts J1 to J6 of robot 1 according to the trajectory of the movement of robot arm 2 and the like of robot mechanism 11 stored in operation and teaching unit 21.

(50) Servo controller 23 controls the rotational operation of each of first to sixth motors 12 in robot mechanism 11 to follow position commands 1c to 6c sent from main controller 22.

(51) Backlash correction blocks 25 are provided between main controller 22 and servo controllers 23 to correspond to joint portions J1 to J6. Backlash correction blocks 25 generate position command correction values 1BL to 6BL based on position commands 1c to 6c received from main controller 22. Generated position command correction values 1BL to 6BL are added to corresponding position commands 1c to 6c and are sent to first to sixth servo controllers 23, respectively. Stop flag signals (Stop_Flg) to be described later are input to backlash correction blocks 25.

(52) Note that the functional blocks in robot control device 20 may be constituted by independent circuits or may be constituted by one integrated circuit. A combination of some functional blocks may be constituted by one integrated circuit.

(53) Functions of main controller 22, servo controller 23, and backlash correction block 25 are substantially realized by executing a program described in software on an integrated circuit such as a CPU.

(54) FIG. 7 is a diagram in which motors 12, reduction gears 13, and a part of robot arm 2 coupled thereto are extracted, as load 30, from robot mechanism 11.

(55) Load 30 includes first arm 31 serving as a motor attachment base, motor 12 coupled to first arm 31, reduction gear 13 including primary side 32 coupled to motor 12 and secondary side 33 having bearing 34, and second arm 35 rotatably coupled to secondary side 33 of reduction gear 13.

(56) Reduction gear primary side 32 is coupled to rotor 36 of motor 12 via the rotation shaft of motor 12, and rotates by motor rotation position M sent from servo controller 23. Reduction gear 13 converts motor rotation position M into arm rotation position L at reduction ratio Rg. Reduction gear 13 has spring component 37 between reduction gear primary side 32 and reduction gear secondary side 33.

(57) Note that position command c indicates an angle command indicating a rotation angle amount of each shaft, and actual position L indicates a rotation angle amount at which each shaft actually rotates.

(58) <Detailed Configuration of Control Block of Articulated Robot>

(59) FIG. 8 is a block diagram of the first servo controller. As illustrated in FIG. 8, position control block 50 of servo controller 23 generates speed command cp by subtracting motor position M from a value obtained by adding position command c and backlash correction value 1BL output from backlash correction block 25 and multiplying the difference by position proportional gain Kpp. Motor position M is obtained from a detection signal in first encoder 14 that is a position detector.

(60) Note that, in the following description, although a configuration and the like of first servo controller 23 will be described as an example, the same applies to second to sixth servo controllers 23.

(61) In speed control block 51, current IM flowing to first motor 12 is calculated by adding a value obtained by subtracting, from speed command cp, motor speed M obtained by differentiating motor position M by speed proportional gain Kps and a value obtained by integrating the difference value and multiplying the integrated value by integrated speed gain Ki. Current IM is input to load 30.

(62) In load 30, IM is a motor current command for driving motor 12, Kt is a torque constant of motor 12, 1/Rg is a reciprocal of the reduction ratio, Ks is a spring constant of reduction gear 13, s is a torsion amount generated between reduction gear primary side 32 and reduction gear secondary side 33, and ddyn is an external force torque applied to robot arm 2.

(63) Motor transfer function 40 and load transfer function 41 are obtained by formulating (modeling) physical phenomena.

(64) In motor transfer function 40, JM is the moment of inertia about a rotation shaft of a combination of rotor 36 of motor 12 and reduction gear primary side 32, and DM is a viscous friction coefficient.

(65) In load transfer function 41, JL is the moment of inertia about a rotation shaft of a combination of second arm 35 and reduction gear secondary side 33, and DL is a viscous friction coefficient.

(66) Note that FIG. 8 is a general control block diagram of the motor to which the load and the reduction gear are coupled, and thus, detailed description of functions other than the above functions will be omitted.

(67) FIG. 9 illustrates a detailed configuration of the backlash correction block. Backlash correction block 25 includes angular velocity calculation block 60, command speed component inversion detection block 61, backlash correction amount calculation block 62, stop and start detection block 63, filter switching timer block 64, and filter processing block 65.

(68) Position command 1c of joint portion J1 to be corrected is input to angular velocity calculation block 60. Angular velocity calculation block 60 calculates angular velocity component dc that is a differential value of position command 1c. Angular velocity component dc is input to command speed component inversion detection block 61.

(69) Command speed component inversion detection block 61 calculates inversion timing dir based on angular velocity component dc. Inversion timing dir is input to backlash correction amount calculation block 62.

(70) Backlash correction amount calculation block 62 calculates correction amount 1BL0 of joint portion J1 based on inversion timing dir. Correction amount 1BL0 is input to filter processing block 65.

(71) A stop flag signal (Stop_Flg) is input to stop and start detection block 63. Stop flag signal (Stop_Flg) is a signal indicating whether or not robot 1 is in a stopped state or a non-stopped state (operation state). Specifically, stop flag signal (Stop_Flg) is 1 when joint portion J1 is in the stopped state, and is 0 when the joint portion is in the non-stopped state.

(72) Stop and start detection block 63 outputs start edge signal (Start_Edg) based on stop flag signal (Stop_Flg). Start edge signal (Start_Edg) is a signal indicating whether a rotation direction of joint portion J1 is inverted from the stopped state or is inverted from the non-stopped state.

(73) Specifically, start edge signal (Start_Edg) is 1 during one sample when stop flag signal (Stop_Flg) changes from 1 to 0, and is 0 when stop flag signal (Stop_Flg) does not change. Start edge signal (Start_Edg) is input to filter switching timer block 64.

(74) Filter switching timer block 64 outputs filter switching signal (Flt_Chng) based on start edge signal (Start_Edg). Filter switching signal (Flt_Chng) is a signal indicating which of a low frequency filter and a high frequency filter is selected in order to change a frequency component of the correction amount.

(75) Specifically, filter switching signal (Flt_Chng) is 1 when the low frequency filter is selected, and 0 when the high frequency filter is selected. Filter switching signal (Flt_Chng) is input to filter processing block 65.

(76) Here, the low frequency is a frequency lower than a predetermined threshold value. The high frequency is a frequency higher than the low frequency. In the present exemplary embodiment, a resonance frequency (for example, 10 Hz) of joint portion J1 is set as a threshold value, the low frequency is set to 5 Hz, and the high frequency is set to 50 Hz.

(77) Filter processing block 65 changes the frequency component of the correction amount based on correction amount 1BL0 and filter switching signal (Flt_Chng).

(78) Specifically, when the high frequency filter (for example, 50 Hz) is selected based on filter switching signal (Flt_Chng), the correction amount changes steeply (see FIG. 11C).

(79) On the other hand, when the low frequency filter (for example, 5 Hz) is selected, the correction amount changes gradually (see FIG. 12C).

(80) Filter processing block 65 outputs correction amount 1BL of which the frequency component is changed.

(81) A new position command obtained by adding correction amount 1BL calculated by backlash correction block 25 to position command 1c sent from main controller 22 to first servo controller 23 is input to first servo controller 23, and the driving of first motor 12 is controlled by first servo controller 23.

(82) Hereinafter, the reason why the frequency component of the correction amount changes by determining whether joint portion J1 is in the non-stopped state or the stopped state before joint portion J1 is inverted will be described.

(83) In the example shown in FIG. 10A, robot arm 2 stops at a position where a distal end portion of the robot arm is extended to a position of 1300 mm in an X direction (see FIG. 1). A trajectory command is given to joint portion J1 to degenerate the joint portion to a position of 1200 mm in the X direction.

(84) As illustrated in FIG. 10B, the rotational speed of joint portion J1 which is the turning shaft changes such that the speed gradually increases from the stopped state. At this time, the backlash correction of joint portion J1 is not performed (see FIG. 10C).

(85) Thus, as illustrated in FIG. 10A, when joint portion J1 is inverted from the stopped state, the actual trajectory of the distal end portion of robot arm 2 is greatly shifted in a Y direction (see FIG. 1) due to the backlash error, and then changes gradually according to the trajectory command.

(86) Subsequently, in the example illustrated in FIG. 11A, when joint portion J1 is inverted from the stopped state, the backlash correction amount is steeply changed (see FIG. 11C). Thus, as illustrated in FIG. 11B, the rotational speed of joint portion J1 is steeply increased from the stopped state and then decreased again, and thereafter, the rotational speed is gradually increased and then changed to a constant speed.

(87) Thus, as illustrated in FIG. 11A, when joint portion J1 is inverted from the stopped state, the actual trajectory of the distal end portion of robot arm 2 is shifted in the Y direction, and then changes gradually according to the trajectory command.

(88) Subsequently, in the example illustrated in FIG. 12A, when joint portion J1 is inverted from the stopped state, the backlash correction amount is gently changed (see FIG. 12C). Thus, as illustrated in FIG. 12B, the rotational speed of joint portion J1 is increased from the stopped state and then decreases again, and thereafter, the rotational speed is gradually increased and then changes to a constant speed.

(89) Thus, as illustrated in FIG. 12A, when joint portion J1 is inverted from the stopped state, the actual trajectory of the distal end portion of robot arm 2 is shifted in the Y direction with a smaller variation range than the example illustrated in FIG. 11A, and then changes gradually according to the trajectory command.

(90) As described above, when robot arm 2 is inverted from the stopped state, the backlash correction amount may be gently changed. As a result, it is possible to suppress an increase in the trajectory error due to the vibration of robot arm 2 caused by a large fluctuation in joint portion J1.

(91) On the other hand, when robot arm 2 is inverted from the non-stopped state, the backlash correction amount may be steeply changed such that the timing of the backlash correction is not delayed.

(92) Thus, in the present exemplary embodiment, the frequency component of the correction amount is changed by determining whether joint portion J1 is in the non-stopped state or the stopped state before joint portion J1 is inverted.

(93) FIG. 13A illustrates switching timings of inversion timing dir, stop flag signal (Stop_Flg), start edge signal (Start_Edg), and filter switching signal (Flt_Chng).

(94) As illustrated in FIG. 13B, while joint portion J1 is stopped, stop flag signal (Stop_Flg) becomes 1. When the inversion of joint portion J1 is detected based on inversion timing dir, the filter switching timer is started, and filter switching is performed within a predetermined set time.

(95) At this time, since joint portion J1 is inverted from the stopped state, the low frequency filter is selected within the set time. As illustrated in FIG. 13C, when the frequency component of the correction amount changes to the low frequency (5 Hz), the correction amount changes gently.

(96) When robot arm 2 is moved in the circular orbit, since joint portion J1 is inverted from the non-stopped state, the high frequency filter is selected. As illustrated in FIG. 13C, when the frequency component of the correction amount changes to the high frequency (50 Hz), the correction amount changes steeply.

(97) Next, as another operation example of robot arm 2, a case where joint portion J1 is temporarily stopped before joint portion J1 is inverted will be described.

(98) FIG. 14A illustrates switching timings of inversion timing dir, stop flag signal (Stop_Flg), start edge signal (Start_Edg), and filter switching signal (Flt_Chng).

(99) As illustrated in FIG. 14B, while joint portion J1 is stopped, stop flag signal (Stop_Flg) becomes 1. When the inversion of joint portion J1 is detected based on inversion timing dir, the filter switching timer is started, and filter switching is performed within a predetermined set time.

(100) At this time, since joint portion J1 is inverted from the stopped state, the low frequency filter is selected within a filter switching time. As illustrated in FIG. 14C, when the frequency component of the correction amount changes to the low frequency (5 Hz), the correction amount changes gently.

(101) Robot arm 2 is moved in the circular orbit, and joint portion J1 is temporarily stopped before a next inversion operation. Thus, since joint portion J1 is inverted from the stopped state, the low frequency filter is selected within the filter switching time. As illustrated in FIG. 14C, when the frequency component of the correction amount changes to the low frequency (5 Hz), the correction amount changes gently.

(102) As described above, in accordance with robot control device 20 according to the present exemplary embodiment, when joint portion J1 is inverted in the middle of the operation, the correction amount can be steeply changed by increasing the frequency component of the correction amount such that the timing of the backlash correction is not delayed.

(103) On the other hand, when joint portion J1 is inverted from the stopped state, the correction amount can be gently changed by lowering the frequency component of the correction amount such that the robot arm does not vibrate due to the large fluctuation in joint portion J1.

(104) As a result, the trajectory error of the robot arm can be reduced.

Other Exemplary Embodiments

(105) The above exemplary embodiment may have the following configuration.

(106) In the present exemplary embodiment, although vertical articulated robot 1 having six shafts has been described as an example, the present invention is not particularly limited thereto. The number of joint portions of the robot is not limited to six, and may be appropriately changed according to specifications of the robot or the like.

INDUSTRIAL APPLICABILITY

(107) As described above, the present disclosure is extremely useful and has high industrial applicability since a highly practical effect that the trajectory error of the robot arm can be reduced is obtained.

REFERENCE MARKS IN THE DRAWINGS

(108) 2: robot arm 20: robot control device 63: stop and start detection block (determination unit) 65: filter processing block (frequency changing unit) J1 to J6: joint portion