JP6911793B2 - Robot control device - Google Patents

Description

The present invention relates to a robot control device.

A rotation sensor such as a rotary encoder is attached to a motor that drives each axis of the robot arm. Then, the arm of the robot is driven via a deceleration mechanism connected to the rotation shaft of the motor. In order to ensure safety when such a robot operates, for example, as shown in FIG. 8, the rotation shaft 25 of the arm that has passed through the reduction mechanism 24, that is, the rotation sensor 28 is also provided on the output side to control the operating speed of the arm. It is conceivable to detect directly and stop the drive of the motor when the operating speed exceeds the upper limit value.

Further, for example, as shown in Patent Document 1, a process of detecting the number of times the motor has rotated twice or more may be performed by referring to the rotation position signal output from the rotation sensor on the output side.

JP 2015-142948

By the way, in the mechanism from the rotation shaft of the motor to the rotation shaft of the arm via the reduction mechanism, there is generally a backlash between the gears constituting each part. Therefore, as the number of gear stages increases, the error due to the accumulation of backlash increases, which may affect control.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a robot control device capable of eliminating the influence of backlash generated in the drive mechanism of an arm.

According to the robot control device according to the first aspect, when the control unit restrains the rotation shaft on the arm side on the output side of the reduction mechanism by the mechanical brake, the restraining force of the mechanical brake is applied to the driving torque of the motor. Set a torque limit value that is lower than that. Then, the motor is rotated until the drive torque of the motor reaches the torque limit value to eliminate the backlash.

That is, if the motor is rotated while the rotation shaft on the arm side is restrained, the motor will rotate by the amount of the backlash, and when the gap is eliminated, the rotation will stop and the motor will stop. Drive torque increases. Therefore, when the drive torque reaches the torque limit value, it can be determined that the gap for the backlash has disappeared, and the backlash can be eliminated to improve the control accuracy of the motor.

According to the robot control device according to claim 2, when the control unit restrains the rotation axis on the arm side by the mechanical brake, the drive of the motor by the drive circuit is stopped, and the posture of the robot arm rotates. Judge whether or not the posture is such that gravity acts on the axis. Then, if the posture is not such that gravity acts on the rotation axis, the backlash elimination process is performed.

In general, the robot arm is often stopped in a posture in which gravity acts on the axis of rotation. In that case, if the drive of the motor is stopped while the rotation shaft on the arm side is restrained, the motor becomes a rotation-free state, and the rotation shaft rotates in the direction in which gravity acts and stops. This eliminates backlash. Then, since the backlash elimination process is performed only when the posture is a special posture in which gravity does not act on the rotation axis, the process can be completed in a short time.

According to the robot control device according to claim 3, the control unit performs multi-rotation detection processing for obtaining the number of rotations of the rotation shaft of the motor by referring to the rotation position obtained from the arm-side rotation sensor. In performing such processing, by eliminating the backlash in advance, the number of rotations can be detected with high accuracy.

A diagram showing a configuration around each axis of a robot and a configuration of a control unit of a controller related thereto as a model, which is one embodiment. Functional block diagram showing the main parts of the controller Diagram showing the configuration of the robot system Flow chart showing the process of eliminating backlash in the deceleration mechanism performed by the control microcomputer Flowchart showing the process of finding the speed of the rotating shaft executed by the speed microcomputer The figure which shows the differential value of a hall sensor signal The figure which shows the combined speed from two differential values Diagram showing the configuration around each axis of the robot as a model

Hereinafter, one embodiment will be described with reference to FIGS. 1 to 8. As shown in FIG. 3, the robot system 1 includes a vertically articulated robot 2 and a controller 3 that controls the robot 2. This robot system 1 is used for general industrial use. The robot 2 has a well-known configuration as a so-called 6-axis vertical articulated robot, and the shoulder 6 rotates horizontally on the base 5 via a first axis (J1) having an axis in the Z direction. It is connected as possible. A lower end portion of a lower arm 7 extending upward via a second axis (J2) having an axial center in the Y direction is rotatably connected to the shoulder 6. The first upper arm 8 is rotatably connected to the tip of the lower arm 7 via a third axis (J3) having an axial center in the Y direction.

A second upper arm 9 is rotatably connected to the tip of the first upper arm 8 via a fourth axis (J4) having an axis in the X direction. A wrist 10 is rotatably connected to the tip of the second upper arm 9 via a fifth axis (J5) having an axis in the Y direction. A flange 11 is twistably and rotatably connected to the wrist 10 via a sixth axis (J6) having an axis in the X direction.

The base 5, the shoulder 6, the lower arm 7, the first upper arm 8, the second upper arm 9, the wrist 10 and the flange 11 function as the arm of the robot 2, and the flange 11 which is the tip of the arm holds the work. A tool (not shown) is attached to this. Each axis (J1 to J6) provided in the robot 2 is provided with a motor (not shown) serving as a drive source corresponding to the shaft (J1 to J6).

The controller 3 is a control device for the robot 2, and controls the robot 2 by executing a computer program in a control means including a computer composed of a CPU, a ROM, a RAM, and the like (not shown). Specifically, the controller 3 includes a drive unit composed of an inverter circuit or the like, and each of them is controlled by feedback, for example, based on the rotation position of the motor detected by the encoder provided corresponding to each motor. Drive the motor.

The controller 3 includes a CPU, a ROM, a RAM, a motor driver corresponding to the drive unit, and the like. The ROM stores a system program, an operation program, and the like of the robot 2. The RAM stores parameter values and the like when executing these programs. In the position detection circuit, the controller 3 is input with the detection signals of each encoder (not shown) provided at each joint of the robot 2.

FIG. 2 is a functional block diagram showing a portion of the controller 3 related to the main part of the present embodiment. The controller 3 includes a main CPU 12, a control microcomputer 13 and a speed microcomputer 14 provided corresponding to each of the axes J1 to J6. A user operation unit 15 such as a teaching pendant is connected to the controller 3, and the user sets various parameters for controlling the robot 2 by the user operation unit 15. The main CPU 12 gives an angle command and a monitoring speed to the control microcomputer 13 and the speed microcomputer 14 of each axis, respectively. The monitoring speed is an upper limit value for monitoring the rotation speed of each axis.

FIG. 1 modelally shows the configuration around each axis of the robot 2 and the configuration of the control unit of the controller 3 related thereto. The configuration around the axis is basically the same as the image shown in FIG. 8, and the same reference numerals are given to FIG. A motor-side encoder 22 is attached to the motor 21. The encoder 22 inputs the rotation angle of the motor 21 to the control microcomputer 13. The rotating shaft 23 of the motor 21 is connected to the input side of the reduction mechanism 24, which is a module composed of a plurality of gears. The output side of the speed reduction mechanism 24 is connected to the rotation shaft 25 of the corresponding robot arm.

A mechanical brake 26 for controlling the movement of the arm is arranged on the rotating shaft 25. Further, a permanent magnet 27 is attached to the rotating shaft 25, and changes in the magnetic field of the permanent magnet 27 due to the rotation of the rotating shaft 25 are detected by two Hall sensors 28 (1) and 28 (2). NS. The Hall sensors 28 (1) and 28 (2) output rotation position signals and sensor signals whose phases differ by 90 degrees.

The sensor signals output from the Hall sensors 28 (1) and 28 (2) are amplified by the amplifiers 29 (1) and 29 (2), respectively, and the A / D converter 30 (1) included in the speed microcomputer 14 is provided. , 30 (2). The data A / D converted by the A / D converters 30 (1) and 30 (2) is input to the angle / velocity calculation unit 31.

The calculation unit 31 calculates the rotation angle and rotation speed of the rotation shaft 25 based on the input data of the two-phase sensor signal. The calculated rotation angle and arm angle are input to the control microcomputer 13. Further, the calculated rotation speed is compared with the monitoring speed given by the main CPU 12 in the calculation unit 31. Then, when the rotation speed of the arm exceeds the monitoring speed, the speed microcomputer 14 outputs a stop command to the control microcomputer 13.

The control microcomputer 13 controls the motor 21 so that the arm angle to be controlled matches the angle command given by the main CPU 12. The control signal of the motor 21 is given to the motor 21 via the amplifier 32 and the motor driver. The control microcomputer 13 also controls ON / OFF of the brake 26. When a stop command is input from the speed microcomputer 14, the control microcomputer 13 turns on the brake 26 and stops the drive control of the motor 21. Further, the control microcomputer 13 performs a multi-rotation speed detection process for detecting how many rotations the motor 21 is, as in Patent Document 1, by referring to the arm angle.

Next, the operation of this embodiment will be described with reference to FIGS. 4 to 7. FIG. 4 is a process performed by the control microcomputer 13 for eliminating backlash in the speed reduction mechanism 24. First, the rotating shaft 25 is locked by the brake 26 (S1). Then, all the switching elements such as FETs constituting the motor driver are turned off to put the motor 21 in a free state (S2). Subsequently, it is determined whether or not each arm axis is in a posture in which it does not move due to the action of gravity (S3).

Here, if the robot 2 is in a posture in which gravity acts on the rotation shaft 25 of the arm (NO), the backlash is eliminated at the time of step S2. Therefore, in this case, only the low-side FET of the motor driver is turned on to put the motor 21 in the short brake state and end the process (S8).

On the other hand, when the arm of the robot 2 is in a posture of being in a straight line along the direction of gravity (S3; YES), the backlash is not eliminated. Therefore, after setting the torque limit value so as to maintain the brake lock state (S4), the motor 21 is put into the servo lock state (S5). Then, the motor 21 is rotated in the clockwise direction or the counterclockwise direction under constant speed control at an extremely low speed (S6). This process is continued until the output torque of the motor 21 reaches the limit value (S7; NO), and when the torque reaches the limit value (YES), the process proceeds to step S8.
The output torque may be estimated by detecting, for example, the current applied to the motor 21, or the value of the q-axis current may be regarded as the torque when performing vector control. This process eliminates backlash. After that, when the drive control of the arm is started, the locked state by the brake 26 is released.

FIG. 5 shows a process of obtaining the rotation speed of the rotation shaft 25 executed by the speed microcomputer 14. The processes of steps S11 to S15 are executed in parallel for the hall sensors 28 (1) and 28 (2). The speed calculation unit 31 determines whether or not the conversion value output by the AD converter 30 has changed (S11). If the conversion value has not changed (NO), the counter for counting the change time is incremented (S15), and the process returns to step S11.

When the AD conversion value changes (S11; YES), the difference between the previous value and the current value of the conversion value is calculated (S12), and the time until the conversion value changes from the count value of the counter is obtained (S13). .. Then, the differential value is calculated by dividing the difference value by the change time (S14). The speed calculation unit 31 also has the functions of the first and second differentiators.

When the differential values of the sensor signals output by the Hall sensors 28 (1) and 28 (2) are obtained as described above, these are combined to obtain the speed of the rotating shaft 25 (S16). That is, assuming that each differential value is a differential value (1) or (2),
(Composite speed) = √ {differential value (1) ² + differential value (2) ² }
Ask more.

The speed microcomputer 14 compares the speed obtained by the above equation with the monitoring speed, and when the speed exceeds the monitoring speed, outputs a stop command to the control microcomputer 13 as described above. Then, the control microcomputer 13 turns on the brake 26 and stops the drive control of the motor 21. When monitoring the speed in this way, the square value V ² may be monitored instead of the combined speed V itself.

That is, in this process, it is assumed that the two-phase sensor signals H1 and H2 output by the Hall sensors 28 (1) and 28 (2) are, for example, sin (ωt) and cos (ωt), respectively. By differentiating these, velocities V1 and V2 can be obtained, respectively.
V1 = dH1 / dt = V1 = ω × cos (ωt)
V2 = dH2 / dt = V2 = -ω x sin (ωt)
Then, the speed V obtained in step S16 is
V = √ {V1 ² + V2 ² }
Is.

The differential values of the signals output by the Hall sensors 28 (1) and 28 (2) are as shown in FIG. 6, and the velocity V obtained by synthesizing these differential values is as shown in FIG. Become. The accuracy of this is improved by about 30 times when the angle θ is obtained from the sensor signal and the speed obtained by differentiating the angle θ and the swing width between the maximum and minimum variations are compared.

As described above, according to the present embodiment, the hall sensors 28 (1) and 28 (2) are arranged on the rotation shaft 25 side of the robot arm so as to output sensor signals having a phase difference of 90 degrees from each other. When the speed calculation unit 31 of the speed microcomputer 14 differentiates each sensor signal, the speed calculation unit 31 calculates the sum of squares of the differential signals to obtain the rotation speed V of the arm. As a result, even if the Hall sensors 28, which have low resolution and are inexpensive, can be used, a rotation speed V with higher accuracy can be obtained as compared with the case where the rotation position is obtained from the sensor signals output by them and the speed is obtained. ..

Further, when the rotating shaft 25 is restrained by the brake 26, the control microcomputer 13 sets a torque limit value lower than the restraining force of the brake 26 for the driving torque of the motor 21. Then, the motor 21 is rotated until the drive torque of the motor 21 reaches the torque limit value to eliminate the backlash.

That is, if the motor 21 is rotated while the rotating shaft 25 is restrained, the motor 21 will rotate by the amount of the presence of the backlash, and the rotation will stop when the gap for that amount disappears, and the motor The drive torque of 21 increases. Therefore, when the drive torque reaches the torque limit value, it can be determined that the gap for the backlash has disappeared, and the backlash can be eliminated to improve the control accuracy of the motor 21.

At this time, when the rotating shaft 25 is restrained by the brake 26, the control microcomputer 13 makes the driving of the motor 21 stopped by the motor driver, and whether or not the posture of the robot arm is the posture in which gravity acts on the rotating shaft. To judge. Then, if the posture is not such that gravity acts on the rotation axis, the backlash elimination process is performed.

In general, the robot arm is often stopped in a posture in which gravity acts on the axis of rotation. In that case, if the drive of the motor 21 is stopped while the rotation shaft on the arm side is restrained, the motor 21 becomes a rotation-free state, and the rotation shaft 25 rotates in the direction in which gravity acts and stops. This eliminates backlash. Then, since the backlash elimination process is performed only when the rotation shaft 25 is in a special posture in which gravity does not act, the process can be completed in a short time.

Further, the control microcomputer 13 performs a multi-rotation detection process for determining the number of rotations of the rotation shaft 22 of the motor 21 by referring to the sensor signal obtained from the Hall sensor 28. In performing such processing, by eliminating the backlash in advance, the number of rotations can be detected with high accuracy.

The present invention is not limited to the embodiments described above or in the drawings, and the following modifications or extensions are possible.
The processing of steps S2 and S3 may be omitted.
The rotation sensor is not limited to the magnetic sensor.
It may be applied to robots other than the vertical 6-axis type.

In the drawing, 1 is a robot system, 2 is a robot, 3 is a controller, 13 is a control microcomputer, 14 is a speed microcomputer, 21 is a motor, 24 is a reduction mechanism, 25 is a rotating shaft, 27 is a permanent magnet, and 28 is a hole. The sensor, 31 indicates an angle / speed calculation unit.

Claims (3)

It is applied to a robot arm drive mechanism composed of a motor, a deceleration mechanism connected to the rotation shaft of the motor, and a rotation shaft of the robot arm driven via the deceleration mechanism.
The drive circuit that drives the motor and
A mechanical brake that restrains the rotation axis of the arm,
A control unit that controls the drive of the mechanical brake and controls the drive of the motor via the drive circuit is provided.
When the control unit restrains the rotation shaft by the mechanical brake,
A control device for a robot that sets a torque limit value lower than the binding force of the mechanical brake for the drive torque of the motor and rotates the motor until the drive torque of the motor reaches the torque limit value. .. When the control unit restrains the rotation shaft by the mechanical brake,
With the drive of the motor stopped by the drive circuit, it is determined whether or not the posture of the robot arm is a posture in which gravity acts on the rotation axis.
The robot control device according to claim 1, wherein the backlash elimination process is performed unless the posture is such that gravity acts on the rotation axis. A motor-side rotation sensor that is placed on the motor and detects the rotation position of the motor,
It is provided with an arm-side rotation sensor that is arranged on the rotation axis of the arm and detects the rotation position of the rotation axis.
The robot control device according to claim 1 or 2, wherein the control unit performs a multi-rotation detection process for determining the number of times the rotation shaft of the motor has rotated by referring to a rotation position obtained from the arm-side rotation sensor.

Images