JP3182542B2 - Robot controller - 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 controller for performing control by applying feedforward to a servomotor.

[0002]

2. Description of the Related Art In an industrial robot, as shown in FIG. 8, interpolation is performed on the basis of data input by a trajectory generating function 12 of a higher-level control unit 11 to create robot position data q. Data q to command position interpolation function 14
, And outputs the result to the servo control unit 16. This is because the interpolation cycle ΔT of the trajectory generation function 12 needs to be larger than the cycle Δt of the servo controller 16 in order to perform complicated coordinate conversion. Conventionally, primary interpolation or secondary interpolation has been used as a method of dividing the interpolation value (position data q) calculated by the trajectory generation function 12 into m periods (ΔT = m · Δt) in the cycle of the servo control unit 16. . Then, on the servo control unit 16 side, based on the position data string sent by dividing into m, the velocity and acceleration calculation function 17 differentiates this value to further differentiate the velocity component and calculate the acceleration, In the servo control function 19, the speed feed forward and the acceleration feed forward are controlled by using a servo motor.

[0003]

However, according to the above-described primary interpolation, there is a problem that the speed command value and the acceleration command value are fixed at each interpolation cycle ΔT and change only in a step-like manner. That is, when a linear interpolation formula for the speed is obtained by differentiating the linear expression relating to the position, the speed does not become a function of the time t (the cycle of the servo control unit 16). It was constant during the interpolation period ΔT. Similarly, even if a quadratic interpolation is performed and a quadratic expression relating to the position is differentiated twice to obtain an acceleration interpolation formula, the acceleration does not become a function of the time t. It was constant. When controlling a robot, the rigidity of the arm to be controlled is very low compared to a machine tool, so it is necessary to output a position command to the servo with a smooth change in speed. Since the speed changes stepwise at ΔT, the robot could not be controlled smoothly. In the above method, the servo control unit 16 obtains the speed command value from the difference between the position data q and the current position, and further obtains the acceleration command value from the difference in speed. A time delay occurs between the position data and the calculated speed command value and acceleration command value, and the speed and acceleration are distorted when feedforward is applied.

SUMMARY OF THE INVENTION The present invention has been made to solve the above-mentioned problems, and an object of the present invention is to provide a robot control device capable of improving control characteristics of speed feed forward and acceleration feed forward. .

[0005]

As shown in FIG. 1, the configuration of the invention for solving the above-mentioned problem is to interpolate based on the input value to calculate position data q and drive the servo motor M1. In a robot controller 50 for controlling the position of the robot 10 by means of a trajectory generating means 1 for outputting position data at every constant interpolation cycle T, a third or higher order interpolation based on the position data output by the trajectory generating means 1 A command position interpolating means 2 which simultaneously calculates a position command value θi, a speed command value θ′i, and an acceleration command value θ ″ i according to the equation, and outputs these command values for every 1 / m of the interpolation cycle T. A servo control unit 3 that feeds forward to the servo motor M1 based on the position command value θi, the speed command value θ′i, and the acceleration command value θ ″ i output from the command position interpolating unit 2, Characterized by having That.

[0006]

According to the above means, the trajectory generating means 1 outputs the position data q of the robot 10 at every fixed interpolation cycle T.
Based on the position data q generated by the trajectory generating means 1, the command position interpolating means 2 simultaneously calculates a position command value θi, a speed command value θ′i, and an acceleration command value θ ″ i by a cubic interpolation formula. Then, these command values are output for each 1 / m of the interpolation cycle T. Then, the servo control means 3 feeds forward the servo motor M1 based on the position command value θi, the speed command value θ′i, and the acceleration command value θ ″ i output from the command position interpolation means 2, and 10 is performed. In this way, control is performed to feed forward the servo motor based on the position command value, speed command value, and acceleration command value calculated simultaneously.

[0007]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the robot control device according to the present invention will be described below with reference to the drawings. First, a robot 10 having six axes controlled by the robot controller of the present embodiment will be described with reference to FIG. Robot 10
A column 14 rotatably mounted on a pillar 12 fixed to a base 13, a first arm 15, a second arm 16, a third arm 17, a nozzle 19 of a sealer for sealing the coating agent, It is composed of And
A first axis a, a second axis b, a third axis c, a fourth axis d, a fifth axis e,
The sixth axis f allows the nozzle 19 to be fed freely with six degrees of freedom.

Next, the configuration of the robot control device 50 of this embodiment will be described with reference to FIG. The CPU 11 for performing the calculation for controlling the robot 10 shown in FIG. 2 includes a ROM 20 storing control information for performing control of the present embodiment and an R 20.
The AM 30, an operating box 27 and an operation panel 26 for inputting control commands, an external storage device 29 for storing control information and the like created by the CPU 11, and a servo controller 40 are connected. The servo controller 40 is connected to a servo motor M1 that drives the first axis a of the robot 10 to a servo motor M6 that drives the sixth axis f,
The position command value, the speed command value, and the acceleration command value calculated by the CPU 11 are fed forward to the servo motors M1 to M6, and the feedback signals from the pulse encoders E1 to E6 attached to the respective servo motors M1 to M6 are fed back. It is supposed to be. In this robot control device 50, the CPU 11 calculates a position command value, a speed command value, and an acceleration command value based on the input teaching point and sends an output to the servo control unit 40. The servo control unit 40 It is configured to control M1 to M6 to drive the robot 10 and to send a nozzle 19 for performing sealing.

Next, the processing by the CPU 11 of the robot controller 50 of the present embodiment will be described with reference to the flowcharts of FIGS. When a program including a teaching point sequence is input by the operator (step 1), the CP
U11 starts the decoding process of the program (Step 2). If the decoded instruction is not an operation end instruction (No in Step 3), it is determined whether the instruction is an operation instruction or a non-operation instruction (Step 4). If the instruction is a non-operation instruction (judgment step 4 is No), the process proceeds to step 5 and a process according to the non-operation instruction is performed. If the command is an operation command (judgment step 4 is Yes), the process proceeds to step 6 to activate the trajectory control task and start the position control process of the robot 10. First, in step 7, a trajectory of the robot 10 is generated, that is, position data q (rotation angle of the servo motor) is generated for each of the first axis a to the sixth axis f at every interpolation cycle ΔT.
Next, in step 8, tertiary interpolation for dividing the position data q for each interpolation cycle ΔT into m equal to the cycle t of the servo control unit 40 is performed, and the target value calculated by the interpolation is used for the servo control unit 40. Output to

Here, the tertiary interpolation in step 8 and the output of the target value to the servo will be described in detail with reference to the flowchart shown in FIG. In this description, the processing for the first axis a will be described as an example. First, in step 81, the position data q0, q1, q2 and the value of the coefficient d calculated each time the interpolation period ΔT elapses are set as the current position θ before the generation of the trajectory. Next, step 8
In step 2, the position data q of the first axis a calculated in step 7 of the flowchart of FIG. 4 is input, and in step 83, the value of the position data q3 is replaced with the position data q.
Then, in step 84, the four position data q0
, Q1, q2, and q3, the coefficients a, b, and c of the cubic equation are obtained from the following equations 1, 2, and 3. Here, m is the interpolation cycle ΔT for calculating the position data q and the cycle t of the servo control unit.
And a relationship of ΔT = tm (hereinafter, m is referred to as “servo division number”).

[Number 1] a = (q3 -3 · q2 +3 · q1 -q0) / (6 · m 3)

[Number 2] b = (q2 -2 · q1 + q0) / (2 · m 2)

## EQU3 ## c = (q2 -q0) / (2.m)

Next, in step 85, the variable i is initialized to 0, and in step 86, 1 is added to the variable i. Then, in step 87, the position data q divided by m
The i-th position command value θi, speed command value θ′i, and acceleration command value θ ″ i are simultaneously calculated based on the following Expressions 4, 5, and 6. That is, t = ΔT / m · i in Equations 4, 5, and 6
To obtain the respective values.

[Number 4] θi = f (t) = at 3 + bt 2 + ct + d

Equation 5 θ′i = f ′ (t) = 3 at ² + 2bt + c

[Formula 6] θ ″ i = f ″ (t) = 6at + 2b

In step 88, step 8
The position command value θi, speed command value θ′i, and acceleration command value θ ″ i calculated in step 7 are output to the servo control unit 40 as servo target values. Thereafter, it is determined in a decision step 89 whether or not the value of the variable i has reached the servo division number m, that is, whether or not the period ΔT has been reached. If the variable i has not reached m (the decision step 89 is No) Returning to step 86, the processing for the next i-th servo cycle is continued. On the other hand, if the variable i has reached m (determination step 89 is Yes), the process proceeds to step 90.
Then, the value of the position data q0 is replaced by q1, the value of the position data q1 is replaced by q2, the value of the position data q2 is replaced by q3, and the value of the coefficient d is replaced by .theta.i calculated as a position command value for the subsequent processing. As described above, in step 90, the position data of four points required for the cubic interpolation is updated by replacing the values with the values of the position data q0 to q3.

As described above, by the processing in step 8 of the flowchart shown in FIG.
The target value is output to the servo control unit 40, and the position of the robot 10 is controlled by driving the motor M1 of the first axis a to the motor M6 of the sixth axis f. Then, the next judgment step 9
It is determined whether the interval between one teaching point has ended, that is, whether the next teaching point has been reached. If the next teaching point has not yet been reached and it is necessary to continue the interpolation processing (determination (Step 9 is No.), and the process returns to step 7 to start processing for the next interpolation cycle ΔT. Then, when the next teaching point is reached, 1
When the interval between two teaching points ends (judgment step 9 is Ye
s) Returning to step 2, the program is decoded, the process is started for the interval to the next teaching point, and the position, speed and acceleration are obtained.

Next, with respect to feedforward on the servo control unit 40 side based on the command value transferred from the CPU 11 by the above processing, a block diagram of FIG. 6 showing the internal configuration of the servo control unit 40 and FIG. D
This will be described with reference to the block diagram of FIG. 7 showing the servo control function by the SP 414. Here, for convenience of explanation, only the control operation of the motor M1 of the first axis a will be described, but the second axes b to 6f are similarly controlled. The servo control unit 40 mainly includes a digital signal processor (hereinafter referred to as “DSP”) 414 and a common RA.
M417, A / D converters 415a and 415b, current value counter 416, ROM 420, inverter 425, and current transformer (hereinafter referred to as "CT") 43
2a and 432b, amplifiers 418a and 418b, and a waveform shaping / direction discriminating circuit 434.

The output of the DSP 414 is input to an inverter 425, which drives the servomotor M1 according to the output signal of the DSP 414. A synchronous motor is used as the servo motor M1, and the load current of the servo motor M1 is controlled by the PWM voltage control of the inverter 425, and as a result, the output torque of the servo motor M1 is controlled.

The u-phase and v-phase load currents of the servo motor M1 are detected by CTs 432a and 432b,
Amplified by 18a and 418b. The amplifier 41
The outputs of 8a and 418b are output from A / D converters 415a and 41
5b, sampled at a predetermined cycle, and converted into a digital value. This sampled value is
As a feedback value of the instantaneous load current, the DSP 414
Is input to

A pulse encoder E1 is connected to the servomotor M1, and the output of the pulse encoder E1 is
It is connected to the current value counter 416 via the waveform shaping / direction discriminating circuit 434. An output signal from the pulse encoder E1 input to the current value counter 416 via the waveform shaping / direction discriminating circuit 434 increases or decreases the value of the current value counter 416. The value of the current value counter 416 represents a position feedback value, that is, the current position θai (rotation angle) of the axis of the servo motor M1 at the current time (i).
The target position command value θi commanded at the current time (i) from the CPU 11 and the position feedback value (current position) θai from the pulse encoder E1 are calculated by the DSP 414.
(Connection point 110 in FIG. 7), and the positional deviation Δθ
i is output. Then, the DSP 414 adds the position feedback loop gain Kp1 to the position deviation Δθi.
The result is multiplied by 12 to calculate the primary target speed Vpi.

On the other hand, the speed command value θ′i transferred from the CPU 11 is multiplied by the feedforward gain Kf116 by the DSP 414, and the feedforward value Vfi at the current time is calculated. This value is added to the primary target speed Vpi calculated based on the position deviation (connection point 114). Thus, feedforward control is performed by the speed component, and the final target speed Vi becomes a speed command value for speed feedback.

On the other hand, the acceleration command value θ ″ i transferred from the CPU 11 is multiplied by a feedforward gain Ka120 by the DSP 414 to calculate an acceleration feedforward value Vgi. This value is added to the current command value (connection point 118).

In the speed feedback loop, D
The current position input to SP 414 is differentiated with respect to time, and the feedback value of the speed feedback loop, that is, the current speed Vai is calculated. The DSP 414 compares the finally determined target speed Vi with the current speed Vai to calculate the speed deviation ΔVi (the connection point 122). Then, after multiplying the speed deviation ΔVi by a speed gain Kup (1 + 1 / TS), an acceleration feedforward value is added. The q-axis component (active current) of the control target current in the current feedback loop is calculated. The d-axis component (reactive current) of the target current is set to zero.

By the DSP 414, the q-axis component and the d-axis component of the target current are detected by the CTs 432a and 432b, and the amplifiers 418a and 418b and the A / D converter 415
The feedback value of the current feedback loop via a and 415b, that is, the q-axis component and the d-axis component at the current time are compared (connection point 124), and the current deviation of each component is calculated. Then, the q-axis component and the d-axis component of the command current at that time are calculated by proportional and integral calculations using the current deviation and the time integration (accumulation) of the current deviation.

The q-axis component and the d-axis component of the command current are dq-converted into the command current of each phase. The command current of each phase is compared with a high-frequency triangular wave, and a voltage control PWM signal for controlling on / off of a transistor of each phase of the inverter 425 is generated. The voltage control PWM signal is supplied to the inverter 425.
, And the transistors of each phase of the inverter 425 are energized. The switching of the inverter 425 controls the load current to the target current. That is, the motor M1 is driven by the CPU 1
It is driven based on the value instructed from 1. Similarly, the other motors M2 to M6 are driven to control the position of the robot 10.

In the embodiment described above, the position and the acceleration command values are calculated by tertiary interpolation by obtaining four points of position data. However, the present invention obtains, for example, five points of position data and obtains the position data of five points by quaternary interpolation. The speed, acceleration, and jerk can also be obtained, and the robot can be more smoothly controlled by controlling the motor based on the obtained jerk. In the above-described embodiment, the CPU
The command value calculated by the CPU 11 is digitally fed forward in the servo control unit 40, but the present invention is not limited to this. For example, the command value calculated by the CPU 11 is D / A converted and converted into an analog signal. It is also possible to feed forward to the servomotor in a formula.

In this embodiment, since the CPU 11 calculates the speed command value and the acceleration command value, the servo control unit 40 does not need to calculate the speed and the acceleration, and the servo control unit performs other processing. It becomes possible. CPU1
1 calculates the speed command value and the acceleration command value at the same time as the position command, so that there is no displacement or distortion between them, and the characteristics of the speed feed forward and the acceleration feed forward can be improved. Further, since the cubic expression is used for interpolation, the acceleration command value θ ″ i becomes a function of time t as shown in Expression 6, and the acceleration component calculated by interpolation becomes a continuous change with time t. A high acceleration feed forward can be obtained.

[0025]

The present invention is constructed as described above. Since the speed command value and the acceleration command value are calculated simultaneously with the position command value and feedforward is applied to the servomotor, the speed feedforward and the acceleration feed are performed. Forward characteristics can be improved. In addition, since the cubic expression is used for interpolation, the interpolated acceleration component changes continuously, and good acceleration feedforward can be performed.

[Brief description of the drawings]

FIG. 1 is an overall configuration diagram for clearly illustrating the present invention.

FIG. 2 is a configuration diagram showing a mechanical configuration of a robot controlled by a robot control device according to one embodiment of the present invention.

FIG. 3 is a block diagram illustrating a configuration of a robot control device according to an embodiment of the present invention.

FIG. 4 is a flowchart illustrating a process of the robot control device according to the embodiment.

FIG. 5 is a flowchart illustrating a cubic interpolation process of the robot control device according to the present embodiment.

FIG. 6 is a block diagram illustrating a configuration of a servo control unit.

FIG. 7 is a block diagram showing a functional configuration of a servo control unit.

FIG. 8 is a configuration diagram of a conventional robot control device.

[Explanation of symbols]

 Reference Signs List 10 robot 11 CPU 19 nozzle 30 RAM 40 servo controller 50 robot controller 414 digital signal processor

Continuation of the front page (58) Field surveyed (Int. Cl. ⁷ , DB name) G05D 3/00-3/20 G05B 11/00-13/04 G05B 19/18-19/46

Claims (1)

(57) [Claims] 1. A robot control device that performs interpolation based on an input value to calculate position data and drives a servomotor to control the position of a robot. Means based on the position data output by the trajectory generating means.
A command position interpolating means for simultaneously calculating a position command value, a speed command value, and an acceleration command value by the following interpolation formulas and outputting these command values for each integral number of the interpolation cycle T; And a servo control means for performing feedforward control on the servomotor based on the position command value, the speed command value, and the acceleration command value output from the controller.