JP6547320B2 - Robot control device and control method - Google Patents

Description

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

  Conventionally, when the movement speed of the control point of the robot exceeds the reference speed at the time of manual operation of the robot, the movement target position is corrected so that the movement speed becomes equal to or less than the reference speed to operate the robot (patented Reference 1).

Patent No. 3994487

  However, even if the moving 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 moving speed of the arm may not be sufficiently suppressed in some cases. The person paid attention.

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

  The first means is installed at a predetermined robot installation place, a plurality of rotating parts, an arm including a joint for rotatably connecting the rotating parts, and a servomotor for driving the rotating parts, A control device for a robot, comprising: a support portion that supports the end portion of the end portion opposite to the tip end portion of the arm so as to control the position and attitude of the tip portion by PTP control. Angular velocity calculating means for calculating an angular velocity for driving each of the servomotors every operation cycle, monitoring speed calculating means for calculating the speed of the monitoring unit set in each of the rotating parts, and calculation by the monitoring speed calculating means On the condition that the maximum speed among the speeds of the monitoring units is higher than the reference speed, the arm is closer to the support section than the monitoring unit having the maximum speed. Selection means for selecting all the joints selected as the speed reduction target axes, and the servomotor for rotating the speed reduction target axes selected by the selection means are reduction target motors, and the speeds of the respective monitoring units are Angular velocity lowering means for decreasing the angular velocity for driving the reduction target motor so as to be equal to or lower than the reference speed, and motors other than the reduction target motor among the servomotors at the angular velocity calculated by the angular velocity calculation means And driving means for driving the reduction target motor at an angular velocity reduced by the angular velocity reduction means.

  In the above configuration, the robot arm includes a plurality of rotating parts, and the rotating parts are rotatably connected to each other by joints. Further, among the both end portions of the arm, the side opposite to the tip end portion of the arm is movably supported by a support portion installed at a predetermined robot installation place. Then, PTP control controls the position and attitude of the tip set as a control point of the robot.

  Here, even if the moving speed of the tip of the arm set as the control point is controlled to be equal to or lower than the reference speed, the moving speed of the part other than the control point in the arm is dependent on the attitude of the arm (robot). The inventor of the present application has noted that it may be higher than the reference speed.

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

  Subsequently, the angular velocity reducing means reduces the angular velocity for driving the servomotor (hereinafter referred to as a reduction target motor) for rotating the selected speed reduction target shaft such that the speed of each monitoring unit becomes equal to or lower than the reference speed. Then, among the servomotors, motors other than the reduction target motor are driven at the angular velocity calculated by the angular velocity calculation means, and the reduction target motor is driven at the angular velocity reduced by the angular velocity reduction means. 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 equal to or lower than the reference speed, and the moving speed of the arm can be sufficiently suppressed.

  Further, in the above configuration, among the servomotors, the motor whose angular velocity is to be reduced is the reduction target motor that rotates the speed reduction target shaft. Therefore, the number of servomotors for reducing the angular velocity can be reduced, as compared with a configuration in which the angular velocities of all the servomotors are uniformly reduced such that the speed of each monitoring unit becomes equal to or less than the reference speed. Therefore, it is possible to shorten the time required for the process of reducing the angular velocity, and to maintain the operation response of the robot comfortable for the operator. Furthermore, in the above configuration, it is not necessary to specify which joint among the joints, which is the major factor causing the maximum speed to exceed the reference speed, and it is possible to simplify the process of reducing the angular velocity. Can.

  In a second aspect of the invention, a next cycle angle calculation means for calculating an angle after the operation cycle of each of the servomotors based on the angular velocity of each of the servomotors calculated by the angular velocity calculation means; The angle after the operation cycle of the reduction target motor among the angles after the operation cycle of each of the servomotors calculated by the means, and the current angle of the reduction target motor, per operation cycle of the reduction target motor The angular velocity reduction means calculates the reduction target motor calculated by the change amount calculation means such that the maximum velocity is equal to or less than the reference velocity The operating cycle of the reduction target motor among the angles after the operating cycle of each of the servomotors calculated by the next cycle angle calculating means by reducing the angle change amount of The angle is updated by 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 apparatus further comprises resetting means for resetting the angle after the operation cycle of each servomotor, wherein the driving means is configured to reset the current angle of each servomotor by the resetting means. The servomotors are driven so as to be controlled after the operation cycle.

  In the above configuration, the next cycle angle calculating means calculates the angle after the operation cycle of each servo motor. Then, based on the angle after the operation cycle of the reduction target motor and the current angle of the reduction target motor, the angle change amount per operation cycle of the reduction target motor is calculated, and the maximum speed among the speeds of each monitoring unit Is reduced below the reference speed, the calculated angle change amount of the reduction target motor is reduced.

  Subsequently, among the angles after the operation cycle of each servomotor calculated by the next cycle angle calculation means, the angle change amount after the operation cycle of the reduction target motor is reduced by the angular velocity reduction means, and the reduction object By updating the angle after the operation cycle of the reduction target motor calculated based on the current angle of the motor, the angle after the operation cycle of each servomotor is reset. Then, each servomotor is driven such that the current angle of each servomotor is controlled after the operation cycle up to the reset angle of each servomotor. Therefore, the angle driven per operation cycle of the reduction target motor is reduced, and the angular velocity driven by the reduction target motor can be appropriately reduced.

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

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

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

  In the above configuration, since the simple operation of dividing the amount of change in angle by the value of the ratio is used, the amount of change in angle 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 reduction amount of the angle change amount. Become.

  In the fifth means, the angular velocity reduction means sets the angular velocity of the reduction target motor among the angular velocities of the servomotors calculated by the angular velocity calculation means so that the maximum velocity is equal to or less than the reference velocity. Of the angular velocities of the servomotors calculated by the angular velocity calculation means, the angular velocity of the reduction target motor is updated by the angular velocity reduced by the angular velocity reduction means to reduce the angular velocity of the servomotors. The drive means drives the servomotors according to the angular velocity of the servomotors reset by the reset means.

  In the above configuration, among the angular velocities of the servomotors calculated by the angular velocity calculating means, the angular velocity of the reduction target motor is decreased such that the maximum velocity is equal to or lower than the reference velocity. Then, among the angular velocities of each servomotor calculated by the angular velocity calculation means, the angular velocity of each servomotor is reset by updating the angular velocity of the reduction target motor with the angular velocity reduced by the angular velocity reduction means. . And each servomotor is driven by the angular velocity of each reset servomotor. Thus, the angular velocity for driving the reduction target motor can be appropriately reduced.

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

  In the above configuration, based on the value of the ratio of the maximum speed to the reference speed among the speeds of each monitoring section, the reduction target motor is driven such that the speed of the monitoring section at which the speed is maximum is equal to or lower than the reference speed. The angular velocity can be reduced appropriately. The value of the ratio between the maximum velocity and the reference velocity is a value obtained by dividing the maximum velocity by the reference velocity (ratio value = maximum velocity / reference velocity).

  In the seventh invention, the angular velocity reduction means reduces the angular velocity by dividing the angular velocity of the reduction target motor 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 present invention, the portion most distant from the joint, which is the rotation center when rotating each of the rotating portions, is set as the monitoring portion of each of the rotating portions.

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

  In a ninth aspect of the invention, a next cycle angle calculation means for calculating an angle after the operation cycle of each of the servomotors based on the angular velocity of each of the servomotors calculated by the angular velocity calculation means; The position after the operation cycle of each monitoring unit is calculated based on the current monitoring position calculation means for calculating the current position and the angle after the operation cycle of each servomotor calculated by the next cycle angle calculation means It further comprises a next cycle monitoring position calculating means, and the monitoring speed calculating means calculates the current position of each monitoring unit calculated by the current monitoring position calculating means, and the next cycle monitoring position calculating means. The speed of each monitoring unit is calculated based on the position after the operation cycle of each monitoring unit.

  In the above configuration, based on the current position of each monitoring unit calculated by the current monitoring position calculation means and the position after the operation cycle of each monitoring unit calculated by the next cycle monitoring position calculation means, 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. Thus, a configuration may be adopted in which the speed of each monitoring unit is calculated.

  Further, as the next cycle monitoring position calculation means, specifically, as in the eleventh invention, the angle after the operation cycle of each servomotor calculated by the next cycle angle calculation means, and each rotating portion A configuration may be adopted in which the position after the operation cycle of each of the monitoring units is calculated based on the size of.

  The twelfth means is installed at a predetermined robot installation place, a plurality of rotating parts, an arm including a joint for rotatably connecting the rotating parts, and a servomotor for driving the rotating parts, A control method of a robot comprising: a support portion that supports one end of the end portion opposite to the tip end portion of the arm so as to control the position and posture of the tip portion by PTP control. Calculating an angular velocity for calculating the angular velocity for driving each of the servomotors in each operation cycle, a monitoring speed calculating step for calculating the speed of the monitoring unit set in each rotating unit, and calculating the monitoring speed calculating step On the condition that the maximum speed among the speeds of the monitoring units is higher than the reference speed, the support unit side of the arm is closer to the supporting unit than the monitoring unit having the maximum speed. The selection step of selecting all the provided joints as the speed reduction target axes, and the servomotor for rotating the speed reduction target axis selected in the selection step is a reduction target motor, and the speed of each monitoring unit is An angular velocity reduction step of decreasing an angular velocity for driving the reduction target motor so as to be equal to or lower than the reference speed, and a motor other than the reduction target motor among the servomotors at the angular velocity calculated in the angular velocity calculation step And driving the motor to be reduced at the angular velocity reduced in the angular velocity reduction step.

  According to the above-mentioned process, the same operation and effect as the first means can be achieved.

The figure which shows the outline of a robot, a controller, and a teaching pendant. The front view which shows the specific attitude | position of a robot. 5 is a flowchart showing a processing procedure of arm speed suppression control according to the first embodiment. The graph which shows the angular velocity pattern of a servomotor. The flowchart which shows the process sequence of arm speed suppression control which concerns on 2nd Embodiment.

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

  First, an outline of the robot 10 will be described based on FIG.

  As shown in the figure, the robot 10 sets a first axis J1, a second axis J2, a third axis J3, a fourth axis J4, a fifth axis J5, as central axes of rotation of the joints that connect the rotating portions to each other. And a sixth axis J6. The operating angle of each part in each of these axes is adjusted through driving of a drive source composed of a servomotor or the like and deceleration by a reduction gear or the like. The servomotors are both capable of rotating in both forward and reverse directions, and drive of the servomotors causes each rotating portion to operate (drive) on the basis of the home position. Each servo motor is provided with an electromagnetic brake for braking its output shaft, and an encoder for outputting a pulse signal according to the rotation angle of the output shaft.

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

  The rotating portion 13 corresponds to a root portion of the both ends of the arm opposite to the tip end of the arm. The rotation unit 13 is rotatable in the horizontal direction about the first axis J1 as a rotation center. That is, the rotating portion 13 extends in the direction of the first axis J1 and is supported by the fixing portion 12 so as to be rotatable about the first axis J1.

  The lower arm 15 (second rotating portion) is rotatably coupled in a clockwise direction or a counterclockwise direction around a second axis line J2 extending in the horizontal direction. That is, the lower arm 15 extends in a direction away from the second axis J2 included in a plane orthogonal to the first axis J1, and is rotatably supported by the rotating portion 13 around the second axis J2. The lower arm 15 is provided to extend in the vertical direction as a basic posture.

  An upper arm 16 is connected to an 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 rotatably supported by the lower arm 15 about the third axis J3. The upper arm 16 is provided to extend in the horizontal direction as a basic posture.

  The upper arm 16 is divided into two arm portions at the base end side (the joint side rotating about the third axis J3 at the time of rotation) and the tip end side, and the base end side is the first upper side. The arm 16A (third rotating portion), and the tip end side is a second upper arm 16B (fourth rotating portion). The second upper arm 16B is rotatable in a twisting 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 a plane orthogonal to the third axis J3, and is rotatably supported by the first upper arm 16A about the fourth axis J4. .

  A wrist portion 17 (fifth rotating portion) is provided at the tip 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 around a fifth axis J5 extending in the horizontal direction. That is, the wrist portion 17 extends in a direction away from the fifth axis J5 orthogonal to the fourth axis J4, and is rotatably supported about the fifth axis J5 by the second upper arm 16B.

  A hand portion 18 (sixth rotating portion) for attaching a work, a tool or the like is provided at the tip of the wrist portion 17. The hand portion 18 is rotatable in a twisting direction around a sixth axis J6 which is a center line of the hand portion 18. 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 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 a system program, an operation program, and the like of the robot 10. The RAM stores parameter values and the like when executing these programs. The detection signal of each encoder is input to the position detection circuit. The position detection circuit detects the rotation angle of the servomotor provided in each joint based on the detection signal of each encoder.

  The CPU executes a preset operation program (program) to control the position and attitude of the control point at the tip of the arm based on the position information input from the position detection circuit. Specifically, the CPU performs feedforward control of the rotation angle (attitude of the arm) of each joint in the arm of the robot 10 to a target rotation angle (target attitude) by PTP (Point To Point) control. In PTP control, when moving the control point to the target position, the operation trajectory (position and attitude) of the control point is not set. In this embodiment, TCP (Tool Center Point) which is the center point 18 a of the hand 18 of the arm is set as the control point. In addition, the CPU calculates the angle of each joint to realize the position and posture based on the position and posture of the TCP by inverse transformation, and the position and posture of the TCP based on the angles of each joint. It has a function to calculate by forward conversion.

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

  The teaching pendant 40 (operation device) includes a microcomputer including a CPU, a ROM, and a 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. The operator (user) can manually operate the pendant 40 to create, correct, register, and set various parameters of the operation program of the robot 10. In teaching which performs correction of an operation program etc., it teaches a teaching point through which a TCP, which is a control point, passes in an operation. Then, the operator can operate the robot 10 through the controller 30 based on the taught 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, even if the moving speed of TCP is controlled to be equal to or lower than the reference speed at the time of teaching (manual operation) of the robot 10, the moving speed of a portion other than the TCP in the arm is dependent on the posture of the robot 10. The inventor of the present application has noted that it 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 of the lower arm 15 (point C2) and one end (point C3) of the upper arm 16 may be higher than the reference speed.

  Then, when rotating each rotation part, the part most distant from the joint (rotation center axis line of each rotation part) used as a rotation center is set as a monitoring part (point C1-C5) of each rotation part. For example, when the lower arm 15 is rotated, a point C2 farthest from the joint (the connecting portion between the rotating portion 13 and the lower arm 15) which is the rotation center is set as the monitoring portion of the lower arm 15. Similarly, when the upper arm 16 is rotated, the points C3 and C4 farthest from the joint (the connecting portion between the lower arm 15 and the upper arm 16) serving as the rotation center are set as the monitoring portion of the upper arm 16 I do. In addition, when another part (part) is attached to rotation parts, such as upper arm 16, you may set the front-end part etc. of the parts as a monitoring part. Then, the angular velocity of each servomotor is suppressed so that the moving speeds of all the monitoring units become equal to or less than the reference speed.

  FIG. 3 is a flowchart showing a processing procedure of speed suppression control for suppressing the moving speed of the arm of the robot 10 to the reference speed or less. This series of processing is repeatedly executed by the controller 30 at each 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 servomotor is detected (S11). Specifically, based on the detection signal of the encoder provided in each servomotor, the position detection circuit detects the current angle θk1 of each servomotor. In addition, k is a number of 1 to 6 respectively corresponding to the first axis J1 to the sixth axis J6.

  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. First, the distance from the center of rotation 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 servomotor, the size of each rotating portion, and the distance.

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

  Subsequently, an angle θk2 after the operation cycle Tr of each servomotor 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, similarly to the processing of S12, the angle θk2 after the operation cycle Tr of each servo motor, the size of each rotating portion, and the rotation center of each rotating portion to the monitoring portion It may be calculated based on the distance of.

  Subsequently, the velocity 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. In addition, i is a number of 1-5 corresponding to points C1-C5, respectively.

  Subsequently, the maximum velocity Vmx, which is the maximum velocity among the velocities Vi of the respective monitoring units, is calculated (S17). Then, among the monitoring units, the monitoring unit having the maximum velocity Vmx is stored in the storage unit (memory) of the controller 30 (S18).

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

  Subsequently, in the arm, all joints provided closer to the fixed unit 12 than the monitoring unit having the maximum velocity Vmx stored in the memory are selected as the speed reduction target axes (S21). Specifically, for example, in FIG. 2 described above, when the monitoring unit at the maximum velocity 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, the servomotor that rotates the 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). In addition, k is a number of 1 to 6 respectively corresponding to the first axis J1 to the sixth axis J6. Then, the angle change amount Δθk is updated by dividing the angle change amount Δθk of the reduction target motor 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, among the angles after the operation cycle Tr of all the servomotors, all the servomotors are updated by updating only the angle θk2 after the operation cycle Tr of the reduction target motor with the newly calculated angle after the operation cycle Tr. The angle θk2 after the operation cycle Tr of is reset. Then, the processing from S15 is performed again using the reset angle θk2.

  On the other hand, when it is determined in S19 that the maximum velocity Vmx is less than or equal to the reference velocity Vlm (S19: NO), the current angle of each servomotor is controlled after the operation cycle Tr up to the angle θk2 of each servomotor. Thus, each servomotor is driven (S25). Here, when the process of S24 is performed, each servomotor is driven such 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 (currently monitoring position calculating process) as the current monitoring position calculating means, the process of S13 corresponds to the process (angular velocity calculating process) as the angular velocity calculating means, and the process of S14 is the next cycle. The process of S15 corresponds to the process as the next cycle monitoring position calculating means (the next cycle monitoring position calculating process), and the process of S16 is the monitoring speed calculating means. Corresponds to the processing (monitoring speed calculation step) as Further, the process of S21 corresponds to the process (selection process) as the selection means, the process of S22 corresponds to the process (change amount calculation process) as the change amount calculation means, and the process of S23 as the 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 above has the following advantages.

  The angular velocity ωk for driving each servomotor is calculated for each operation cycle Tr, and the velocity 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 of the calculated speeds Vi of each monitoring unit is higher than the reference speed Vlm, 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 for rotating the speed reduction target shaft is decreased such that the speed Vi of each monitoring unit becomes equal to or less than the reference speed Vlm. Then, among 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 reduced based on the 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 reduction target motor, the angle θk2 after the operation cycle Tr of each servo motor is reset. Then, each servomotor is driven such that the current angle of each servomotor is controlled after the operation cycle Tr up to the angle θk2 of each of the servomotors set again. Therefore, the angular velocity for driving the reduction target motor can be appropriately reduced. As a result, not only the TCP as the control point but also the speed Vi of each monitoring unit can be made equal to or lower than the reference speed Vlm, and the moving speed of the arm can be sufficiently suppressed.

  Further, among the servomotors, the motor to be reduced for the angle change amount Δθk is a reduction target motor for rotating the speed reduction target shaft. As a result, the number of servomotors for reducing the angle change amount .DELTA..theta.k is reduced compared to a configuration in which the angle change amounts .DELTA..theta.k for all servomotors are uniformly reduced such that the speed Vi of each monitoring unit becomes equal to or less than the reference speed Vlm. It can be reduced. Therefore, the time required for recalculation to reduce the angle change amount Δθ k can be shortened, and the robot's operation response comfortable for the operator can be maintained at the time of teaching. Further, among the joints of the robot, a process for identifying which joint is a major factor causing the maximum velocity Vmx to exceed the reference velocity Vlm, and a servomotor for rotating the identified joint Compared to the configuration in which the process of reducing the angle change amount Δθk is performed, a program for reducing the angle change amount Δθk can be simplified 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 reduction target motor by the value α of the ratio between the maximum velocity Vmx and the reference velocity Vlm. The angle change amount Δθk can be easily reduced because the simple operation of dividing the angle change amount Δθk by the ratio value α is used. Furthermore, since the operation of dividing the angle change amount Δθk by the ratio value α is used, the angle change amount Δθk can be reduced by a ratio determined from a certain law, and the operator can easily predict the reduction amount of the angle change amount Δθk. Become.

  -The part most distant from the joint which becomes the rotation center when rotating each rotation part is set as a monitoring part of each rotation part. For this reason, in each rotation part, the part with high possibility that speed becomes high can be set to a monitoring part, and the movement speed of an arm can fully be suppressed.

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

  FIG. 5 is a flowchart showing a processing procedure of speed suppression control for suppressing the moving speed of the arm of the robot 10 to the reference speed or less. This series of processing is repeatedly executed by the controller 30 at each operation cycle Tr for operating the arm. In FIG. 5, the same step numbers are given to the same processes as the processes described in FIG. 3 above for the sake of convenience.

  In this series of processing, after the processing of S11 to S18, it is determined whether the maximum velocity Vmx is higher than the reference velocity Vlm (S19). When it is determined in S19 that the maximum velocity Vmx is higher than the reference velocity Vlm (S19: YES), after S20 and S21, the angular velocity ωk of the reduction target motor among all the servomotors is the 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 servomotors are reset (S27). Specifically, among the angular velocities ωk of all the servomotors set in S13, the angular velocities ωk of all the servomotors are updated by updating only the angular velocity of the reduction target motor with the angular velocity of the reduction target motor updated in S26. Reset. Then, the processing from S14 is executed again using the reset angular velocity ωk.

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

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

  The angular velocity ωk for driving each servomotor is calculated for each operation cycle Tr, and the velocity 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 of the calculated speeds Vi of each monitoring unit is higher than the reference speed Vlm, 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.

  Then, the angular velocity for driving the reduction target motor that rotates the selected speed reduction target axis is reduced such that the speed Vi of each monitoring unit becomes equal to or less than the reference speed Vlm. Then, the angular velocity ωk of each servomotor is reset by updating the angular velocity of the reduction target motor with the decreased angular velocity among the angular velocities ωk of the servomotors already calculated. Then, each servomotor is driven by the reset angular velocity ωk of each servomotor. As a result, not only the TCP as the control point but also the speed Vi of each monitoring unit can be made equal to or lower than the reference speed Vlm, and the moving speed of the arm can be sufficiently suppressed.

  Further, among the servomotors, the motor whose angular velocity ω k is to be reduced is set as the reduction target motor for rotating the speed reduction target shaft. Thereby, the number of servomotors for reducing the angular velocity ωk can be reduced, as compared with a configuration in which the angular velocities ωk of all the servomotors are uniformly reduced such that the velocity Vi of each monitoring unit becomes equal to or less than the reference velocity Vlm. Therefore, the time required for recalculation to reduce the angular velocity ωk can be shortened, and the robot's operation response comfortable for the operator can be maintained during teaching. Further, among the joints of the robot, a process for identifying which joint is a major factor causing the maximum velocity Vmx to exceed the reference velocity Vlm, and a servomotor for rotating the identified joint Compared to the configuration in which the processing for reducing the angular velocity ωk is performed, the program for reducing the angular velocity ωk can be simplified while realizing the same level of safety as this configuration.

  The first and second embodiments may be modified as follows.

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

  In S23 of FIG. 3, the angle change Δθ k is reduced by dividing the angle change Δθ k by the ratio value α, but the angle change is calculated by dividing the angle change Δθ k by a value slightly larger than the ratio value α. The amount Δθ k may be reduced. Also in this case, the speed suppression control of the arm can be terminated promptly. 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 the standard such as JIS or ISO is used as the reference speed Vlm, but a speed slightly lower than that, for example, 230 mm / s, is set as the reference speed Vlm. You may use. 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, instead of the vertical articulated robot 10, a horizontal articulated robot or the like may be employed.

  DESCRIPTION OF SYMBOLS 10 ... Robot, 13 ... Rotation 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 rotatably connecting the rotating parts to each other, and an arm including a servomotor for driving the rotating parts;
The present invention is applied to a robot provided at a predetermined robot installation location and having a support portion that supports the opposite end of the arm movably at both ends of the arm, and the position of the tip by PTP control. And a control device of a robot that controls the posture,
Angular velocity calculating means for calculating an angular velocity for driving each of the servomotors every operation cycle;
Monitoring speed calculation means for calculating the speed of the monitoring unit set in each of the rotating units;
The monitoring at the maximum speed among the joints constituting the arm , provided that the maximum speed among the speeds of the monitoring units calculated by the monitoring speed calculation means is higher than a reference speed Selection means for selecting all the joints provided on the side of the support section rather than the section as the speed reduction target axes;
The servomotor for rotating the speed reduction target shaft selected by the selection means is a reduction target motor, and the angular velocity for driving the reduction target motor is reduced such that the speed of each monitoring unit is equal to or less than the reference speed Angular velocity reduction means for
Driving means for driving a motor other than the reduction target motor among the servomotors at an angular velocity calculated by the angular velocity calculation means, and driving the reduction target motor at an angular velocity reduced by the angular velocity reduction means And a controller of a robot. Next cycle angle calculation means for calculating an angle after the operation cycle of each servomotor based on the angular velocity of each servomotor calculated by the angular velocity calculation means;
Of the angles after the operation cycle of each servomotor calculated by the next cycle angle calculation means, the reduction target based on the angle after the operation cycle of the reduction target motor and the current angle of the reduction target motor Change amount calculation 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 becomes equal to or less than the reference speed.
Among the angles after the operation cycle of each servomotor calculated by the next cycle angle calculation means, the angle change amount obtained by reducing the angle after the operation cycle of the reduction target motor by the angular velocity reduction means, and Resetting means for resetting the angle after the operation cycle of each of the servomotors by updating with the angle after the operation cycle of the reduction target motor calculated based on the current angle of the reduction target motor is further provided ,
The driving means drives the servomotors so that the current angle of the servomotors is controlled after the operation cycle to the angle of the servomotors reset by the resetting means. The control device of the robot according to 1).   The control device of a robot according to claim 2, wherein the angular velocity reduction means reduces the amount of angle change calculated by the change amount calculation means based on a value of a ratio between the maximum speed and the reference speed.   The control device of a robot according to claim 3, wherein the angular velocity reduction means reduces the amount of change in angle by dividing the amount of change in angle by the value of the ratio. The angular velocity reduction means reduces the angular velocity of the reduction target motor among the angular velocities of the servomotors calculated by the angular velocity calculation means such that the maximum velocity is equal to or less than the reference velocity.
Of the angular velocities of the servomotors calculated by the angular velocity calculation means, the angular velocity of the servomotors is reset by updating the angular velocity of the reduction target motor with the angular velocity reduced by the angular velocity reduction means. Further comprising reset means for
The control device of a robot according to claim 1, wherein the drive means drives the servomotors by the angular velocity of the servomotors reset by the reset means.   The control device of a robot according to claim 5, wherein 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 velocity and the reference velocity. .   The control device of a robot according to claim 6, wherein the angular velocity reduction means reduces the angular velocity by dividing the angular velocity of the reduction target motor calculated by the angular velocity calculation means by the value of the ratio.   The control device of a robot according to any one of claims 1 to 7, wherein a portion farthest from the joint, which is a rotation center when rotating each rotating unit, is set as the monitoring unit of each rotating unit. . Next cycle angle calculation means for calculating an angle after the operation cycle of each servomotor based on the angular velocity of each servomotor calculated by the angular velocity calculation means;
Current monitoring position calculation means for calculating the current position of each of the monitoring units;
Next cycle monitoring position calculating means for calculating the position after the operation cycle of each of the monitoring units based on the angle after the operation cycle of each of the servomotors calculated by the next cycle angle calculating means;
The monitoring speed calculating means may calculate the current position of each monitoring unit calculated by the current monitoring position calculating means, and the position after the operation cycle of each monitoring unit calculated by the next cycle monitoring position calculating means. The control device of a robot according to any one of claims 1 to 8, wherein a speed of each of the monitoring units is calculated based on the control.   The monitoring speed calculation means calculates the speed of each monitoring section by dividing the distance between the current position of each monitoring section and the position after the operating cycle of each monitoring section by the operating cycle. The control device of the described robot.   The next cycle monitoring position calculation means is 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 calculation means and the size of each rotating unit. The control device of the robot according to claim 9 or 10 which calculates the position of. A plurality of rotating parts, a joint rotatably connecting the rotating parts to each other, and an arm including a servomotor for driving the rotating parts;
The present invention is applied to a robot provided at a predetermined robot installation location and having a support portion that supports the opposite end of the arm movably at both ends of the arm, and the position of the tip by PTP control. And a control method of a robot that controls the posture,
An angular velocity calculating step of calculating an angular velocity for driving each of the servomotors every operation cycle;
A monitoring speed calculation step of calculating the speed of the monitoring unit set in each of the rotating units;
The monitoring at the maximum speed among the joints constituting the arm, on the condition that the maximum speed among the speeds of the monitoring units calculated in the monitoring speed calculating step is higher than a reference speed Selecting all the joints provided on the side of the support part rather than the part as the speed reduction target axes;
The servomotor for rotating the speed reduction target shaft selected in the selection step is a reduction target motor, and the angular velocity for driving the reduction target motor is reduced such that the speed of each monitoring unit is equal to or less than the reference speed. Angular velocity reduction step
Driving a motor other than the reduction target motor among the servomotors at an angular velocity calculated in the angular velocity calculation step, and driving the reduction target motor at an angular velocity reduced in the angular velocity reduction step; And a control method of a robot.

Images