JP2016147324A - Robot control apparatus, and control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control apparatus and a control method capable of suppressing the travel speed of an arm sufficiently.SOLUTION: A controller 30 is applied to a robot 10 having an arm and including: a plurality of rotary parts 13 to 18; articulates for connecting the rotary parts rotatably relative to each other; and servomotors for driving the individual rotary parts. The controller 30 comprises: angular speed calculation means for calculating an angular speed for driving each servomotor for every operation cycle; selection means for selecting all articulations, which are disposed closer to an arm root side than a monitoring part having the highest velocity in the arm, as speed reducing object shafts, on condition that the maximum one of the speeds of the monitoring parts set at every rotary parts 13 to 18 is higher than the reference speed; and angular speed lowering means for setting the servomotors for rotating the speed reducing object shafts selected, as reduction object motors, and for lowering the angular speeds to drive the reduction object motors, so that the speeds of the individual monitoring parts may be at or lower than the reference speed.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a robot control device and a control method.

  Conventionally, during manual operation of a robot, when the movement speed of the control point of the robot exceeds the reference speed, there is an apparatus that operates the robot by correcting the operation target position so that the movement speed becomes lower than the reference speed (patent) Reference 1).

Japanese Patent No. 3994487

  However, even if the movement speed of the tip of the arm set as the control point of the robot is controlled to be equal to or lower than the reference speed, the movement speed of the arm may not be sufficiently suppressed. Paid attention.

  The present invention has been made in view of such circumstances, and a main object of the present invention is to provide a robot control device and a control method capable of sufficiently suppressing the moving speed of the arm.

  The first means includes a plurality of rotating units, a joint that rotatably connects the rotating units to each other, and an arm including a servo motor that drives each rotating unit, and is installed at a predetermined robot installation location. A robot control device that is applied to a robot that includes a support portion that operably supports the opposite side of the tip of the arm among both ends, and controls the position and posture of the tip by PTP control. The angular velocity calculating means for calculating the angular speed for driving each servo motor for each operation cycle, the monitoring speed calculating means for calculating the speed of the monitoring section set in each rotating section, and the monitoring speed calculating means. In addition, on the condition that the maximum speed among the speeds of each of the monitoring units is higher than the reference speed, the arm is closer to the support unit than the monitoring unit having the maximum speed. Selection means for selecting all the joints selected as speed reduction target axes, and the servo motor for rotating the speed reduction target axis selected by the selection means as a reduction target motor, and the speed of each of the monitoring units is An angular speed reducing means for reducing the angular speed for driving the motor to be reduced so as to be equal to or lower than the reference speed, and a motor other than the motor to be reduced among the servo motors at an angular speed calculated by the angular speed calculating means. Drive means for driving and driving the motor to be reduced at the angular velocity reduced by the angular velocity reduction means.

  In the above configuration, the arm of the robot includes a plurality of rotating units, and the rotating units are connected to each other by joints so as to be rotatable. And the opposite side to the front-end | tip part of an arm among the both ends of an arm is supported by the support part installed in the predetermined robot installation place so that operation | movement is possible. Then, the position and posture of the tip set as the control point of the robot are controlled by PTP control.

  Here, even if the movement speed of the tip of the arm set as the control point is controlled to be equal to or lower than the reference speed, depending on the posture of the arm (robot), the movement speed of the part other than the control point in the arm may be The inventor of the present application has paid attention to the fact that the speed may be higher than the reference speed.

  Therefore, in the above configuration, a monitoring unit is set for each rotating unit. For example, the portion farthest from the joint that is the center of rotation when rotating each rotating unit is set as the monitoring unit of each rotating unit. Then, an angular velocity for driving each servo motor is calculated for each operation cycle by the angular velocity calculating means, and a speed of each monitoring unit is calculated by the monitoring velocity calculating means. Then, on the condition that the maximum speed among the calculated speeds of the respective monitoring units is higher than the reference speed, the robot arm is provided on the support unit side of the monitoring unit having the maximum speed among the respective monitoring units. All the selected joints are selected as speed reduction target axes.

  Subsequently, the angular velocity for driving a servo motor (hereinafter referred to as a reduction target motor) that rotates the selected speed reduction target shaft is reduced by the angular velocity reduction means so that the speed of each monitoring unit is equal to or lower than the reference speed. Of the servo motors, motors other than the reduction target motor are driven at the angular speed calculated by the angular speed calculation unit, and the reduction target motor is driven at the angular speed reduced by the angular speed reduction unit. Thereby, not only the control point of the robot but also the speed of the monitoring unit set in each rotating unit can be made lower than the reference speed, and the moving speed of the arm can be sufficiently suppressed.

  Moreover, in the said structure, the motor used as the reduction object of angular velocity among each servomotor is made into the reduction object motor which rotates the said speed reduction object axis | shaft. For this reason, the number of servo motors that reduce the angular speed can be reduced as compared with a configuration in which the angular speeds of all the servo motors are uniformly reduced so that the speed of each monitoring unit is equal to or less than the reference speed. For this reason, the time required for the process of reducing the angular velocity can be shortened, and the robot motion response comfortable for the operator can be maintained. Furthermore, in the above configuration, it is not necessary to specify which joint is the major factor that caused the maximum speed to exceed the reference speed, and the process of reducing the angular speed is simplified. Can do.

  In the second invention, the next cycle angle calculation means for calculating the angle after the operation cycle of each servo motor based on the angular velocity of each servo motor calculated by the angular velocity calculation means, and the next cycle angle calculation Based on the angle after the operation cycle of the reduction target motor among the angles after the operation cycle of the servo motors calculated by the means, and the current angle of the reduction target motor, Change amount calculating means for calculating the angle change amount of the motor, and the angular velocity lowering means calculates the reduction target motor calculated by the change amount calculation means so that the maximum speed is equal to or less than the reference speed. Among the angles after the operation cycle of each servo motor calculated by the next cycle angle calculation means, and the operation cycle of the motor to be reduced Is updated with the angle after the operation cycle of the reduction target motor calculated based on the angle change amount reduced by the angular velocity reduction means and the current angle of the reduction target motor, The servo motor further comprises resetting means for resetting the angle after the operation cycle of each servo motor, and the driving means sets the current angle of each servo motor to the angle of each servo motor reset by the resetting means. The servo motors are driven so as to be controlled after the operation cycle.

  In the above configuration, the angle after the operation cycle of each servo motor is calculated by the next cycle angle calculation means. Then, based on the angle after the operation cycle of the motor to be reduced and the current angle of the motor to be reduced, the angle change amount per operation cycle of the motor to be reduced is calculated, and the maximum speed among the speeds of the respective monitoring units The amount of change in the angle of the motor to be reduced that is calculated is reduced so that is below the reference speed.

  Subsequently, among the angles after the operation cycle of each servo motor calculated by the next cycle angle calculation means, the angle change amount by which the angle after the operation cycle of the reduction target motor is reduced by the angular velocity reduction means, and the reduction target By updating with the angle after the operation cycle of the motor to be reduced calculated based on the current angle of the motor, the angle after the operation cycle of each servo motor is reset. Then, each servo motor is driven so that the current angle of each servo motor is controlled after the operation cycle to the reset angle of each servo motor. For this reason, the angle driven per operation cycle of the reduction target motor is reduced, and the angular velocity at which the reduction target motor is driven can be appropriately reduced.

  In the third invention, the angular velocity reducing means reduces the angular change amount calculated by the change amount calculating means based on a value of a ratio between the maximum speed and the reference speed.

  In the above configuration, the amount of change in the angle of the motor to be reduced is reduced based on the value of the ratio between the maximum speed and the reference speed among the speeds of the monitoring units. By using the value of the above ratio, it is possible to reduce the amount of angle change reflecting the excess of the maximum speed with respect to the reference speed. The value of the ratio between the maximum speed and the reference speed is a value obtained by dividing the maximum speed by the reference speed (ratio value = maximum speed / reference speed).

  In a fourth invention, the angular velocity reduction means reduces the angle change amount by dividing the angle change amount by the value of the ratio.

  In the above configuration, since the simple calculation of dividing the angle change amount by the value of the ratio is used, the angle change amount can be easily reduced. Furthermore, in the above configuration, since the calculation of dividing the angle change amount by the value of the ratio is used, the angle change amount can be reduced at a ratio determined from a certain law, and the operator can easily predict the decrease in the angle change amount. Become.

  In a fifth means, the angular speed reducing means calculates the angular speed of the motor to be reduced among the angular speeds of the servo motors calculated by the angular speed calculating means so that the maximum speed is equal to or less than the reference speed. The angular speed of each servo motor is updated by updating the angular speed of the motor to be reduced with the angular speed reduced by the angular speed reducing means among the angular speeds of the servo motors calculated by the angular speed calculating means. Resetting means for resetting the servomotor, and the driving means drives each servo motor at the angular velocity of each servomotor reset by the resetting means.

  In the above configuration, among the angular velocities of the servomotors calculated by the angular velocity calculation means, the angular velocities of the reduction target motors are reduced so that the maximum speed is equal to or lower than the reference speed. Then, among the angular velocities of the servo motors calculated by the angular velocity calculating means, the angular velocities of the motors to be reduced are updated with the angular velocities reduced by the angular speed reducing means, thereby resetting the angular velocities of the servo motors. . Then, each servo motor is driven by the reset angular velocity of each servo motor. Thereby, the angular velocity which drives a reduction object motor can be reduced appropriately.

  In a sixth aspect of the invention, the angular velocity reduction means reduces the angular velocity of the reduction target motor calculated by the angular velocity calculation means based on a value of a ratio between the maximum speed and the reference speed.

  In the above configuration, based on the value of the ratio between the maximum speed and the reference speed among the speeds of the respective monitoring units, the motor to be reduced is driven so that the speed of the monitoring unit that maximizes the speed is equal to or lower than the reference speed. The angular velocity can be appropriately reduced. The value of the ratio between the maximum speed and the reference speed is a value obtained by dividing the maximum speed by the reference speed (ratio value = maximum speed / reference speed).

  In a seventh invention, the angular velocity reduction means reduces the angular velocity by dividing the angular velocity of the motor to be reduced calculated by the angular velocity calculation means by the value of the ratio.

  In the above configuration, the angular velocity for driving the reduction target motor can be easily and appropriately reduced.

  In an eighth aspect of the invention, a portion that is the most distant from the joint that is the center of rotation when rotating each rotating unit is set as the monitoring unit of each rotating unit.

  According to the above configuration, the portion farthest from the joint serving as the center of rotation when rotating each rotating unit is set as the monitoring unit of each rotating unit. For this reason, in each rotation part, the part with the highest possibility that speed becomes the highest can be set to a monitoring part, and the moving speed of an arm can fully be suppressed.

  In a ninth invention, based on the angular velocity of each servo motor calculated by the angular velocity calculating unit, a next cycle angle calculating unit that calculates an angle after the operation cycle of each servo motor; and Based on the current monitoring position calculation means for calculating the current position and the angle after the operation cycle of each servo motor calculated by the next cycle angle calculation means, the position after the operation cycle of each monitoring unit is calculated. A next cycle monitoring position calculating means, wherein the monitoring speed calculating means is calculated by the current position of each monitoring unit calculated by the current monitoring position calculating means and the next period monitoring position calculating means. The speed of each of the monitoring units is calculated based on the position after the operation cycle of each of the monitoring units.

  In the above configuration, based on the current position of each monitoring unit calculated by the current monitoring position calculation unit and the position after the operation cycle of each monitoring unit calculated by the next cycle monitoring position calculation unit, The speed can be calculated appropriately.

  As the monitoring speed calculation means, specifically, as in the tenth invention, the distance between the current position of each monitoring unit and the position after the operation cycle of each monitoring unit is divided by the operation cycle. Accordingly, it is possible to adopt a configuration in which the speed of each monitoring unit is calculated.

  Further, as the next cycle monitoring position calculation means, specifically, as in the eleventh aspect, the angle after the operation period of each servo motor calculated by the next cycle angle calculation means, and each rotation unit It is possible to employ a configuration in which the position of each monitoring unit after the operation cycle is calculated based on the size of the monitor.

  A twelfth means is installed at a predetermined robot installation location, including a plurality of rotating units, a joint that rotatably connects the rotating units to each other, and a servo motor that drives the rotating units. A control method for a robot that is applied to a robot that includes a support portion that operably supports a side opposite to the tip portion of the arm among both ends, and that controls the position and posture of the tip portion by PTP control. An angular velocity calculating step for calculating an angular velocity for driving each servo motor for each operation cycle, a monitoring velocity calculating step for calculating a velocity of a monitoring unit set in each rotating unit, and a monitoring velocity calculating step. Further, on the condition that the maximum speed among the speeds of each of the monitoring units is higher than the reference speed, the arm is provided on the support unit side with respect to the monitoring unit having the maximum speed. A selection step of selecting all the joints provided as speed reduction target axes, and the servo motor that rotates the speed reduction target axis selected in the selection step is a reduction target motor, and the speed of each of the monitoring units is An angular speed reduction step of reducing an angular speed for driving the motor to be reduced so as to be equal to or lower than the reference speed, and motors other than the motor to be reduced among the servo motors at the angular speed calculated by the angular speed calculation step. And a driving step of driving the motor to be reduced at the angular velocity reduced by the angular velocity reduction step.

  According to the said process, there can exist an effect similar to a 1st means.

The figure which shows the outline | summary of a robot, a controller, and a teaching pendant. The front view which shows the specific attitude | position of a robot. The flowchart which shows the process sequence of the arm speed suppression control which concerns on 1st Embodiment. The graph which shows the angular velocity pattern of a servomotor. The flowchart which shows the process sequence of the arm speed suppression control which concerns on 2nd Embodiment.

(First embodiment)
Hereinafter, a first embodiment embodied in a control device for a vertical articulated robot will be described with reference to the drawings. The robot of this embodiment is used in an assembly system such as a machine assembly factory as an industrial robot, for example.

  First, an outline of the robot 10 will be described with reference to FIG.

  As shown in the figure, the robot 10 includes a first axis J1, a second axis J2, a third axis J3, a fourth axis J4, a fifth axis J5 as rotation center axes of the joints that connect the rotating units to each other. And a six-axis robot having a sixth axis J6. The operating angle of each part in each axis is adjusted through driving of a driving source composed of a servo motor or the like and deceleration by a speed reducer or the like. Both servomotors can be rotated in both forward and reverse directions, and each rotating unit operates (drives) based on the origin position by driving the servomotor. Each servo motor is provided with an electromagnetic brake that brakes its output shaft and an encoder that outputs a pulse signal corresponding to the rotation angle of the output shaft.

  The robot 10 is installed on the floor, and the first axis J1 extends in the vertical direction. In the robot 10, the base 11 has a fixed portion 12 fixed to the floor or the like, and a rotating portion 13 (first rotating portion) provided above the fixed portion 12. The arm of the robot 10 includes a lower arm 15, an upper arm 16, a wrist portion 17, and a hand portion 18 in addition to the rotating portion 13. In the present embodiment, the fixing portion 12 corresponds to a support portion.

  The rotating part 13 corresponds to a root part on the opposite side of the arm tip part from both ends of the arm. The rotating unit 13 is rotatable in the horizontal direction around the first axis J1. That is, the rotating portion 13 extends in the direction of the first axis J1 and is supported by the fixed portion 12 so as to be rotatable about the first axis J1.

  The lower arm 15 (second rotating portion) is coupled to be rotatable in a clockwise direction or a counterclockwise direction about a second axis J2 extending in the horizontal direction as a rotation center. That is, the lower arm 15 extends in a direction away from the second axis J2 included in the plane orthogonal to the first axis J1, and is supported by the rotating unit 13 so as to be rotatable about the second axis J2. The lower arm 15 is provided in a direction extending in the vertical direction as a basic posture.

  The upper arm 16 is coupled to the upper end portion of the lower arm 15 so as to be rotatable clockwise or counterclockwise about a third axis J3 extending in the horizontal direction. That is, the upper arm 16 extends in a direction away from the third axis J3 parallel to the second axis J2, and is supported by the lower arm 15 so as to be rotatable about the third axis J3. The upper arm 16 is provided in a direction extending in the horizontal direction as a basic posture.

  The upper arm 16 is configured to be divided into two arm portions on the base end side (joint side with the third axis J3 as the rotation center during rotation) and the tip end side, and the base end side is the first upper side. The arm 16A (third rotating part), the tip side is a second upper arm 16B (fourth rotating part). The second upper arm 16B is rotatable in the torsional direction with respect to the first upper arm 16A with a fourth axis J4 extending in the longitudinal direction of the upper arm 16 as a rotation center. That is, the second upper arm 16B extends in the direction of the fourth axis J4 included in the plane orthogonal to the third axis J3, and is supported by the first upper arm 16A so as to be rotatable about the fourth axis J4. .

  A wrist portion 17 (fifth rotating portion) is provided at the distal end portion of the upper arm 16 (specifically, the second upper arm 16B). The wrist portion 17 is rotatable with respect to the second upper arm 16B with the fifth axis J5 extending in the horizontal direction as the center of rotation. That is, the wrist 17 extends in a direction away from the fifth axis J5 orthogonal to the fourth axis J4, and is supported by the second upper arm 16B so as to be rotatable about the fifth axis J5.

  A hand portion 18 (sixth rotating portion) for attaching a work, a tool, or the like is provided at the distal end portion of the wrist portion 17. The hand portion 18 is rotatable in the torsional direction around the sixth axis J6 that is the center line thereof. That is, the hand portion 18 extends in the direction of the sixth axis J6 orthogonal to the fifth axis J5, and is supported by the wrist portion 17 so as to be rotatable about the sixth axis J6.

  The controller 30 (control device) includes a CPU, a ROM, a RAM, a drive circuit, a position detection circuit, and the like. The ROM stores system programs and operation programs for the robot 10. The RAM stores parameter values and the like when executing these programs. Detection signals from the encoders are input to the position detection circuit. The position detection circuit detects the rotation angle of the servo motor provided at each joint based on the detection signal of each encoder.

  The CPU controls the position and orientation of the control point of the arm tip based on the position information input from the position detection circuit by executing a preset operation program (program). Specifically, the CPU feed-forward-controls the rotation angle (arm posture) of each joint in the arm of the robot 10 to the target rotation angle (target posture) by PTP (Point To Point) control. In PTP control, the operation trajectory (position and orientation) of the control point is not set when the control point is moved to the target position. In the present embodiment, a TCP (Tool Center Point) that is the center point 18a of the arm hand portion 18 is set as the control point. Further, the CPU calculates the angle and angle of each joint for realizing the position and posture based on the TCP position and posture, and the position and posture of the TCP based on the angle of each joint. And a function to calculate by forward conversion.

  In the present embodiment, the controller 30 executes speed suppression control that suppresses the moving speed of the arm of the robot 10 to a reference speed or less during teaching of the robot 10 (during manual operation). The reference speed is defined as, for example, 250 mm / s according to standards such as JIS and ISO.

  The teaching pendant 40 (operation device) includes a microcomputer including a CPU, ROM, and RAM, various manual operation keys, a display 42, and the like. The pendant 40 is connected to the controller 30 and can communicate with the controller 30. An operator (user) can manually operate the pendant 40 to create, modify, register, and set various parameters of the operation program of the robot 10. In teaching for correcting an operation program or the like, a teaching point through which a TCP as a control point passes in the work is taught. The operator can operate the robot 10 through the controller 30 based on the teaching operation program. In other words, the controller 30 controls the operation of the arm of the robot 10 based on the preset operation program and the operation of the pendant 40.

  Here, at the time of teaching of the robot 10 (manual operation), even if the movement speed of the TCP is controlled to be equal to or less than the reference speed, the movement speed of the part other than the TCP in the arm may be different depending on the posture of the robot 10. The inventor of the present application paid attention to the fact that the speed may be higher than the reference speed. For example, when the robot 10 is in the posture shown in FIG. 2, when the rotating unit 13 is rotated, the moving speed of the TCP (point C5) is sufficiently smaller than the reference speed. However, the moving speed of the tip end portion (point C2) of the lower arm 15 and one end portion (point C3) of the upper arm 16 may be higher than the reference speed.

  Therefore, the portion farthest from the joint (rotation center axis of each rotation unit) when rotating each rotation unit is set as a monitoring unit (points C1 to C5) of each rotation unit. For example, the point C2 farthest from the joint (the connecting portion between the rotating unit 13 and the lower arm 15) that becomes the center of rotation when the lower arm 15 is rotated is set as the monitoring unit of the lower arm 15. Similarly, the points C3 and C4 that are farthest from the joint (the connecting portion between the lower arm 15 and the upper arm 16) that becomes the center of rotation when the upper arm 16 is rotated are set as monitoring units for the upper arm 16, and so on. I do. When other parts (parts) are attached to the rotating part such as the upper arm 16, the tip part of the part may be set as the monitoring part. Then, the angular speed of each servo motor is suppressed so that the moving speeds of all the monitoring units are equal to or lower than the reference speed.

  FIG. 3 is a flowchart illustrating a processing procedure of speed suppression control that suppresses the moving speed of the arm of the robot 10 to a reference speed or less. This series of processing is repeatedly executed by the controller 30 every operation cycle Tr for operating the arm. The operation cycle Tr (control cycle) is, for example, 8 ms.

  In this series of processing, first, the current angle θk1 of each servo motor is detected (S11). Specifically, the current angle θk1 of each servo motor is detected by the position detection circuit based on the detection signal of the encoder provided in each servo motor. Note that k is a number from 1 to 6 corresponding to the first axis J1 to the sixth axis J6, respectively.

  Subsequently, the current position Pi1 of each monitoring unit is calculated based on the current angle θk1 of each servomotor and the size of each rotating unit (S12). i is a number from 1 to 5 corresponding to points C1 to C5, respectively. A specific example of the calculation method of the current position Pi1 will be described. First, the distance from the rotation center of each rotating unit to the monitoring unit is calculated based on the size of each rotating unit and the set position of each monitoring unit. . Then, the positions of the points C1 to C5 are calculated by combining the current angle θk1 of each servo motor, the size of each rotating unit, and the distance.

  Subsequently, the angular velocity ωk of each servo motor is calculated (S13). Specifically, at the time of teaching, the target angle of each servo motor is calculated based on the teaching point taught as the point through which TCP passes. For example, as shown in FIG. 4, a pattern of the angular velocity ωk when driving each servo motor to the target angle is set. Therefore, the current angular velocity ωk of each servo motor is calculated based on the set pattern of the angular velocity ωk. Note that k is a number from 1 to 6 corresponding to the first axis J1 to the sixth axis J6, respectively.

  Subsequently, an angle θk2 after the operation cycle Tr of each servo motor is calculated (S14). Specifically, the angle θk2 is calculated by the equation θk2 = θk1 + ωk × Tr.

  Subsequently, the position Pi2 after the operation cycle Tr of each monitoring unit is calculated (S15). i is a number from 1 to 5 corresponding to points C1 to C5, respectively. Here, the position Pi2 after the operation cycle Tr is the same as the processing of S12, the angle θk2 after the operation cycle Tr of each servo motor, the size of each rotation unit, and the rotation center of each rotation unit to the monitoring unit. May be calculated based on the distance.

  Subsequently, the speed Vi of each monitoring unit is calculated (S16). Specifically, the speed Vi is calculated by dividing the distance between the current position Pi1 of each monitoring unit and the position Pi2 after the operation cycle Tr by the operation cycle Tr. Note that i is a number from 1 to 5 corresponding to the points C1 to C5, respectively.

  Subsequently, the maximum speed Vmx, which is the maximum speed among the speed Vi of each monitoring unit, is calculated (S17). And the monitoring part which takes the maximum speed Vmx among each monitoring part is memorize | stored in the memory | storage means (memory) of the controller 30 (S18).

  Subsequently, it is determined whether or not the maximum speed Vmx is higher than the reference speed Vlm (S19). In this determination, when it is determined that the maximum speed Vmx is higher than the reference speed Vlm (S19: YES), a value α of the ratio between the maximum speed Vmx and the reference speed Vlm is calculated (S20). That is, the ratio value α is calculated according to the equation α = Vmx / Vlm (α> 1).

  Subsequently, in the arm, all joints provided on the fixed unit 12 side with respect to the monitoring unit taking the maximum speed Vmx stored in the memory are selected as speed reduction target axes (S21). Specifically, for example, in FIG. 2, when the monitoring unit having the maximum speed Vmx is the point C2, the first axis J1 and the second axis J2 are selected as the speed reduction target axes. In the present embodiment, a servo motor that rotates a speed reduction target shaft is referred to as a reduction target motor.

  Subsequently, the difference (θk2−θk1) between the angle θk2 after the operation cycle Tr of the reduction target motor and the current angle θk1 is calculated as the angle change amount Δθk of the reduction target motor (S22). Note that k is a number from 1 to 6 corresponding to the first axis J1 to the sixth axis J6, respectively. Then, the angle change amount Δθk is updated by dividing the angle change amount Δθk of the motor to be reduced by the ratio value α (S23).

  Subsequently, the angle θk2 after the operation cycle Tr of all the servomotors is reset (S24). Specifically, first, the angle after the operation cycle Tr of the reduction target motor is newly calculated by adding the angle change amount Δθk updated in the process of S23 and the current angle θk1 of the reduction target motor. Then, by updating only the angle θk2 after the operation cycle Tr of the reduction target motor among the angles after the operation cycle Tr of all the servomotors, all the servomotors are updated. The angle θk2 after the operation cycle Tr is reset. Then, the process is executed again from the process of S15 using the reset angle θk2.

  On the other hand, if it is determined in S19 that the maximum speed Vmx is equal to or less than the reference speed Vlm (S19: NO), the current angle of each servo motor is controlled after the operation cycle Tr to the angle θk2 of each servo motor. Thus, each servo motor is driven (S25). Here, in the case of passing through the process of S24, each servomotor is driven so that each servomotor is controlled after the operation cycle Tr up to the angle θk2 reset in the process of S24. Then, this series of processing is temporarily ended (END).

  The process of S12 corresponds to the process as the current monitoring position calculation means (current monitoring position calculation process), the process of S13 corresponds to the process as the angular velocity calculation means (angular speed calculation process), and the process of S14 is the next cycle. The processing as the angle calculation means (next cycle angle calculation step) corresponds to the processing of S15, the processing as the next cycle monitoring position calculation means (next cycle monitoring position calculation step), and the processing of S16 as monitoring speed calculation means. It corresponds to the process (monitoring speed calculation process). Further, the process of S21 corresponds to a process (selection process) as a selection means, the process of S22 corresponds to a process (change amount calculation process) as a change amount calculation means, and the process of S23 is a process as an angular velocity reduction means. The process of S24 corresponds to the process (resetting process) as the resetting means, and the process of S25 corresponds to the process (driving process) as the driving means.

  The embodiment described in detail above has the following advantages.

  The angular velocity ωk for driving each servo motor is calculated for each operation cycle Tr, and the speed Vi of the monitoring unit (points C1 to C5) set in each rotating unit is calculated. Then, on the condition that the maximum speed Vmx is higher than the reference speed Vlm among the calculated speeds Vi of each monitoring unit, the arm is closer to the fixed unit 12 than the monitoring unit having the maximum speed Vmx among the monitoring units. All provided joints are selected as speed reduction target axes.

  Subsequently, the angle change amount Δθk per operation cycle Tr of the reduction target motor that rotates the speed reduction target shaft is decreased so that the speed Vi of each monitoring unit is equal to or less than the reference speed Vlm. Of the angles θk2 after the operation cycle Tr of each servo motor, the angle after the operation cycle Tr of the reduction target motor is based on the reduced angle change amount Δθk and the current angle θk1 of the reduction target motor. By updating the calculated angle after the operation cycle Tr of the motor to be reduced, the angle θk2 after the operation cycle Tr of each servo motor is reset. Then, each servo motor is driven such that the current angle of each servo motor is controlled after the operation cycle Tr to the reset angle θk2 of each servo motor. For this reason, the angular velocity for driving the reduction target motor can be appropriately reduced. Thereby, not only the TCP that is the control point, but also the speed Vi of each monitoring unit can be set to the reference speed Vlm or less, and the moving speed of the arm can be sufficiently suppressed.

  In addition, among the servo motors, a motor that is subject to reduction in the angle change amount Δθk is a reduction target motor that rotates a speed reduction target shaft. As a result, the number of servo motors that reduce the angle change amount Δθk is reduced as compared with the configuration in which the angle change amounts Δθk of all the servo motors are uniformly reduced so that the speed Vi of each monitoring unit is equal to or less than the reference speed Vlm. Less. For this reason, it is possible to shorten the time required for recalculation for reducing the angle change amount Δθk, and it is possible to maintain a robot motion response that is comfortable for the operator during teaching. Furthermore, among the joints of the robot, a process for identifying which joint is a major factor that causes the maximum speed Vmx to exceed the reference speed Vlm, and a servo motor for rotating the identified joint Compared to the configuration for performing the process of reducing the angle change amount Δθk, it is possible to simplify the program for reducing the angle change amount Δθk while realizing the same level of safety as this configuration.

  The angle change amount Δθk is reduced by dividing the angle change amount Δθk of the motor to be reduced by the value α of the ratio between the maximum speed Vmx and the reference speed Vlm. Since a simple calculation of dividing the angle change amount Δθk by the ratio value α is used, the angle change amount Δθk can be easily reduced. Further, since the calculation of dividing the angle change amount Δθk by the ratio value α is used, the angle change amount Δθk can be reduced at a ratio determined from a certain law, and the decrease in the angle change amount Δθk can be easily predicted by the operator. Become.

  The portion that is farthest from the joint that is the center of rotation when rotating each rotating unit is set as the monitoring unit of each rotating unit. For this reason, in each rotation part, the part with the highest possibility that speed becomes the highest can be set to a monitoring part, and the moving speed of an arm can fully be suppressed.

(Second Embodiment)
In the second embodiment, the angular velocity reduction method for driving the reduction target motor is changed. Hereinafter, the difference from the first embodiment will be mainly described.

  FIG. 5 is a flowchart illustrating a processing procedure of speed suppression control that suppresses the moving speed of the arm of the robot 10 to a reference speed or less. This series of processing is repeatedly executed by the controller 30 every operation cycle Tr for operating the arm. In FIG. 5, the same steps as those described in FIG. 3 are given the same step numbers for convenience.

  In this series of processes, it is determined whether the maximum speed Vmx is higher than the reference speed Vlm after the processes of S11 to S18 (S19). In the determination of S19, when it is determined that the maximum speed Vmx is higher than the reference speed Vlm (S19: YES), after passing through S20 and S21, the angular speed ωk of the motor to be reduced among all the servo motors is a ratio value α. The angular velocity ωk of the reduction target motor is updated by dividing by (S26).

  Subsequently, the angular velocities ωk of all the servo motors are reset (S27). Specifically, among the angular velocities ωk of all the servo motors set in S13, only the angular velocities of the reduction target motors are updated with the angular velocities of the reduction target motors updated in S26. Reset it. Then, using the reset angular velocity ωk, the process is executed again from the process of S14.

  On the other hand, if it is determined in S19 that the maximum speed Vmx is equal to or less than the reference speed Vlm (S19: NO), each servomotor is driven at the angular speed ωk up to the angle θk2 of each servomotor calculated in S14 ( S28). Here, when the process of S27 is performed, each servo motor is driven at the angular velocity ωk reset in the process of S27. Then, this series of processing is temporarily ended (END).

  The embodiment described in detail above has the following advantages. Here, only the advantages different from the first embodiment will be described.

  The angular velocity ωk for driving each servo motor is calculated for each operation cycle Tr, and the speed Vi of the monitoring unit (points C1 to C5) set in each rotating unit is calculated. Then, on the condition that the maximum speed Vmx is higher than the reference speed Vlm among the calculated speeds Vi of each monitoring unit, the arm is closer to the fixed unit 12 than the monitoring unit having the maximum speed Vmx among the monitoring units. All provided joints are selected as speed reduction target axes.

  And the angular velocity which drives the reduction object motor which rotates the selected speed reduction object axis | shaft is reduced so that the speed Vi of each monitoring part may be below the reference speed Vlm. Then, among the already calculated angular velocities ωk of the servo motors, the angular velocities of the reduction target motors are updated with the reduced angular velocities, whereby the angular velocities ωk of the respective servo motors are reset. Then, each servo motor is driven by the reset angular velocity ωk of each servo motor. Thereby, not only the TCP that is the control point, but also the speed Vi of each monitoring unit can be set to the reference speed Vlm or less, and the moving speed of the arm can be sufficiently suppressed.

  Further, among the servo motors, a motor that is a target of reduction of the angular velocity ωk is a reduction target motor that rotates the speed reduction target shaft. As a result, the number of servo motors that decrease the angular speed ωk can be reduced as compared with a configuration in which the angular speed ωk of all the servomotors is uniformly decreased so that the speed Vi of each monitoring unit is equal to or lower than the reference speed Vlm. For this reason, the time required for recalculation for reducing the angular velocity ωk can be shortened, and the robot motion response comfortable for the operator can be maintained during teaching. Furthermore, among the joints of the robot, a process for identifying which joint is a major factor that causes the maximum speed Vmx to exceed the reference speed Vlm, and a servo motor for rotating the identified joint Compared to a configuration that performs processing for reducing the angular velocity ωk, it is possible to simplify a program that reduces the angular velocity ωk while realizing the same level of safety as this configuration.

  It should be noted that the first and second embodiments can be modified as follows.

  In S19 of FIGS. 3 and 5, it is determined whether or not the maximum speed Vmx is higher than the reference speed Vlm, but it is determined whether or not the maximum speed Vmx is higher than a determination speed set slightly higher than the reference speed Vlm. May be. In this case, the arm speed suppression control can be completed quickly.

  In S23 of FIG. 3, the angle change amount Δθk is reduced by dividing the angle change amount Δθk by the ratio value α, but the angle change is obtained by dividing the angle change amount Δθk by a value slightly larger than the ratio value α. The amount Δθk may be reduced. Also in this case, the arm speed suppression control can be completed quickly. Similarly, in S26 of FIG. 5, the angular velocity ωk may be decreased by dividing the angular velocity ωk by a value slightly larger than the ratio value α.

  In the first and second embodiments, 250 mm / s defined by standards such as JIS and ISO is used as the reference speed Vlm, but a slightly lower speed, for example, 230 mm / s is used as the reference speed Vlm. It may be used. In these cases, the moving speed of the arm can be reliably and easily reduced to less than 250 mm / s.

  In the first and second embodiments, a horizontal articulated robot or the like may be employed instead of the vertical articulated robot 10.

  DESCRIPTION OF SYMBOLS 10 ... Robot, 13 ... Rotating part, 15 ... Lower arm, 16 ... Upper arm, 16A ... 1st upper arm, 16B ... 2nd upper arm, 17 ... Wrist part, 18 ... Hand part, 30 ... Controller.

Claims (12)

A plurality of rotating parts, a joint that rotatably connects the rotating parts to each other, and an arm including a servo motor that drives each of the rotating parts;
The robot is installed in a predetermined robot installation location, and is applied to a robot having a support portion that operatively supports the opposite side of the end portion of the arm among both end portions of the arm, and the position of the tip portion by PTP control And a robot control device for controlling the posture,
Angular velocity calculating means for calculating an angular velocity for driving each servo motor for each operation cycle;
Monitoring speed calculating means for calculating the speed of the monitoring section set in each of the rotating sections;
On the condition that the maximum speed among the speeds of each of the monitoring units calculated by the monitoring speed calculation means is higher than a reference speed, the arm is closer to the support unit than the monitoring unit having the maximum speed. Selecting means for selecting all of the joints provided in as the speed reduction target axes;
The servo motor that rotates the speed reduction target axis selected by the selection unit is set as a reduction target motor, and the angular speed for driving the reduction target motor is reduced so that the speed of each monitoring unit is equal to or lower than the reference speed. Angular velocity reduction means
Driving means for driving a motor other than the motor to be reduced among the servo motors at an angular speed calculated by the angular speed calculating means, and driving the motor to be reduced at an angular speed reduced by the angular speed reducing means; A robot control device comprising: A next period angle calculation means for calculating an angle after the operation period of each servo motor based on the angular speed of each servo motor calculated by the angular speed calculation means;
Based on the angle after the operation cycle of the motor to be reduced among the angles after the operation cycle of each servo motor calculated by the next cycle angle calculation means, and the current angle of the motor to be reduced, the reduction target A change amount calculating means for calculating an angle change amount per operation cycle of the motor,
The angular velocity reduction means reduces the angle change amount of the reduction target motor calculated by the change amount calculation means so that the maximum speed is equal to or less than the reference speed,
Of the angles after the operation cycle of each servo motor calculated by the next cycle angle calculation means, the angle change amount by which the angle after the operation cycle of the motor to be reduced is reduced by the angular velocity reduction means, and Resetting means for resetting the angle after the operation cycle of each servo motor by updating the angle after the operation cycle of the motor to be reduced calculated based on the current angle of the motor to be reduced. ,
The drive means drives each servo motor so that the current angle of each servo motor is controlled after the operation cycle to the angle of each servo motor reset by the resetting means. The robot control apparatus according to 1.   The robot control apparatus according to claim 2, wherein the angular velocity reduction means reduces the angle change amount calculated by the change amount calculation means based on a value of a ratio between the maximum speed and the reference speed.   The robot control device according to claim 3, wherein the angular velocity reduction unit reduces the angle change amount by dividing the angle change amount by the value of the ratio. The angular speed reduction means reduces the angular speed of the motor to be reduced among the angular speeds of the servo motors calculated by the angular speed calculation means so that the maximum speed is equal to or less than the reference speed.
Of the angular velocities of the servo motors calculated by the angular velocity calculation means, the angular velocities of the servo motors are updated by updating the angular velocities of the reduction target motors with the angular velocities reduced by the angular speed reduction means. Further comprising resetting means,
The robot control apparatus according to claim 1, wherein the driving unit drives the servo motors at an angular velocity of the servo motors reset by the resetting unit.   6. The robot control apparatus according to claim 5, wherein the angular velocity lowering unit lowers the angular velocity of the motor to be reduced calculated by the angular velocity calculating unit based on a value of a ratio between the maximum speed and the reference speed. .   The robot control device according to claim 6, wherein the angular velocity lowering unit lowers the angular velocity by dividing the angular velocity of the motor to be reduced calculated by the angular velocity calculating unit by the value of the ratio.   The robot control device according to any one of claims 1 to 7, wherein a portion farthest from the joint that becomes a rotation center when rotating each rotation unit is set as the monitoring unit of each rotation unit. . A next period angle calculation means for calculating an angle after the operation period of each servo motor based on the angular speed of each servo motor calculated by the angular speed calculation means;
A current monitoring position calculating means for calculating a current position of each of the monitoring units;
A next cycle monitoring position calculating unit that calculates a position after the operation cycle of each monitoring unit based on the angle after the operation cycle of each servo motor calculated by the next cycle angle calculating unit;
The monitoring speed calculation means includes a current position of each monitoring unit calculated by the current monitoring position calculation means and a position after an operation cycle of each monitoring unit calculated by the next cycle monitoring position calculation means. The robot control device according to claim 1, wherein the speed of each of the monitoring units is calculated based on the control unit.   The monitoring speed calculation means calculates the speed of each monitoring unit by dividing the distance between the current position of each monitoring unit and the position after the operation cycle of each monitoring unit by the operation cycle. The robot control device described.   The next cycle monitoring position calculating means is configured to determine the position after the operation cycle of each monitoring unit based on the angle after the operation cycle of each servo motor calculated by the next cycle angle calculating unit and the size of each rotating unit. The robot control apparatus according to claim 9, wherein the position of the robot is calculated. A plurality of rotating parts, a joint that rotatably connects the rotating parts to each other, and an arm including a servo motor that drives each of the rotating parts;
The robot is installed in a predetermined robot installation location, and is applied to a robot having a support portion that operatively supports the opposite side of the end portion of the arm among both end portions of the arm. And a robot control method for controlling the posture,
An angular velocity calculating step for calculating an angular velocity for driving each servo motor for each operation cycle;
A monitoring speed calculating step for calculating the speed of the monitoring unit set in each of the rotating units;
On the condition that the maximum speed among the speeds of the respective monitoring units calculated by the monitoring speed calculating step is higher than a reference speed, the arm is closer to the support unit than the monitoring unit having the maximum speed. A selection step of selecting all the joints provided in as the speed reduction target axes;
The servo motor that rotates the speed reduction target axis selected in the selection step is set as a reduction target motor, and the angular speed for driving the reduction target motor is reduced so that the speed of each monitoring unit is equal to or lower than the reference speed. An angular velocity lowering step,
A driving step of driving a motor other than the reduction target motor among the servo motors at an angular velocity calculated by the angular velocity calculation step, and driving the reduction target motor at an angular velocity reduced by the angular velocity reduction step; A method for controlling a robot, comprising:

Images