JP3302758B2 - Robot speed control method and device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot speed control method and apparatus for controlling the speed of driving by a servomotor in a robot whose movable part is driven by a servomotor.

[0002]

2. Description of the Related Art Conventionally, in industrial robots and the like,
A mechanism for moving a movable part such as a rotary joint axis and a translation axis of a robot according to a predetermined control pattern by a servomotor is generally known.

In such a mechanism, a control pattern for moving the robot axis by driving the servo motor is generally as shown in FIG. That is,
The control pattern shown in FIG. 5 has an acceleration section SA, a constant speed section SC, and a deceleration section SD. The control pattern is accelerated to the required maximum speed MV in the acceleration section SA, and the required maximum speed MV is maintained in the constant speed section SC. Then, the vehicle is decelerated until it stops in the deceleration section SD, and the acceleration section SA and the deceleration section SD are symmetric.

In accordance with this control pattern, a target position is instructed every fixed minute time, and the target speed is adjusted according to the deviation between the target position and the actual position detected by the encoder. The servo motor drive current is controlled by feedback control so that the actual speed becomes the target speed in accordance with the deviation from.

[0005] In order to perform such control, it is necessary to input conditions for specifying the control pattern by specific numerical values.

Conventional control methods of this kind include a travel distance (PT), a required maximum speed (MV), an acceleration AC
Is input to the arithmetic control unit, and the arithmetic control unit calculates the acceleration (deceleration) time TA, the acceleration (deceleration) distance PA, the constant speed travel time TCa, and the constant speed travel distance PCa from these input values.

[0007]

## EQU1 ## Generally, the control pattern is specified by calculating TA = MV / AC, PA = MV ² / 2AC, TCa = PCa / MV, and PCa = PT-2PA.

FIG. 5 shows a trapezoidal control pattern in which the acceleration in the acceleration (deceleration) section is constant for the sake of simplicity of the description. Although adopting a smooth curve is performed, even in this case, the constant speed portion exists in common. The time and distance of the deceleration can be calculated with the same number of dimensions as in the case of the trapezoidal control pattern from the three conditions of the moving distance, the required maximum speed and the acceleration, although the calculation becomes somewhat complicated.

[0009]

In the conventional method as described above, the resolution of speed control is limited in relation to the output of the encoder, the sampling time, the approximation of the input in the servo motor control section, and the like. However, it has been difficult to achieve a high resolution of a certain level or more, and it has been difficult to control at an extremely low speed.

The problem will be described with specific numerical values as an example. The sampling time of the controller for the servo motor is 10 msec (1/100 sec = 1 csec), the rated rotation speed of the servo motor is 3000 rpm, and the encoder pulse number is 4000 p. 2000 rad /
In the case of ss, assuming that the rated rotation speed is the required maximum speed MV, the required maximum speed MV and the acceleration AC are

[0011]

MV = (3000/60 sec / 100) × 4000p = 2000 (p / csec) AC = (2000 / 2π) × 4000p / (100 × 100) = 127.32 (p / cse)
c ² ).

Then, the acceleration time TA and the acceleration distance PA are

[0013]

## EQU3 ## TA = 2000 / 127.32 = 15 (csec) PA = (2000 × 2000) / (2 × 127.32) = 15708 (p) The value on the rightmost side is truncated to the nearest integer.

Incidentally, PCa = PT-2PA, TCa
= PCa / MV, for example, if the moving distance PT is 5
Assuming 0000,

[0015]

PCa = 50000−2 × 15708 = 18584 TCa = 18584/2000 ≒ 9. From these, when the speed of the constant speed section is calculated backward,

[0016]

MV = 18584/9 = 2064.9, and the MV deviates from the input value (2000).

As described above, if the control is performed by defining the moving distance in relation to the processing such as fraction rounding, an error occurs in the speed MV of the constant speed section SC. In addition, due to the engineering limitations of the number of encoder pulses and the sampling time of the controller that can be used with current ordinary industrial robots, the absolute acceleration time is practically calculated on the order of tens of degrees. In relation to the conversion process, the resolution of the speed control in the constant speed section is limited to about 1/1000 of the rated speed.

Therefore, it is difficult to further increase the accuracy and resolution of speed control by the conventional method. In particular, when driving a servo motor at an extremely low speed, there is a limit to the speed reduction due to the above-mentioned resolution.

In view of the above circumstances, it is an object of the present invention to provide a method and an apparatus for controlling the driving of a robot by a servomotor, in which the resolution of speed control in a constant speed section is greatly increased as compared with the conventional art. It is another object of the present invention to enable control at an extremely low speed.

[0020]

In order to achieve the above object, a first aspect of the present invention is to provide a constant speed in which the motor is driven at a constant speed at the required maximum speed in an acceleration section from the start of driving of the servomotor to the required maximum speed. And a control pattern for normal control having a section and a deceleration section from the required maximum speed to stop, and an extremely low speed control for eliminating only the constant speed section by erasing the acceleration section and deceleration section from the control pattern for normal control. The control pattern can be selectively set, and information indicating whether to request the normal control or the extremely low speed control and the control condition including the assumed value of the constant speed moving distance are input to request the extremely low speed control. When the information shown is input, a control pattern for very low speed control is set, and a constant speed travel time assumption value (TC) given by a preset fixed value and the constant speed travel distance assumption value are given. Since the PC) request the maximum speed of the PC /
By using TC, data necessary for control by a control pattern for an extremely low speed is given, and the speed of the servomotor is controlled using this data.

The second invention relates to a speed control device for a robot used directly in the method of the first invention, comprising a servomotor for driving a movable portion of the robot, a servomotor control unit, In the robot speed control device that controls the servo motor based on the control pattern by the servo motor control unit, a control including information indicating whether to request the normal control or the extremely low speed control and an assumed value of the constant speed moving distance. Input means for inputting conditions, determining means for determining whether or not very low speed control is required based on information input by the input means, and acceleration from the start of driving of the servomotor until reaching the required maximum speed A control pattern for normal control including a section, a constant speed section in which the vehicle moves at a constant speed at the required maximum speed, and a deceleration section from the required maximum speed to the stop. Clear the acceleration section and the deceleration section from the control pattern for the normal control and the control pattern for the extremely low speed control to only the constant speed can be selectively set,
When the determination means determines that the extremely low speed control is required, the control pattern for the extremely low speed control is set, and the assumed value (TC) of the constant speed movement time given by a fixed value set in advance. Processing means for providing data necessary for control by a control pattern for an extremely low speed by setting PC / TC to the required maximum speed based on the assumption value (PC) of the constant-speed moving distance, and a drive amount of the servomotor. Detecting means for detecting, and control means for sequentially comparing the target value of the driving amount obtained from the data provided by the processing means with the driving amount detected by the detecting means, and controlling the servo motor driving current based on the comparison. It is provided with.

[0022]

According to the method of the first invention and the device of the second invention, it is possible to select between normal control and extremely low speed control.
When the extremely low speed control is selected, the control pattern for the normal speed control is deleted from the control pattern for the normal control, and the control pattern for the extremely low speed control is set to only the constant speed section by erasing the acceleration section and the deceleration section. Can be greatly improved as compared with the related art. Therefore, control when the required maximum speed is extremely low is performed with high resolution,
Very low speed control becomes possible.

[0023]

1 and 2 show an example of the speed control of a robot which does not constitute the present invention but can increase the resolution of speed control in a constant speed section.

FIG. 1 is a block diagram showing the configuration of a robot speed control device according to this embodiment. In FIG. 1, reference numeral 1 denotes a servo motor which drives a robot shaft (movable portion of a robot) such as a ball screw. I have. An encoder (detection means) 2 for detecting the rotation amount of the servo motor 1 to detect the position of the robot axis.
Is provided. The servo motor 1 and the encoder 2 are electrically connected to a servo motor control unit 10 formed by a microcomputer or the like.

The servo motor control unit 10 includes a condition input unit 11, an input value storage unit 12, a processing unit 13 for calculating and storing data for specifying a control pattern, a control pattern and a signal from the encoder 2. Control means 20 for controlling the drive current to servo motor 1 based on
And

In the condition input means 11, the assumed values TC and PC of the constant speed moving time and the constant speed moving distance and the acceleration AC
And the total travel distance PT.
The assumed values TC and PC are values for giving a value corresponding to the required maximum speed in the form of a fraction. That is, in the control pattern as shown in FIG. 5, the required maximum speed MV is the constant speed movement time TCa and the constant speed movement distance PCa (constant speed section SCa).
MV = PCa / TCa from the above), but the above assumed values TC and PC do not necessarily have to be the constant speed moving time TCa and the constant speed moving distance PCa according to the original request.
If PC / TC = PCa / TCa, required maximum speed MV
give. Therefore, in order to increase the resolution of speed control, the above assumed values TC and PC are set to the original constant speed movement time T.
Larger than Ca and the constant speed travel distance PCa, PC /
A value is input such that TC corresponds to the required maximum speed.
The input value storage means 12 stores a fractional expression (PC / TC) corresponding to the required maximum speed and other input values AC,
Store the PT.

The processing means 13 has an acceleration time / acceleration distance calculation means 14 and a movement data storage means 15,
The calculating means 14 calculates the acceleration time (the required time of the acceleration section SA) and the acceleration distance (the distance of the acceleration section SA) from the acceleration AC and the fractional expression (PC / TC) corresponding to the required maximum speed. The movement data storage means 15 stores the acceleration time and the acceleration distance and the fractional expression (PC
/ TC) is stored as movement data (data specifying a control pattern). Further, the processing means 13
It includes stop position determining means 16 for determining that the vehicle has approached the stop position based on the total travel distance PT.

The control means 20 includes a position command generation means 21,
It has a speed command generator 22 and a current generator 23.

Based on the movement data read from the movement data storage means 15, the position command generation means 21 obtains a target position in accordance with the movement data at regular small time intervals Δt (for example, 10 msec). Is output. In other words, in the acceleration section SA specified by the acceleration time and the acceleration distance, the target position at that time is calculated based on the elapsed time from the drive start time and the acceleration, and if the vehicle reaches the constant speed section SC after passing the acceleration section. , The fixed minute time Δ
PC / TC corresponding to the time and required maximum speed for each t
The target position is added by a value (Δt · PC / TC) multiplied by Then, a deviation between the target position and the actual position output from the encoder 2 is calculated, and the position deviation is input to the speed command generating means 22. When the stop position determining means 16 determines that the vehicle has approached the stop position, the position command generating means 21 sets the target position so as to reach the stop state through a deceleration section SD symmetrical to the acceleration section SA. Can be changed.

The speed command generating means 22 changes the target speed by a value proportional to the position deviation so that the actual position approaches the target position, and gives a target speed corresponding to the change in the target position. Then, a deviation between the target speed and the actual driving speed corresponding to the differential value (dp / dt) of the output of the encoder 2 is calculated, and this speed deviation is input to the current generating means 23.

The current generating means 23 controls the servo motor drive current so that the actual drive speed converges to the target speed by feedback control such as proportional differential control according to the speed deviation.

FIG. 2 is a flowchart showing a specific example of the speed control method performed by the control device shown in FIG.

In this flowchart, first, in step S1, as the processing of the condition input means 11, the assumed values TC and PC of the constant speed movement time and the constant speed movement distance such that the required maximum speed is PC / TC are set. , Acceleration AC,
The moving distance PT is input.

Subsequently, at step S2, the calculating means 14
As the processing of the acceleration time TA and the acceleration distance PA,

[0035]

## EQU6 ## TA = (1 / AC)） (PC / TC) PA = (１ ／) ・ (1 / AC) ・ (PC / TC) ² Then, in step S3, the servo motor 1
Is driven to start the movement of the robot axis.

After the movement of the robot axis is started, step S4
Then, it is determined as a process of the stop position determining means 16 whether or not the vehicle has approached the stop position. Specifically, the moving distance P
It is determined whether or not the current position detected by the encoder is equal to or greater than the value of PT-PA based on T and the acceleration distance PA equal to the distance of the deceleration section. It is checked whether it is within the distance of.

When the determination in step S4 is NO, the process proceeds to step S5, where the movement of the robot axis is continued, and the control by the control means 20 is performed based on the movement data. That is, the position command generating means 21 and the speed command generating means 2
2 and the current generating means 23, a position command based on the movement data as described above, a speed command according to a deviation from the actual position, and control of a servo motor drive current according to a deviation from the actual speed. Is performed. Then, while the determination in step S4 is NO, this control is repeated to shift to the constant speed section via the acceleration section, and the state where the drive speed is controlled to PC / TC corresponding to the required maximum speed is maintained. It is.

When the current position approaches the stop position, step S
If the determination in step 4 becomes YES, the deceleration of the robot axis is started (step S6), and then the robot axis is stopped (step S7).

According to this example, in the control of driving the robot axis by the servo motor 1, the resolution of the speed adjustment is greatly increased.

That is, the movement data is determined by the condition input at the step S1 and the calculation at the step S2, and the control based on the movement data is performed until the stop position is approached. In this case, in step S5, the control of the acceleration section SA and the constant speed section SC in the control pattern is accurately performed without being restricted by the moving distance. In the constant speed section SC, the moving speed reaches the required maximum speed. It is correctly controlled to have the corresponding value.

Further, in step S1, instead of a numerical value directly representing the required maximum speed MV, PC and TC are inputted so that PC / TC becomes the required maximum speed, and the required maximum speed is given in the form of a fraction. And this fraction in the form of a PC
By performing the calculation of the acceleration time TA and the acceleration distance PA in step S2 and the calculation and control in step S5 using / TC, the resolution of the required maximum speed is increased. For example, if you want the maximum required speed to be 2,000,
If TC = 20 and PC = 40,000, PC / TC =
40,000 / 20, and TC or P
If C is slightly changed, the required maximum speed changes subtly, and a high resolution can be obtained.

The control for stopping when the vehicle has moved by the necessary distance is achieved by the processing in steps S6 and S7 when the determination in step S4 is YES.

FIG. 3 is a block diagram showing the configuration of a robot speed control device according to an embodiment of the present invention. In this figure, a servo motor control unit 10 electrically connected to a servo motor 1 and an encoder 2
1, a determination means 32 for a required maximum speed, and a processing means 3 for calculating and storing data for specifying a control pattern
3 and control means 20 for controlling the servo motor drive current.

In the condition input means 31, information relating to the requested maximum speed, a provisional distance required for the extremely low speed control (a value which is an assumed value of the constant speed moving distance) is obtained by input processing according to the user's request. Control conditions such as acceleration and moving distance necessary for normal control are input. Further, in the determining means 32, it is determined whether or not the condition for performing the extremely low speed control is based on the information on the required maximum speed.
When the input value of the required maximum speed is set to zero, the control condition is determined to be an extremely low speed control condition, and otherwise the control condition is determined to be a normal control condition.

Therefore, when the user requests the extremely low speed control, the input value of the required maximum speed may be set to zero. In other words, in this embodiment, the information on the requested maximum speed also serves as information indicating which of the normal control and the extremely low speed control is required,
The user can select between the normal control and the extremely low speed control by inputting this information, and the determining means 32 determines whether the extremely low speed control is required based on this information. It is.

The processing means 33 can selectively set a control pattern for normal control and a control pattern for extremely low speed control, which will be described later, and set one of the control patterns in accordance with the judgment by the judgment means 32. At the same time, data necessary for control based on the set control pattern is given. The normal control condition calculating means 34, the low speed control condition setting means 35, the stop position determining means 36, the movement end determining means 37 for a temporary distance, It has moving data storage means 38.

More specifically, when it is determined by the determining means 32 that the normal control is requested, the normal control condition calculating means 34 controls the control pattern (servo motor drive) as shown in FIG. A control pattern for normal control including an acceleration section from the start of driving to the required maximum speed, a constant speed section for performing constant speed movement at the required maximum speed, and a deceleration section from the required maximum speed to stop is set. The time, distance, and the like of each of the acceleration, deceleration, and low-speed sections in this control pattern are calculated, and the data is stored in the movement data storage unit 38.

On the other hand, when the determining means 32 determines that the extremely low speed control is required, the low speed control condition setting means 35 sets conditions. in this case,
If the speed is extremely low, the acceleration and deceleration sections can be ignored. Therefore, the control pattern for the ultra-low speed control in which the acceleration section and the deceleration section are deleted from the control patterns for the normal control and only the constant speed section is set is set. In addition, a fixed value set in advance is provided as an assumed value (TC) of the constant speed moving time, and PC / PC is calculated from this value and the assumed value of the constant speed moving distance (PC).
By setting the TC to the required maximum speed, data necessary for control by the control pattern for extremely low speed control is given,
This data is stored in the moving data storage means 38. Further, in the case of the extremely low-speed control condition, determinations by the stop position determination means 36 and the movement end determination means 37 for the provisional distance are performed based on the movement distance and the provisional distance. The data provided by the storage means 38 is input to the control means 20.

The control means 20 has the same configuration as that shown in FIG.

FIG. 4 is a flowchart showing a specific example (an embodiment of the method of the present invention) of the speed control method performed by the control device shown in FIG.

In this flowchart, first, in step S11, as the processing of the condition input means 31, the movement distance PT, the acceleration AC, the required maximum speed MV, and the temporary distance PX are input. However, in the case of the normal control condition, the provisional distance is unnecessary, whereas in the case of the extremely low speed control condition, the acceleration AC is unnecessary and the input value of the required maximum speed MV is set to zero.

Subsequently, in step S12, it is determined whether or not the extremely low speed control condition is satisfied based on whether or not the input value of the required maximum speed MV is zero.

If the determination in step S12 is NO (normal control condition), in step S13, the normal control condition calculating means 34 executes the acceleration time TA and the acceleration distance P
A, the constant speed moving time TCa and the constant speed moving distance PCa are respectively:

[0054]

Equation 7: TA = MV / AC, PA = MV ² / 2AC TCa = PCa / MV, PCa = PT-2PA

Then, in step S14, the servo motor 1
The movement of the robot axis is started by driving the
In steps S15 and S16, the control under the normal control conditions is performed until the movement is completed.

On the other hand, the determination in step S12 is YES
In the case of (extremely low speed control condition), in step S17, the acceleration time TA and the acceleration distance PA are both set to 0 and fixed as the assumed value TC of the constant speed movement time as the processing of the low speed control condition setting means 35. A value is set, for example, TC = 30,000 is set as a value corresponding to 5 minutes when the sampling time is 10 msec. Further, the above temporary distance PX is given as PC. Then, in step S18, the movement of the robot axis by the drive of the servomotor 1 is started. Then, in step S19, it is determined whether or not the current position has reached the movement distance PT (the stop position determination means 3).
6), and in step S20, it is determined whether or not the current position has reached the temporary distance PX (processing as the movement end determination means 37 for the temporary distance).

Steps S19 and S20
If both determinations are NO, the process proceeds to step S21, in which the movement of the robot axis is continued.
The control is performed based on the data set in. More specifically, the target position is set to t · PX / 30,000 in accordance with the time t from the start of the movement, and the control by the control unit is repeated every predetermined minute time. Then, step S19
Is NO and the determination in step S20 is YES, the robot axis is re-moved, and the above control is continued. If the determination in step S19 is YES, the robot axis is stopped.

According to the method of the present embodiment as described above, it is possible to move the robot axis at an extremely low speed, which was difficult in the prior art.

That is, according to the specific example shown in the flowchart of FIG. 4, when extremely low speed control is performed, step S1 is executed.
Since TC = 30,000 given as a fixed value in 7 corresponds to 5 minutes, the provisional distance PX input in step S11 indicates the distance to move in 5 minutes. You just need to select it. For example, PX =
Assuming 300 (p), (300 / 30,000) · 100 = 1 (p / se
c), and an extremely low speed of one pulse per second can be realized. In such a case, the actual movement of the robot axis is approximately 0 when the calculated value (t · PX / 30,000) is considerably smaller than one pulse of the encoder in the control processing in step S21. Therefore, the robot axis does not substantially move, but if the calculated value approximates one pulse of the encoder after a certain period of time, the robot axis moves by one pulse. Thus,
Control in which the average moving speed becomes extremely low can be easily achieved by selecting the temporary distance PX.

In the embodiment shown in FIG. 4, the processing in the case of the normal control (steps S13 to S16) is the same as the conventional one, but in the case of the normal control, the same control as in the example of FIG. 2 is performed. You may do so.

[0061]

According to the control method and apparatus of the present invention,
When the control condition including information indicating whether to request the normal control or the extremely low speed control and the assumed value (PC) of the constant speed moving distance is input, when the information requesting the extremely low speed control is input, the normal control is performed. A control pattern for very low speed control, in which the acceleration section and the deceleration section are erased from the time control pattern and only the constant speed section is set, is assumed, and a constant value traveling time assumed value (TC) given as a fixed value. Since the required maximum speed is set to PC / TC based on the above-mentioned assumed value (PC) of the constant speed moving distance and control is performed by the control pattern for an extremely low speed, the accuracy and resolution of control at an extremely low speed are reduced. It is possible to achieve driving and control at an extremely low speed, which was difficult in the past.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating an example of a robot speed control device that can increase the resolution of speed control in a constant speed section.

FIG. 2 is a flowchart showing an example of a control method using the device shown in FIG.

FIG. 3 is a block diagram schematically illustrating an apparatus according to an embodiment of the present invention.

FIG. 4 is a flowchart illustrating an example of a control method using the apparatus according to the embodiment of the present invention.

FIG. 5 is a diagram showing a control pattern.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Servo motor 2 Encoder 10 Servo motor control unit 20 Control means 31 Condition input means 32 Judgment means 33 Processing means

Claims (2)

(57) [Claims] 1. A control system for normal control, comprising: an acceleration section from the start of driving of a servo motor to a required maximum speed, a constant speed section at a constant speed at the required maximum speed , and a deceleration section from the required maximum speed to a stop . Control pattern and this normal control
Acceleration section and deceleration section from control pattern for
Control pattern for very low speed control with only constant speed section
Information selectively settable and to indicate whether to request any of the usual control and the extremely low speed control
When the control condition including the assumed value of the constant-speed moving distance and the information indicating that the extremely low-speed control is required is input.
In addition to setting a control pattern for extremely low speed control,
The constant speed travel time given with a fixed value set in advance
The assumption value (TC) and the assumption value (PC) of the constant speed moving distance
Extremely low speed by setting the required maximum speed to PC / TC
A speed control method for a robot , which provides data necessary for control by a control pattern for a robot and uses the data to control the speed of a servo motor. With wherein comprising a servo motor for driving the movable portion of the robot comprises a servomotor control unit, by the servo motor controller, the robot-speed controller for controlling the servo motor based on the control pattern, Information indicating whether normal control or very low speed control is required
Enter the control conditions including the assumed value of the constant speed travel distance
Very low speed control based on input means and information input by the input means
Determining means for determining whether or not a request is made, and from the start of driving of the servo motor until the required maximum speed is reached.
Acceleration section, constant-speed section with constant speed movement at the required maximum speed,
Normal with deceleration section from required maximum speed to stop
Control pattern for control and this control pattern for normal control
Acceleration section and deceleration section are deleted from
Selectable control pattern for very low speed control
And extremely low speed control is requested by the determination means.
Control pattern for very low speed control
Sets the over emissions, with the advance settings boss were fixed value
The assumed value (TC) of the given constant speed travel time and the above constant speed travel
Requests PC / TC from the assumed value of moving distance (PC).
Control by control pattern for extremely low speed
Processing means for providing necessary data to the motor; detecting means for detecting the driving amount of the servo motor; and comparing the target value of the driving amount obtained from the data provided by the processing means with the driving amount detected by the detecting means. And a control means for controlling the servo motor drive current based on the speed control.