WO2020170306A1 - Robot control device - Google Patents

Abstract

This robot control device (1) comprises: a pre-smoothing command generation unit (11) that generates a pre-smoothing command (11a) that is a command for driving a plurality of arms of a robot (30); a smoothing unit (12) that generates a post-smoothing command (12a) by smoothing the pre-smoothing command (11a); a command output unit (13) that outputs the post-smoothing command (12a) to the robot (30); a speed estimation unit (14) that estimates the operation speed of a monitoring point provided in the robot (30), on the basis of the pre-smoothing command (11a) and the post-smoothing command (12a); and an override correction unit (15) that corrects an override that is a virtual time correction ratio on the basis of the operation speed estimated by the speed estimation unit (14) and a predetermined speed limit. The pre-smoothing command generation unit (11) generates the pre-smoothing command (11a) by using the override obtained by the override correction unit (15).

Description

Robot controller

The present invention relates to a robot control device that controls a robot.

In recent years, the development of a human-cooperative robot system in which the worker and the robot share a work space without being separated by a safety fence is in progress. In the human collaborative robot system, in order to reduce the possibility of the robot colliding with a person, when the person invades a specified range, it is necessary to have a function to operate the robot by setting the operation speed of the robot below a predetermined limit value. It is said that. Conventionally, the drive angular velocity of the servo motor attached to the robot is calculated for each operation cycle of the robot, and the drive angular velocity of the servo motor is calculated based on the ratio between the robot operation velocity and the speed limit obtained from the calculated drive angular velocity. Techniques for reducing the amount have been proposed (for example, see Patent Document 1).

JP, 2016-43452, A

However, in the conventional technology, the corrected angular velocity command for adjusting the operation speed of the robot is directly output to the servo motor, so that there is a problem that acceleration and vibration exceeding the allowable value of the servo motor occur.

Generally, when the robot controller outputs a command to the servo motor, it is necessary to perform smoothing processing on the generated command in order to realize a smooth motion of the robot. When the trapezoidal pattern is used to generate the angular velocity command output to the servo motor based on the displacement amount between the initial angle and the final angle of the servo motor, if the angular velocity command is not smoothed, the command start point , The end point, and the change point of the angular velocity command such as the apex of the trapezoid may cause acceleration and vibration exceeding the allowable value of the servo motor. The smoothing process is a process for suppressing the occurrence of acceleration and vibration that exceed the above allowable values.

When performing the smoothing process to control the robot's operating speed below the limit value, it is necessary to calculate the robot's operating speed based on the command after the smoothing process, which is the actual command output to the servo motor. However, since the process of lowering the value of the angular velocity command for making the operation speed of the robot less than or equal to the speed limit is the process performed for the command before smoothing, the timing at which the process is reflected is only the smoothing process. There is a problem that the operation speed of the robot may exceed the speed limit with a delay.

The present invention has been made in view of the above, and provides a robot control device that operates a robot at an operating speed within a limit value even if a process of smoothing a command is performed to smoothly operate the robot. With the goal.

In order to solve the above-mentioned problems and to achieve the object, the present invention is a robot control device for controlling a robot having a plurality of arms, wherein smoothing is a command for driving a plurality of arms of the robot. Pre-smoothing command generation unit that generates a pre-command, a smoothing unit that smoothes the pre-smoothing command generated by the pre-smoothing command generation unit to generate a post-smoothing command, and a smoothing unit And a command output unit that outputs a command after smoothing to the robot. The present invention is a speed estimation for estimating an operation speed of a monitoring point provided in a robot based on a pre-smoothing command generated by a pre-smoothing command generation unit and a post-smoothing command generated by a smoothing unit. And a override correction unit that corrects the override that is the virtual time correction rate based on the operation speed estimated by the speed estimation unit and the predetermined speed limit. The pre-smoothing command generation unit generates a pre-smoothing command using the override obtained by the override correction unit.

According to the present invention, it is possible to obtain the effect that the robot can be operated at the operation speed within the limit value even if the process of smoothing the command is performed to smoothly operate the robot.

FIG. 1 is a diagram showing a hardware configuration of a robot controller according to the first embodiment Block diagram for explaining the function of the robot controller according to the first embodiment 2 is a flowchart showing the procedure of the operation of the robot controller according to the first embodiment. FIG. 3 is a diagram showing a configuration of a pre-smoothing command generation unit included in the robot control device according to the first embodiment. 6 is a flowchart showing the procedure of the operation of the pre-smoothing command generation unit included in the robot control device according to the first embodiment. FIG. 3 is a diagram showing a configuration of a speed estimation unit included in the robot control device according to the first embodiment. 3 is a flowchart showing an operation procedure of a speed estimation unit included in the robot control device according to the first embodiment. Block diagram for explaining the function of the robot controller according to the second embodiment 6 is a flowchart showing the procedure of the operation of the suppression unit included in the robot control device according to the second embodiment. Block diagram for explaining the function of the robot controller according to the third embodiment 6 is a flowchart showing a procedure of an operation of a speed estimation unit included in the robot control device according to the third embodiment. A processor in the case where some or all of the functions of a pre-smoothing command generation unit, a smoothing unit, a command output unit, a speed estimation unit, and an override correction unit included in the robot control device according to the first embodiment are realized by the processor. Figure A processing circuit in the case where a part or all of a pre-smoothing command generation unit, a smoothing unit, a command output unit, a speed estimation unit, and an override correction unit included in the robot control device according to the first embodiment is realized by a processing circuit. Figure

A robot controller according to an embodiment of the present invention will be described below in detail with reference to the drawings. The present invention is not limited to this embodiment.

In this specification, the robot posture means a robot hand position and a robot hand posture. The robot hand position is the position of the robot hand in a reference coordinate system set at an arbitrary position serving as a reference in a three-dimensional space. The robot hand posture can be represented by coordinate conversion from the coordinates of the tool coordinate system defined at the robot hand position. For the coordinate conversion, for example, a coordinate conversion matrix, an Euler angle, a quaternion or a Rodrigues parameter is used. In this specification, the robot posture is described as the position and posture of the robot hand, but in the coordinate system moved by performing a predetermined coordinate conversion from the coordinate system set for the robot hand, the position is the same as that of the robot hand. And the attitude can be calculated. The robot posture is not limited to the robot hand position and the robot hand posture.

Embodiment 1.
FIG. 1 is a diagram showing a hardware configuration of the robot controller 1 according to the first embodiment. The robot control device 1 is a device that controls a robot having a plurality of arms, and stores a CPU (Central Processing Unit) 2 that operates to control the robot, and data used by the CPU 2 when the CPU 2 operates. And a storage device 3 that operates. CPU2 is an example of a processor. The storage device 3 stores data indicating a housing model of the robot controlled by the robot control device 1 and data indicating a robot posture. An example of the storage device 3 is a ROM (Read Only Memory), a HDD (Hard Disk Drive), or a RAM (Random Access Memory).

The robot control device 1 further includes an IO (Input Output) interface 4 having a function of transmitting a signal to the receiving device 6 and a function of receiving a signal transmitted from the input device 7. The receiving device 6 and the input device 7 are also shown in FIG. The IO interface 4 is connected to the receiving device 6 by a cable 8 and is connected to the input device 7 by a cable 9. The receiving device 6 receives the signal transmitted from the robot control device 1. An example of the receiving device 6 is a lamp or a buzzer that notifies the outside of the robot control device 1 of the operation state of the robot controlled by the robot control device 1. The input device 7 transmits a signal to the robot control device 1. An example of the input device 7 is a light curtain or an area sensor. The robot control device 1 further includes a bus 5 that interconnects the CPU 2, the storage device 3, and the IO interface 4.

FIG. 2 is a block diagram for explaining the function of the robot control device 1 according to the first embodiment. The robot control device 1 is a device that controls the robot 30. The robot 30 is also shown in FIG. The robot 30 has a plurality of arms and a robot hand provided at the tip of each of the plurality of arms. Each arm is rotatably connected to the main body of the robot 30.

　The connection part of each arm with the main body is defined as a joint part. A drive device for changing the angle of the joint and a joint angle detector for measuring the rotation angle of the joint are attached to each joint. The drive device is a device that drives the joint portion and has an electric motor. Examples of electric motors are servomotors or stepping motors. The joint angle detector has an image sensor and an encoder. The position and orientation of each arm of the robot 30 changes as the drive device changes the angle of each joint.

The robot controller 1 outputs the robot operation command 1a to the robot 30 at a predetermined control cycle. The robot drive command 1a is a joint angle command output to a drive device provided in each joint of the robot 30, and is a command for controlling the operation of a plurality of arms of the robot 30.

The robot control device 1 includes a pre-smoothing command generation unit 11 that generates a pre-smoothing command 11a that is a command for driving a plurality of arms of the robot 30. Details of the pre-smoothing command generation unit 11 will be described later with reference to FIGS. 4 and 5.

The robot control device 1 further includes a smoothing unit 12 that smoothes the pre-smoothing command 11a generated by the pre-smoothing command generating unit 11 to generate a post-smoothing command 12a. The post-smoothing command 12a is the robot drive command 1a. The smoothing unit 12 may generate the post-smoothing command 12a by using the average of changes in the values included in the pre-smoothing command 11a, or may use the filter corresponding to the Nth delay element to perform the post-smoothing command. 12a may be generated. N is an integer of 1 or more.

The robot control device 1 further includes a command output unit 13 that outputs the smoothed command 12 a generated by the smoothing unit 12 to the robot 30. The smoothed command 12 a output from the command output unit 13 to the robot 30 is the robot drive command 1 a for driving the robot 30.

The robot control device 1 determines the monitoring location of the robot 30 based on the pre-smoothing command 11a generated by the pre-smoothing command generation unit 11 and the post-smoothing command 12a generated by the smoothing unit 12. It further has a speed estimation unit 14 for estimating an operation speed. The monitoring points are preset in the robot 30, and the number of monitoring points is one or more. The operation speed estimated by the speed estimation unit 14 is the representative monitoring speed 14a. Details of the speed estimation unit 14 will be further described later with reference to FIGS. 6 and 7.

The robot control device 1 further includes an override correction unit 15 that corrects the override, which is a virtual time correction rate, based on the representative monitoring speed 14a estimated by the speed estimation unit 14 and a predetermined speed limit. The data indicating the speed limit is stored in the storage device 3 of FIG. The override correction unit 15 obtains the corrected override 15a by performing the correction.

The override 15a is a value obtained by dividing the target operation speed of the robot 30 by the representative monitoring speed 14a, and is a value from 0 to 1. When the override 15a is represented by "OVRD" and the target operation speed of the robot 30 is represented by "Vlim", the override 15a is obtained by the following equation (1).
OVRD=min(1,max(0,Vlim/V)) (1)

The target operation speed of the robot 30 may be input to the robot controller 1 from the input device 7 and stored in the storage device 3, or may be stored in the storage device 3 in advance. The storage device 3 stores a plurality of target operation speeds, and one of the plurality of target operation speeds is selected depending on the operation state of the robot 30 or the signal transmitted from the input device 7 to the robot control device 1. The operating speed may be selected.

The functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 are realized by the CPU 2. The CPU 2 realizes the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 by reading and executing the program stored in the storage device 3. To do. The storage device 3 stores a program in which the steps executed by the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 are eventually executed. It is also a device for doing. The program stored in the storage device 3 causes the CPU 2 to execute the procedure or method executed by the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15. is there.

The pre-smoothing command 11a includes a pre-smoothing command value, and the post-smoothing command 12a includes a post-smoothing command value. In this specification, the pre-smoothing command value and the post-smoothing command value are the angle command value and the angular velocity command value of each joint of the robot 30. The robot drive command 1a output to the robot 30 by the robot controller 1 in each control cycle includes an angle command value. In this specification, the angle command value before smoothing is represented as θ_i, the angular velocity command value before smoothing is represented as ω_i, the angle command value after smoothing is represented as φ_i, and the angular velocity command after smoothing is represented. The value may be represented as ψ_i. The subscript i is a number that identifies each joint.

FIG. 3 is a flowchart showing an operation procedure of the robot controller 1 according to the first embodiment. The CPU 2 processes a program stored in advance in the storage device 3 to determine whether or not there is an operation command (S1).

When the CPU 2 determines that the operation command does not exist (No in S1), it does not start the operation and waits. That is, when the CPU 2 determines that the operation command does not exist (No in S1), the process of step S1 is repeated. When the CPU 2 determines that the operation command is present (Yes in S1), the CPU 2 performs the acquisition process of the current robot posture (S2).

The CPU 2 acquires data indicating a joint angle from a joint angle detector attached to each joint of the robot 30 in order to acquire data indicating the current robot posture. After that, the CPU 2 calculates the position and orientation of the robot hand from the joint angle of the robot 30 using the data indicating the housing model of the robot 30 stored in the storage device 3. Hereinafter, calculating the position and orientation of the robot hand from the joint angle of the robot 30 using the data indicating the housing model of the robot 30 stored in the storage device 3 is referred to as “forward conversion”.

After that, the CPU 2 performs a process of acquiring a target robot posture (S3). Next, the CPU 2 initializes the virtual time (S4). The order in which each process from step S2 to step S4 is performed is not limited.

After that, the pre-smoothing command generation unit 11 updates the pre-smoothing command 11a for moving the robot 30 from the current posture to the target posture every control cycle (S5). In the process of step S5, the pre-smoothing command generation unit 11 acquires the data indicating the joint angle from the joint angle detector attached to each joint of the robot 30 and performs the forward conversion, for example, as in the process of step S2. By doing so, data indicating the current posture of the robot 30 in each control cycle is acquired. The pre-smoothing command generation unit 11 may store the attitude data updated in each control cycle and use the attitude data of one cycle before as the data indicating the current attitude in the next control cycle.

After that, the smoothing unit 12 smoothes the pre-smoothing command 11a to generate the post-smoothing command 12a (S6). Next, the speed estimation unit 14 estimates the operation speed of the monitoring location (S7). Next, the override correction unit 15 corrects the override (S8). Next, the command output unit 13 outputs a drive command to the robot 30 (S9). The drive command is the smoothed command 12a generated by the smoothing unit 12.

After that, the CPU 2 determines whether the current posture of the robot 30 has reached the target posture in each control cycle (S10), and when it determines that the current posture has reached the target posture (Yes in S10), the operation is performed. finish. When the CPU 2 determines that the current posture of the robot 30 has not reached the target posture (No in S10), the operation of the robot controller 1 proceeds to step S5.

FIG. 4 is a diagram showing a configuration of the pre-smoothing command generation unit 11 included in the robot control device 1 according to the first embodiment. The pre-smoothing command generation unit 11 includes a virtual time calculation unit 111 that calculates a virtual time 111a different from the actual control time based on the override 15a. The pre-smoothing command generation unit 11 further includes a motion planning unit 112 that acquires from the storage device 3 the motion parameters 112a of acceleration/deceleration and maximum speed when the robot 30 operates. The pre-smoothing command generating unit 11 is a command for generating the pre-smoothing command 11a using the operation parameter 112a acquired by the motion planning unit 112 based on the virtual time 111a obtained by the virtual time calculating unit 111. It further has a value updating unit 113.

The operation planning unit 112 acquires the operation parameter 112 a previously stored in the storage device 3 from the storage device 3 and outputs it to the command value updating unit 113. The motion parameter 112a includes data of angular acceleration/deceleration and maximum angular velocity for generating an angular velocity pattern until each joint moves from the current angle to the target angle. When the storage device 3 stores a plurality of data sets for the operation parameter 112a, the operation planning unit 112 corresponds to the operation status of the robot 30 from the plurality of data sets stored in the storage device 3. The operation parameter 112a may be selected and output to the command value updating unit 113.

FIG. 5 is a flowchart showing an operation procedure of the pre-smoothing command generation unit 11 included in the robot control device 1 according to the first embodiment. The virtual time calculation unit 111 updates the virtual time 111a based on the override 15a which is the virtual time correction rate (S11). In the following, the override 15a may be represented as “OVRD” and the virtual time may be represented as “KT”.

Specifically, the virtual time calculation unit 111 updates the virtual time using the following equation (2).
KT(k)=KT(k−1)+OVRD(k)×ΔT (2)
ΔT is a control cycle, and KT(k) represents virtual time at k steps. OVRD is a real number from 0 to 1.

Next, the command value updating unit 113 determines whether to update the command value based on the virtual time KT(k) (S12). When the virtual time KT(k) exceeds the j-th command value update time T(j), the command value updating unit 113 determines to update the command value (Yes in S12), and the virtual time KT(k) is When the command value update time T(j) for the jth time is not exceeded, it is determined that the command value is not updated (No in S12). When it is determined that the command value is not updated (No in S12), the command value updating unit 113 continues to output the previously output command value. When the command value updating unit 113 determines not to update the command value (No in S12), the operation of the pre-smoothing command generating unit 11 ends. When the command value updating unit 113 determines to update the command value (Yes in S12), the command value updating unit 113 updates the command value updating time using the following formula (3) (S13).
T(j+1)=T(j)+ΔT (3)

After performing the process of step S13, the command value updating unit 113 generates a pre-smoothing command 11a that is an angle command and an angular velocity command of each joint before smoothing (S14). That is, in step S14, the command value updating unit 113 updates the pre-smoothing command 11a to generate a new pre-smoothing command 11a. For example, when updating the pre-smoothing command 11a, the command value updating unit 113 updates the angular velocity command value ω_i using the following equation (4) and then uses the angular velocity command value ω_i obtained by the updating. To update the angle command value θ_i.
ω_i(k)=min(ωm_i, ωa_i(k), ωb_i(k)) (4)

Ωm_i is the maximum angular velocity command value at the joint i, and ωa_i(k) is the angular velocity command value when the joint i is accelerated by the angular velocity increase amount α_i. ωb_i(k) is the remaining movement amount Δθ_i( which is obtained by subtracting the current angle command value θ_i(k) from the target angle θf_i, when the angular velocity is decelerated by the angular velocity decrease amount β_i and stops. k)=abs(θf_i−θ_i(k)).

ωb_i(k) is calculated by the following equation (5).
ωb_i(k)=sqrt(2×Δθ_i(k)×β_i) (5)
The angle command value of each joint is updated by the following equation (6) based on the updated angular velocity command value.
θ_i(k)=θ_i(k−1)+ω_i(k)×ΔT (6)

FIG. 6 is a diagram showing a configuration of the speed estimation unit 14 included in the robot control device 1 according to the first embodiment. The speed estimation unit 14 generates a pre-smoothing command 11a including the angle command value of each joint before smoothing, a post-smoothing command 12a including the angle command value of each joint after smoothing, and an override 15a. Upon reception, the operating speeds of one or more monitoring points are estimated, and the representative monitoring speed 14a that is a representative of the operating speeds of the one or more monitoring points is output. The monitoring point may be described as a monitoring point.

The speed estimation unit 14 receives the pre-smoothing command 11a and calculates the difference (θ_i(k)-θ_i(k-1)) between the joint angle command values before smoothing, which is updated in each control cycle, before smoothing. It has the 1st movement amount acquisition part 141 which acquires as command value movement amount 141a. The velocity estimation unit 14 receives the smoothed command 12a and smoothes the difference (φ_i(k)-φ_i(k-1)) between the smoothed joint angle command values updated in each control cycle. It further has the 2nd moving amount acquisition part 142 acquired as command value moving amount 142a.

The speed estimation unit 14 receives the pre-smoothing command value movement amount 141a and the post-smoothing command 12a, and sets the post-smoothing joint angle command value φ_i(k) as the pre-smoothing command value movement amount (θ_i(k)−θ_i It further includes a first monitoring point position calculation unit 143 that calculates the first monitoring point position corresponding to the joint angle command value to which (k-1)) is added. Hereinafter, the first monitoring point position may be described as “first monitoring point position P_A143a”. The velocity estimation unit 14 receives the smoothed command 12a and the smoothed command value movement amount 142a, and converts the smoothed command value movement amount (φ_i(k)-φ_i) into the smoothed joint angle command value φ_i(k). A second monitoring point position calculation unit 144 that calculates the second monitoring point position corresponding to the joint angle command value to which (k-1)) is added is further included. Below, a 2nd monitoring point position may be described as "2nd monitoring point position P_B144a." The velocity estimation unit 14 further includes a third monitoring point position calculation unit 145 that receives the smoothed command 12a and calculates a third monitoring point position corresponding to the smoothed joint angle command value φ_i(k). Hereinafter, the third monitoring point position may be described as “third monitoring point position P_C145a”.

The speed estimation unit 14 calculates a first estimated speed 146a obtained by dividing the difference between the value of the first monitoring point position P_A143a and the value of the third monitoring point position P_C145a by the control cycle ΔT. Further has 146. In other words, the first speed calculator 146 calculates the position of the monitoring point calculated based on the post-smoothing command 12a and the pre-smoothing position that is the movement amount of the monitoring point calculated based on the pre-smoothing command 11a. The first estimated speed 146a of the monitoring location is calculated based on the command value movement amount 141a. For example, the first speed calculation unit 146 calculates the pre-smoothing command value movement amount 141a based on the pre-smoothing command 11a which is the basis of the post-smoothing command 12a output to the robot 30 one cycle before. Then, the first estimated speed 146a is calculated using the pre-smoothing command value movement amount 141a obtained by the calculation. The first estimated speed 146a is an estimated speed of motion.

The speed estimation unit 14 calculates a second estimated speed 147a obtained by dividing the difference between the value of the second monitoring point position P_B144a and the value of the third monitoring point position P_C145a by the control cycle ΔT. Further has 147. That is, the second speed calculation unit 147 calculates the position of the monitoring point calculated based on the post-smoothing command 12a and the smoothed position that is the movement amount of the monitoring point calculated based on the post-smoothing command 12a. The second estimated speed 147a of the monitoring location is calculated based on the command value movement amount 142a. For example, the second speed calculator 147 calculates the smoothed command value movement amount 142a based on the smoothed command 12a output to the robot 30 one cycle before, and the smoothed command obtained by the calculation. The second estimated speed 147a is calculated using the value movement amount 142a. The second estimated speed 147a is an estimated speed of motion.

The speed estimation unit 14 further includes a representative speed calculation unit 148 that receives the first estimated speed 146a, the second estimated speed 147a, and the override 15a and calculates the representative monitored speed 14a at the monitoring location. That is, the representative speed calculation unit 148 estimates the speed of the monitoring location based on the first estimated speed 146a and the second estimated speed 147a. Specifically, the representative speed calculation unit 148 estimates the larger estimated speed of the first estimated speed 146a and the second estimated speed 147a as the operating speed of the monitoring location.

FIG. 7 is a flowchart showing an operation procedure of the speed estimation unit 14 included in the robot control device 1 according to the first embodiment. The CPU 2 initializes the representative monitoring speed (S21). The representative monitoring speed may be represented as “representative monitoring speed V”. Specifically, since the first estimated speed and the second estimated speed are compared after step S21, the CPU 2 initializes the representative monitoring speed to 0. Thereafter, the first movement amount acquisition unit 141 calculates the pre-smoothing command value movement amount 141a, and the second movement amount acquisition unit 142 calculates the smoothed command value movement amount 142a (S22). Either the pre-smoothing command value movement amount 141a or the post-smoothing command value movement amount 142a may be calculated first.

The robot controller 1 sets one or more monitoring points in the robot 30 in order to monitor the operation speed of the robot 30. The monitoring location is a predetermined location on the housing of the robot 30 or a location away from the location in a predetermined direction by a predetermined distance, and corresponds to the motion of the joint of the robot 30. Move. The monitoring point may be a hand position of the robot 30, or may be a position away from the hand position by a predetermined distance in a designated direction. The data indicating the monitoring location is stored in the storage device 3 similarly to the data indicating the housing model of the robot 30. The robot control device 1 can calculate the position of the monitoring point from the angle of each joint of the robot 30 by performing the same process as the process for the robot posture.

After the process of step S22 is performed, each of the first monitoring point position calculation unit 143, the second monitoring point position calculation unit 144, and the third monitoring point position calculation unit 145 monitors the operation speed of the robot 30. One monitoring point is selected (S23). When a plurality of monitoring points are set, each of the first monitoring point position calculation unit 143, the second monitoring point position calculation unit 144, and the third monitoring point position calculation unit 145 is one of the plurality of monitoring points. Select the monitoring point of. When a plurality of monitoring points are set, each of the first monitoring point position calculation unit 143, the second monitoring point position calculation unit 144, and the third monitoring point position calculation unit 145 selects from a list including data indicating the monitoring points. One monitoring location may be selected at random, or one monitoring location may be selected according to a predetermined rule.

Then, the first monitoring point position calculation unit 143 calculates the first monitoring point position P_A143a, the second monitoring point position calculation unit 144 calculates the second monitoring point position P_B144a, and the third monitoring point position calculation unit 145 The three monitoring point position P_C145a is calculated (S24). The first monitoring point position P_A143a is a command value movement amount before the smoothing (θ_i( which is a difference between the smoothed joint angle command values φ_i(k) and the respective joint angle command values before the smoothing for the selected monitoring point. k)−θ_i(k−1)) is added to the joint angle for forward conversion. The second monitoring point position P_B144a is the smoothed command value movement amount (φ_i( which is the difference between the smoothed joint angle command values φ_i(k) and the respective smoothed joint angle command values for the selected monitoring point. k)−φ_i(k−1)) is added to the joint angle for forward conversion. The third monitoring point position P_C145a is calculated by forward-converting the smoothed joint angle command value φ_i(k) for the selected monitoring point. The first monitoring point position P_A143a, the second monitoring point position P_B144a, and the third monitoring point position P_C145a may be calculated in any order.

Next, the first speed calculation unit 146 calculates the first estimated speed, and the second speed calculation unit 147 calculates the second estimated speed (S25). The first estimated speed may be represented as “first estimated speed V1” and the second estimated speed may be represented as “second estimated speed V2”. Either the first estimated speed V1 or the second estimated speed V2 may be calculated first. The first estimated speed V1 is calculated by the following equation (7), and the second estimated speed V2 is calculated by the following equation (8).
V1=norm(P_AP_C)/ΔT/OVRD (7)
V2=norm(P_B-P_C)/ΔT/OVRD (8)
The first monitoring point position P_A143a, the second monitoring point position P_B144a, and the third monitoring point position P_C145a are all three-dimensional positions. The norm operator calculates the Euclidean norm of an array. Each of the first estimated speed V1 and the second estimated speed V2 can be converted into a speed when the override is 1 by dividing by the value of the override 15a.

After the first estimated speed V1 and the second estimated speed V2 are calculated, the representative speed calculation unit 148 determines which of the first estimated speed V1 and the second estimated speed V2 is larger (S26). When it is determined that the first estimated speed V1 is equal to or higher than the second estimated speed V2 (Yes in S26), the representative speed calculation unit 148 updates the representative monitored speed V with V=max(V, V1) (S27). .. That is, in step S27, the representative speed calculation unit 148 sets the first estimated speed V1 as the representative monitoring speed V. When the representative speed calculation unit 148 determines that the first estimated speed V1 is smaller than the second estimated speed V2 (No in S26), the representative monitored speed V is updated to V=max(V, V2) (S28). That is, in step S28, the representative speed calculation unit 148 sets the second estimated speed V2 as the representative monitoring speed V. Since the representative monitoring speed V has been initialized in step S21, the representative monitoring speed V becomes the second estimated speed V2 in the first processing.

Thereafter, the representative speed calculation unit 148 determines whether or not the processing from step S24 to step S28 has been performed for all the set monitoring points (S29). When the representative speed calculation unit 148 determines that the processing from step S24 to step S28 has not been performed for all the monitoring points (No in S29), the operation of the speed estimation unit 14 proceeds to step S23. In step S23, each of the first monitoring point position calculation unit 143, the second monitoring point position calculation unit B144, and the third monitoring point position calculation unit 145 monitors all the monitoring points to the monitoring points selected in step S23 from the previous time. One monitoring point is selected from the monitoring points excluding. After the process of step S23 is performed, the processes of steps S24 to S28 described above are performed.

When the representative speed calculation unit 148 determines that the processing from step S24 to step S28 has been performed for all the monitoring points (Yes in S29), it outputs the current representative monitoring speed V (S30). When the representative speed calculator 148 outputs the current representative monitoring speed V, the operation of the speed estimator 14 ends.

As described above, the robot control device 1 according to the first embodiment uses the pre-smoothing command 11a generated by the pre-smoothing command generation unit 11 and the post-smoothing command 12a generated by the smoothing unit 12 as a basis. In addition, the robot 30 has a speed estimation unit 14 that estimates the speed of a monitoring location provided on the robot 30. That is, the robot control device 1 estimates the speed estimated for adjusting the operation speed of the robot 30, using the command before the smoothing of the drive command that is actually output to the robot 30.

Therefore, the robot control device 1 can estimate the future operation speed that is closer to the actually detected operation speed of the robot 30 by the time required for the smoothing process, and the robot 30 is estimated using the estimated operation speed. The operating speed of can be adjusted. That is, the robot control device 1 can estimate the increase in the operation speed of the robot 30 in the near future.

The robot control device 1 has an override correction unit 15 that corrects an override that is a virtual time correction rate based on the representative monitoring speed 14a estimated by the speed estimation unit 14 and a predetermined speed limit. Therefore, the robot control device 1 can output a smooth drive command after smoothing to the robot 30, and can suppress the possibility that the operating speed of the robot 30 exceeds the speed limit.

That is, the robot control device 1 can operate the robot 30 at an operation speed within the limit value even if the process of smoothing the command is performed to smoothly operate the robot 30. Furthermore, the robot control device 1 estimates the operating speed of the monitoring location provided in the robot 30 based on the pre-smoothing command 11a and the post-smoothing command 12a. Therefore, the robot control device 1 suppresses the operating speed of the monitoring location within the speed limit without generating acceleration or vibration exceeding the allowable value of the electric motor of the drive device attached to each joint of the robot 30. The robot 30 can be operated.

Embodiment 2.
FIG. 8 is a block diagram for explaining the function of the robot control device 1A according to the second embodiment. In FIG. 8, the same components as those shown in FIG. 2 are designated by the same reference numerals. In the second embodiment, description of the components described in the first embodiment will be omitted. The robot controller 1</b>A has a pre-smoothing command generation unit 11, a smoothing unit 12, a command output unit 13, a speed estimation unit 14, and an override correction unit 15, and is obtained by the override correction unit 15. The suppression unit 20 that receives the override 15a and outputs the suppression override 20a is included. The pre-smoothing command generation unit 11 uses the suppression override 20a output from the suppression unit 20 to generate the pre-smoothing command 11a.

FIG. 9 is a flowchart showing an operation procedure of the suppression unit 20 included in the robot control device 1A according to the second embodiment. The suppression unit 20 acquires the override 15a output from the override correction unit 15 (S31). The suppression unit 20 performs suppression processing on the acquired override 15a (S32) and generates the suppression override 20a. Details of the suppression processing will be described later. The suppression unit 20 stores the generated suppression override 20a (S33), and then outputs the suppression override 20a to the pre-smoothing command generation unit 11 (S34). The suppression unit 20 has a memory for storing information.

Next, the suppression process performed in step S32 will be described. The suppression unit 20 determines whether the acquired value of the override 15a is larger than the value of the suppressed override 20a stored in the immediately preceding control cycle. When the suppression unit 20 determines that the acquired value of the override 15a is larger than the value of the suppressed override 20a stored in the immediately preceding control cycle, the suppression unit 20 sets the value of the suppressed override 20a in the current control cycle to the value of the acquired override 15a. The value is set to a value between the value and the stored value of the suppression override 20a, and the suppression override 20a after the setting is output. When the suppression unit 20 determines that the acquired value of the override 15a is less than or equal to the value of the suppression override 20a stored in the immediately previous control cycle, the suppression unit 20 outputs the suppression override 20a in the current control cycle.

The suppression unit 20 suppresses the rate of increase in the value of the suppression override 20a output in step S34 in each control cycle by performing the above-described processing in step S32. The suppression unit 20 may suppress the increase amount by presetting the upper limit of the stored increase amount of the suppression override 20a in each control cycle. The suppression unit 20 suppresses the increase amount by adding to the stored suppression override 20a a value obtained by multiplying the difference between the stored suppression override 20a and the input override 15a by a constant discount rate. Good.

In the second embodiment, the suppression unit 20 suppresses the increase width of the override 15a when the override 15a increases. Therefore, the robot control device 1A according to the second embodiment can suppress the increase rate of the operating speed of the robot 30 and reduce the possibility that the operating speed exceeds the speed limit. Since the robot control device 1A does not affect the command for making the operation of the robot 30 relatively slow, the operation of the robot 30 is made relatively slow when it is determined that the operating speed of the robot 30 exceeds the speed limit. It is possible to immediately output the command to perform, and it is possible to reduce the possibility that the operating speed of the robot 30 exceeds the speed limit.

Note that the override modifying unit 15 may obtain the modified overriding so that the amount of increase of the overriding 15a obtained by performing the modification in each control cycle is less than a predetermined value. In that case, the robot control device 1A does not have to include the suppression unit 20. In that case, the robot control device 1A can reduce the possibility that the operating speed of the robot 30 exceeds the speed limit by suppressing the increase width of the override.

Embodiment 3.
FIG. 10 is a block diagram for explaining the function of the robot control device 1B according to the third embodiment. In FIG. 10, the same components as those shown in FIG. 2 are designated by the same reference numerals. In the third embodiment, description of the components described in the first embodiment will be omitted. The robot control device 1B includes a pre-smoothing command generation unit 11, a smoothing unit 12, a command output unit 13, a speed estimation unit 14A, and an override correction unit 15. That is, the robot control device 1B has a speed estimation unit 14A instead of the speed estimation unit 14 included in the robot control device 1 according to the first embodiment.

Like the speed estimation unit 14, the speed estimation unit 14A includes a first speed calculation unit 146 and a second speed calculation unit 147. The first speed calculation unit 146 included in the speed estimation unit 14A calculates the pre-smoothing command value movement amount 141a based on the pre-smoothing command 11a which is the basis of the post-smoothing command 12a output to the robot 30 in the future. To do. The second speed calculator 147 included in the speed estimator 14A calculates the smoothed command value movement amount 142a based on the smoothed command 12a output to the robot 30 in the future.

FIG. 11 is a flowchart showing an operation procedure of the speed estimation unit 14A included in the robot control device 1B according to the third embodiment. In FIG. 11, the same steps as those shown in FIG. 7 are designated by the same reference numerals. In the third embodiment, the description of the steps described in the first embodiment will be omitted. After executing the processing of step S29 described with reference to FIG. 7, the speed estimation unit 14A determines whether or not to repeatedly execute the processing of steps S22 to S29 (S41).

When it is determined that the processing from step S22 to step S29 is not repeatedly executed (No in S41), the speed estimation unit 14A outputs the current representative monitoring speed V (S30). When the current representative monitoring speed V is output, the speed estimating unit 14A ends the operation. When it is determined that the process from step S22 to step S29 is to be repeatedly executed (Yes in S41), the speed estimation unit 14A updates the pre-smoothing command 11a and the post-smoothing command 12a based on the current value of the override 15a. Then (S42), the operation of the speed estimating unit 14A proceeds to step S22. The process of step S42 is similar to the process performed by the pre-smoothing command generation unit 11 and the smoothing unit 12.

The speed estimating unit 14A estimates the drive command to be output to the robot 30 in the future under the current override 15a in the current control cycle by executing the processes of steps S22 to S29, step S41, and step S42. Then, the operation speed of the robot 30 is estimated based on the estimated drive command. Therefore, in the processing from step S22 to step S29, step S41, and step S42, when the drive command based on the current override 15a is continuously output to the robot 30, the future operation speed of the robot 30 becomes the speed limit. In other words, it is a process for determining whether or not there is a possibility of exceeding.

The process of step S41 is a process of considering the operation speed of the robot 30 according to a drive command output to the robot 30 a certain number of times from the current control period in the future control period. The number of times may be set in advance or may be the number of times the drive command is output to the robot 30 until the posture of the robot 30 reaches the target point. The number of times may be specified not to be a fixed number of times but to increase as the operation speed of the robot 30 increases.

Based on the drive command to be output to the robot 30 in the future, assuming that the drive command based on the current override 15a is continuously output to the robot 30, the speed estimation unit 14A determines, The operation speed of the robot 30 in the future can be estimated. Therefore, the robot control device 1B according to the third embodiment can adjust the operation speed of the robot 30 according to the estimated operation speed, and thus the operation speed of the robot 30 may exceed the speed limit. It can be reduced.

FIG. 12 is a partial or all function of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 that the robot control device 1 according to the first embodiment has. FIG. 9 is a diagram showing a processor 81 when is realized by a processor 81. That is, some or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 execute the processor 81 that executes the program stored in the memory 82. May be realized by The processor 81 is a processing device, a computing device, a microprocessor, or a DSP (Digital Signal Processor). The memory 82 is also shown in FIG.

When a part or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 are realized by the processor 81, the part or all of the functions are performed. Is realized by the processor 81 and software, firmware, or a combination of software and firmware. The software or firmware is described as a program and stored in the memory 82. The processor 81 reads a program stored in the memory 82 and executes the program to execute a part of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15, or Realize all functions.

When some or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 are realized by the processor 81, the robot control device 1 performs smoothing. It stores a program that results in the steps executed by part or all of the pre-ization command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15. It has a memory 82 for. The program stored in the memory 82 causes a computer to execute a procedure or method executed by a part or all of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15. It can also be said to be something to be executed.

The memory 82 is, for example, a nonvolatile or volatile semiconductor memory such as RAM, ROM, flash memory, EPROM (Erasable Programmable Read Only Memory), EEPROM (registered trademark) (Electrically Erasable Programmable Read-Only Memory), a magnetic disk, It is a flexible disk, an optical disk, a compact disk, a mini disk or a DVD (Digital Versatile Disk). The memory 82 may be the storage device 3 or may be included in the storage device 3.

FIG. 13 shows that part or all of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 included in the robot control device 1 according to the first embodiment are processed. It is a figure which shows the processing circuit 91 when implement|achieved by the circuit 91. That is, part or all of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 may be realized by the processing circuit 91.

The processing circuit 91 is dedicated hardware. The processing circuit 91 is, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof. Is.

Regarding the plurality of functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15, some of the plurality of functions are realized by software or firmware. The rest of the plurality of functions may be realized by dedicated hardware. As described above, the plurality of functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, and the override correction unit 15 are implemented by hardware, software, firmware, or a combination thereof. Can be realized.

Some or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, the override correction unit 15, and the suppression unit 20 included in the robot control device 1A according to the second embodiment. May be implemented by a processor executing a program stored in memory. In the memory, some or all steps executed by the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, the override correction unit 15, and the suppression unit 20 are executed as a result. It is a memory for storing a program to be executed. The processor is the same processor or CPU as the processor 81. A part or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14, the override correction unit 15, and the suppression unit 20 may be realized by a processing circuit. The processing circuit is a processing circuit similar to the processing circuit 91.

Some or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14A, and the override correction unit 15 included in the robot control device 1B according to the third embodiment are stored in a memory. It may be realized by a processor that executes a stored program. In the memory, some or all steps executed by the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14A, and the override correction unit 15 are executed as a result. Is a memory for storing the program. The processor is the same processor or CPU as the processor 81. Some or all of the functions of the pre-smoothing command generation unit 11, the smoothing unit 12, the command output unit 13, the speed estimation unit 14A, and the override correction unit 15 may be implemented by a processing circuit. The processing circuit is a processing circuit similar to the processing circuit 91.

The configurations described in the above embodiments are examples of the content of the present invention, and can be combined with another known technique, and the configurations of the configurations are not deviated from the scope not departing from the gist of the present invention. It is also possible to omit or change parts.

1, 1A, 1B robot control device, 1a robot operation command, 2 CPU, 3 storage device, 4 IO interface, 5 bus, 6 receiving device, 7 input device, 8, 9 cable, 11 pre-smoothing command generation unit, 11a Pre-smoothing command, 12 smoothing part, 12a post-smoothing command, 13 command output part, 14,14A speed estimation part, 14a representative monitoring speed, 15 override correction part, 15a override, 20 suppression part, 20a suppression override, 30 Robot, 111 virtual time calculation unit, 111a virtual time, 112 operation planning unit, 112a operation parameter, 113 command value updating unit, 141 first movement amount acquisition unit, 141a pre-smoothing command value movement amount, 142 second movement amount acquisition Part, 142a smoothed command value movement amount, 143 first monitoring point position calculation part, 144 second monitoring point position calculation part, 145 third monitoring point position calculation part, 146 first speed calculation part, 146a first estimated speed 147 second speed calculation unit, 147a second estimated speed, 148 representative speed calculation unit, 81 processor, 82 memory, 91 processing circuit.

Claims (6)

A robot controller for controlling a robot having a plurality of arms,
A pre-smoothing command generation unit that generates a pre-smoothing command that is a command for driving the plurality of arms of the robot;
A smoothing unit that smoothes the pre-smoothing command generated by the pre-smoothing command generation unit to generate a post-smoothing command;
A command output unit that outputs the smoothed command generated by the smoothing unit to the robot,
A speed for estimating the operation speed of a monitoring point provided in the robot based on the pre-smoothing command generated by the pre-smoothing command generation unit and the post-smoothing command generated by the smoothing unit An estimation section,
An override correction unit that corrects an override that is a virtual time correction rate based on an operation speed estimated by the speed estimation unit and a predetermined speed limit,
The robot controller according to claim 1, wherein the pre-smoothing command generation unit generates the pre-smoothing command using the override obtained by the override correction unit. The speed estimation unit,
Based on the position of the monitoring point calculated based on the post-smoothing command and the pre-smoothing command value movement amount that is the movement amount of the monitoring point calculated based on the pre-smoothing command. And a first speed calculator for calculating a first estimated speed of the monitoring point,
Based on the position of the monitoring point calculated based on the post-smoothing command and the smoothed command value movement amount that is the movement amount of the monitoring point calculated based on the post-smoothing command. And a second speed calculator for calculating the second estimated speed of the monitoring point,
The robot controller according to claim 1, further comprising: a representative speed calculator that estimates an operation speed of the monitoring location based on the first estimated speed and the second estimated speed. The first speed calculation unit calculates the pre-smoothing command value movement amount based on the pre-smoothing command, which is the basis of the post-smoothing command output to the robot one cycle ago,
The robot according to claim 2, wherein the second speed calculator calculates the smoothed command value movement amount based on the smoothed command output to the robot one cycle before. Control device. The first speed calculation unit calculates the pre-smoothing command value movement amount based on the pre-smoothing command, which is the basis of the post-smoothing command to be output to the robot in the future,
The robot controller according to claim 2, wherein the second speed calculator calculates the smoothed command value movement amount based on the smoothed command that is output to the robot in the future. The representative speed calculation unit estimates a larger estimated speed of the first estimated speed and the second estimated speed as an operation speed of the monitoring location. The robot controller according to item 1. 6. The overriding correction unit obtains the overriding after the correction so that the amount of increase in each overriding control cycle obtained by performing the correction is less than a predetermined value. The robot controller according to item 1.

Images