JPH06289938A - Obstacle detection device in servo feeding - Google Patents

Abstract

PURPOSE:To prevent the destruction of an object by detecting a collision with an obstacle at an eary stage. CONSTITUTION:Disturbance torque in a next control period is predicted by an operation (302) by the same dimensional observer for a digital servo control system from a rotation angle detected by an angle detection means, the prediction value of the rotation angle, the prediction value of the rotary angular velocity, the prediction value of disturbance torque and instructed target current. Predicted disturbance torque is compared with a prescribed value. When disturbance torque becomes more than the prescribed value, the moving object by the servo motor is judged to collide with the obstacle.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a digital servo control device for digitally controlling an AC servo motor, and more particularly to a device for detecting a collision with an obstacle.

[0002]

2. Description of the Related Art Conventionally, a digital servo controller has a position feedback loop, a velocity feedback loop,
It has a current feedback loop. This digital servo controller is used for robot arm drive axis and NC.
It is used to control the rotation of the feed shaft of machine tools. In such a device, when the moving body collides with an obstacle against the intention of the operator, it is necessary to immediately detect the collision and stop the rotation of the servo motor.
In order to detect the collision with the obstacle, the load current of the servo motor is detected, and when the load current exceeds a predetermined value, it is determined that the collision with the obstacle.

[0003]

Therefore, facilities such as a detection head and an amplifier for detecting the load current of the servomotor are required. In addition, the rotation of the servomotor is forcibly stopped by the obstacle, so that the load current actually increases, and as a result, a collision with the obstacle is detected for the first time, resulting in a detection time delay. . as a result,
The robot arm, NC machine tool tools, tables, obstacles, etc. may be damaged.

The present invention has been made to solve the above problems, and an object of the present invention is to detect a collision with an obstacle at an early stage in servo feed without special equipment, thereby damaging the object. Is to prevent.

[0005]

The structure of the invention for solving the above-mentioned problems is to provide an obstacle detecting device in servo feed for giving a target value for each feedback loop by a digital amount, by an angle detecting means in a current control cycle. From the detected rotation angle, the predicted value of the rotation angle, the predicted value of the rotational angular velocity, the predicted value of the disturbance torque, and the commanded target current, by the calculation by the same dimension observer for the digital servo control system, Disturbance torque in the control cycle is predicted, the predicted disturbance torque is compared with a predetermined value, and when the disturbance torque is equal to or greater than the predetermined value, it is determined that the moving object by the servomotor has collided with the obstacle. Is characterized by.

[0006]

From the rotation angle detected by the angle detection means, the rotation angle predicted value, the rotation angular velocity predicted value, the disturbance torque predicted value, and the commanded target current, in the current control cycle, a digital value is calculated. The disturbance torque in the next control cycle is predicted by the calculation by the same-dimensional observer for the servo control system. Then, the predicted disturbance torque in the next control cycle is compared with a predetermined value, and when the disturbance torque becomes equal to or larger than the predetermined value, it is determined that the moving object by the servo motor has collided with the obstacle.

Therefore, before the load current of the servo motor actually increases, the increase in the disturbance torque is detected early.

[0008]

As described above, according to the present invention, the disturbance torque generated in the next control cycle is predicted by the predictive calculation using the same-dimensional observer for the digital servo control system, and the magnitude thereof is determined. It is possible to determine whether or not the vehicle has collided with an obstacle without requiring special equipment such as a detection head. Therefore, the collision with the obstacle can be detected at an early stage, and damage to the moving body and the obstacle can be prevented.

[0009]

EXAMPLES The present invention will be described below based on specific examples. FIG. 1 is a block diagram showing the configuration of a digital servo control device according to the present invention. The digital servo control device 10 mainly includes the CPU 11 and the R
OM12, RAM13, digital signal processor (hereinafter referred to as "DSP") 14, common RAM17, C
PU23, A / D converters 15a, 15b, ROM20,
It is composed of a RAM 24 and a current value counter 16. A keyboard 21 and a CRT display device 22 are connected to the CPU 11 via an interface 19.

The DSP 14 performs calculation for position and speed control, the output of the DSP 14 is input to the CPU 23 for current control, and the output of the CPU 23 is input to the inverter 25. The inverter 25 drives the servo motor 31 according to the output signal of the CPU 23. A synchronous motor is used as the servo motor 31, and the load voltage of the servo motor 31 is controlled by the PWM voltage control of the inverter 25, and as a result, the output torque is controlled.

The u-phase and v-phase load currents of the servomotor 31 are detected by the CTs 32a and 32b, and the amplifier 18
It is amplified by a and 18b. The amplifiers 18a, 18
The output of b is input to the A / D converters 15a and 15b,
It is sampled at a predetermined cycle and converted into a digital value. The sampled value is input to the CPU 23 as a feedback value of the instantaneous load current. or,
A pulse encoder 33 is connected to the servo motor 31, and its current position (current rotation angle) is detected. The output of the pulse encoder 33 is connected to the current value counter 16 via the waveform shaping / direction discrimination circuit 34.

The output signal from the pulse encoder 33 input to the current value counter 16 via the waveform shaping / direction discriminating circuit 34 adjusts the value of the current value counter 16. D
The value of the current value counter 16 is read by the SP 14 as a current position feedback value, and the DSP 14
The position deviation (angle deviation) is calculated by comparing with the target value (target rotation angle) output from the CPU 11. And DS
In P14, the velocity target value (rotational angular velocity target value) is calculated based on the position deviation.

The current position feedback value input to the DSP 14 is differentiated to calculate the speed feedback value. The DSP 14 compares the speed target value determined according to the position deviation with the speed feedback value to calculate the speed deviation, and calculates the current target value based on the speed deviation.

On the other hand, the load currents detected by the CTs 32a and 32b are the amplifiers 18a and 18b and the A / D converter 1.
Input to the CPU 23 via 5a and 15b. And
As described later in detail, the disturbance torque in the next current control cycle is predicted and calculated based on the rotation angle and the rotation angular velocity detected in the current control cycle, and the target torque related value instructed in the current control cycle. . Most specifically, the target torque-related value is a target current.

Then, the CPU 23 compares the target current value with the predicted current feedback value to calculate the current deviation. The instantaneous current command value at that current control time is calculated based on the instantaneous current deviation at that time, the cumulative value of the instantaneous current deviation, and the current target value, that is, by the proportional integral calculation. The instantaneous current command value is compared with a high frequency triangular wave, and a voltage control PWM signal for controlling on / off of each phase transistor of the inverter 25 is generated.

The voltage control PWM signal is sent to the inverter 2
5, and the transistors of each phase of the inverter 25 are driven. By the switching of the inverter 25, the load current of each phase is controlled to the current target value. The positioning of the servo motor 31 is C
When the PU 11 determines that the output value of the current value counter 16 becomes equal to the target position value, the process is completed.
The u-phase and v-phase load current values sampled by the A / D converters 15a and 15b are dq converted by the CPU 23.

As described above, the digital servo controller of this embodiment is composed of three feedback loops of position, velocity and current. A lower feedback loop requires higher responsiveness, for example, the lowest current feedback loop is 100 μs,
The velocity feedback loop is several times as long as the position feedback loop, and the position feedback loop is further synchronously sampled at time intervals, and the processing of each feedback loop is executed.

Next, the operation of the apparatus of this embodiment will be described. FIG. 2 is a flowchart showing a program executed by the CPU 11 stored in the ROM 12.
In the state before this program is executed, the servo motor 31 is in a stopped state.

In step 200, one block of movement command data is read from the RAM 13, and step 2
In 02, the acceleration / deceleration flag in the flag area of the RAM 13 is turned on. Next, in step 204, the interpolation target position (interpolation target rotation angle) in the acceleration region for accelerating the servo motor 31 from the stopped state to the commanded constant speed is calculated, and the interpolation target position is momentarily stored in the common RAM 17. It is output and stored there.

Next, when the acceleration is completed, step 206
In step 20, the acceleration / deceleration flag is turned off, and the next step 20
8, the constant speed flag in the flag area of the RAM 13 is turned on. Next, in step 210, the interpolation target position in the constant speed region is calculated, and the interpolation target position is output to the common RAM 17 every moment and stored therein.

Next, at step 212, it is judged whether or not the movement command data of one block includes a command for temporarily stopping at the command target position. If the stop instruction is not given, the movement instruction data of the next block is input in step 214, and the interpolation target position in the constant velocity region is calculated in step 210. Step 21
By repeating 0 and 214, the target position can be sequentially updated at a constant speed, and the interpolation target position can be sequentially output.

When it is determined in step 212 that the movement command data includes a temporary stop command, the acceleration / deceleration flag is turned on in step 216, and the constant speed flag is turned off in step 218. Then, in step 220, the interpolation target position in the deceleration area is sequentially calculated, and the interpolation target position is momentarily shared by the common RAM.
It is output to 17 and stored there.

Next, after the deceleration interpolation is completed, the servo lock flag in the flag area of the RAM 13 is turned on in step 222 to indicate that the servo lock state is established. Then, in step 224, the same target position is output momentarily to the common RAM 17, and the target position is stored therein. As a result, the servo motor 31 is held at the same position in the servo lock state, that is, in the energized state.

After the temporary stop for the time instructed by the movement command data is completed, the process returns to step 200, the movement command data of the next block is read, and the position control of the servo motor is performed in the same manner as the above-mentioned step. Be seen. In this way, the position of the servo motor is commanded.

Next, the DSP 14 executes the program of FIG. 3 stored in the ROM 20, and the CPU 23 makes the ROM 23.
4 is executed and the position, speed and torque of the servo motor 31 are controlled. The programs of FIGS. 3 and 4 are repeatedly executed by the DSP 14 and the CPU 23 at each predetermined minimum cycle.

In step 100, it is judged whether or not the current execution cycle is the position deviation calculation timing.
The target position θ (i) (interpolation target angle) commanded at that time by the CPU 11 from M is input and stored. Further, the target position within the past fixed time is stored in the common RAM 17.

Next, at step 104, the current position (electrical angle) θa (i) held in the current value counter 16 is read. Next, in step 106, the current time (i)
Position deviation Δθ between the target position θ (i) and the current position θa (i)
(i) is calculated. Next, at step 108, the target speed V (i) is calculated by a value proportional to the position deviation ΔP (i), that is, the following equation.

## EQU1 ## V (i) = k.ΔP (i)

The feedback control of the above position is shown in FIG.
Is executed at the timing indicated by the signal S1.

Next, at step 110, it is judged if the current execution cycle is the speed deviation calculation timing.
If it is the speed deviation calculation tamming, the routine proceeds to step 112, where the current position θa (n) held in the current value counter 16 is read. Next, the process proceeds to step 114 and the current time
The current speed Va (n) at (n) is calculated. Current speed V
a (n) is the current position θa (n-1) read at the previous speed deviation calculation timing and the current position θa (n) input this time.
And the speed control cycle D are calculated by the following equation.

[Formula 2] Va (n) = (θa (n) -θa (n-1)) / D

Next, at step 116, the deviation between the target speed V (i) calculated at step 108 and the current speed Va (n), that is, the speed deviation ΔV (n) is calculated. Also, the cumulative value (integration) S of the speed deviation ΔV (n) is S = S + ΔV (n)
Is calculated by Next, in step 118, the speed deviation ΔV (n) calculated in step 116 and the integral S of the speed deviation, and the proportional gain K _p and target speed V (i) set in the common RAM 17 are set. Using the integration time T _i , the q-axis component of the target current (active current, which is proportional to Tokle of the servomotor) Iq (n) is calculated by the following equation. The d-axis component (reactive current) of the target current is 0
Is.

## EQU3 ## Iq (n) = K _p (ΔV (i) + S / T _i ).

Next, in step 119, the disturbance torque T _L in the nth control cycle is calculated by the same-dimensional observer.
(n) 'is calculated. The prediction calculation by the same dimension observer is executed according to the program shown in FIG. Prior to performing the prediction calculation, the servo motor 31 and the pulse encoder 33 can be modeled as shown in FIG. That is, the following relationships are established among the disturbance torque T _L , the target current Iq, the inertia moment J _m of the servo motor 31, the inertia moment J _{L of the} load, the rotational angular velocity ω and the rotational angle θ.

## EQU4 ## K _tn Iq −T _L = (J _m + J _L ) S ² θ where K _tn is a torque constant. When FIG. 8 is created based on the above equation and the state equation is derived,

[Equation 5] However, J = J _m + J _L This equation is discretized, and the prediction operation is performed by the equation (Equation 7 below) that constitutes the same-dimensional observer.
In step 300 of FIG. 6, the previous time, that is, the (n-1) th
The error in the control cycle is calculated by the following equation.

## EQU00006 ## e (n-1) =. Theta. (N-1) '-. Theta.a (n-1) where e (n-1), .theta. (N-1)' and .theta.a (n-1) are These are respectively a position (rotation angle) prediction error, a position (rotation angle) prediction calculation value, and a rotation angle actual measurement value in the (n-1) th control cycle. Here, it is not necessary to specify the initial value θ (1) ′ of θ (n−1) ′, but normally the measured value θa (1) is used.

Next, in step 302, the above equation 5
The target current Iq (n-1) in the (n-1) th control cycle, the predicted value θ (n-1) 'of the rotation angle, and the predicted value of the rotational angular velocity that is the time derivative thereof are calculated by the equation 7 which is a discretized expression. From ω (n-1) ', the disturbance torque predicted value T _L (n-1)', and the error e (n-1), the rotation angle predicted value θ (n) 'in the nth control cycle, and its time Predicted value ω (n) 'of rotational angular velocity, which is a derivative, and predicted value T of disturbance torque
_L (n) 'is calculated.

[0033]

[Equation 7]

Here, t is the time interval between the (n-1) th control cycle and the nth control cycle, J is the sum of the inertia moment J _m of the rotor of the servomotor and the inertia moment J _{L of the} load, and K
_tn is a torque constant.

Next, at step 304, the converted current I _L (n) corresponding to the predicted value _TL (n) 'of the disturbance torque is _TL.
It is calculated by (n) '/ K _tn . Next, at step 306, it is judged if the converted current I _L (n) is not less than the predetermined value I _th . When the converted current I _L (n) is equal to or _larger than the predetermined value I _th , the estimated value T _L (n) ′ of the disturbance torque is the predetermined value K _tn × I _th
This means that the object moved by the servo motor 31 has collided with the fixed obstacle. Therefore, in this case, in step 308,
Immediately after the servo lock flag is turned on, a predetermined servo lock state command is output to the CPU 23 and the CPU 11. That is, in step 308, the target current Iq
(n) = 0, target velocity V (i) = 0, target position θ (i) = constant command is given to the current feedback loop, velocity feedback loop, and position feedback loop. On the other hand, if the converted current I _L (n) is smaller than the predetermined value I _th , it is not a collision with an obstacle, and therefore, step 118
The target currents Iq (n) and Id (n) calculated in (3) are control targets of the current in the current feedback loop. In this way, the DSP 14 repeatedly executes the speed control in the speed control cycle. This speed feedback control is executed at the timing shown by the signal S2 in FIG.

The current control CPU 23 executes the program shown in FIG. In step 120, it is determined whether the current execution cycle is the current deviation calculation timing.
If it is the current deviation calculation timing, the routine proceeds to step 122. Step 122 and subsequent steps are current feedback control, and this control is executed at the timing shown by signal S3 in FIG. In step 122, the current detection time Δt ₁ measured from the beginning of the current control period, the load current control time Δt ₂ measured from the beginning of the current control period, and the current speed V.
The current detection electrical angle θ ₁ and the control electrical angle θ _{2 which} are the electrical angles corresponding to the time are interpolated using a (n).

[0037]

[Equation 8] θ ₁ = θa (n) + Va (n) · Δt ₁

[Equation 9] θ ₂ = θa (n) + Va (n) · Δt ₂

The times Δt ₁ and Δt ₂ and the electrical angles θ ₁ and θ
₂ corresponds as shown in FIG. Next, in step 124, the current values of the u-phase and v-phase load currents, that is, the currents Iu and Iv are read from the A / D converters 15a and 15b. Next, at step 126, the currents Iu and Iv are dq converted, and the d-axis component Iad and the q-axis component Iaq are calculated by the following equation.

[Equation 10] Iad = 2 ^1/2 {lusin (θ ₁ + 2π / 3) -Ivsin θ ₁ }

[Equation 11] Iaq = 2 ^1/2 {Iucos (θ ₁ + 2π / 3) -Ivcos θ ₁ }

As is well known, the dq coordinate system is d
The axis is in phase with the exciting magnetic field, and the q axis is a coordinate system with a phase difference of 90 ° in electrical angle from the exciting magnetic field. The d-axis component represents an invalid component and the q-axis component represents an effective component. Next, step 1
28, the target current I calculated in step 118
deviation between q (n), target current Id (n) = 0, and d-axis component Iad and q-axis component Iaq of the current current obtained in step 126,
That is, the d-axis component deviation and the q-axis component deviation are obtained. Then, based on the d-axis component deviation and the q-axis component deviation, the d-axis component Id (j) ^# and the q-axis component Iq (j) ^# of the command current are calculated by the proportional / integral calculation.

Next, at step 130, the d-axis component and the q-axis component Id (j) ^# , Iq (j) ^# of the command current are calculated by the following equation.
Is inversely dq-converted and each phase current command value Iu (j) ^# , Iv (j) ^# ,
Iw (j) ^# is calculated.

[Equation 12] Iu (j) ^# = (2/3) ^1/2 · {Id (j) ^# cos θ ₂ −I
q (j) ^# sin θ ₂ }

[Equation 13] Iv (j) ^# = (2/3) ^1/2 · {Id (j) ^# cos (θ ₂ + 2π /
3) −Iq (j) ^# sin (θ ₂ + 2π / 3)} Note that Iw (j) ^# is calculated by Iw (j) ^# =-(Iu (j) ^# + Iv (j) ^# ). It

Next, in steps 132 and 134,
Using the level relationship between the phase current command values Iu (j) ^# , Iv (j) ^# , Iw (j) ^# and the high frequency triangular wave, that is, using the average voltage method, within one control cycle A series of PWM signals at is generated. A series of PWM signals can be represented by a voltage vector indicating the voltage application state of each phase. The rotating magnetic field vector is expressed as the integral of this voltage vector. Therefore, the trajectory of the tip of the rotating magnetic field vector is drawn by the sum of each voltage vector × duration. In order to position the rotating magnetic field at a point on the circumference at the shortest path for each angle 2π / n, the inverter 25 is controlled by three vectors of two adjacent voltage vectors and zero vector V ₀ for each control cycle. Needs to be done. The combination of these three voltage vectors and the phase of the rotating magnetic field uniquely correspond to each other. Correspondence table of the combination of the phase of the rotating magnetic field and the voltage vector (zero vector V ₀
Is always one element of the combination, so only a set of two voltage vectors is necessary) stored in the ROM 24 in advance.

In step 132, the control electrical angle θ
_{From 2} (phase of the rotating magnetic field), the table in the ROM 24 is searched to find the combination of voltage vectors at that time.
In step 134, the duration t _1, t of each voltage vector
_2, t ₃ is calculated. For example, _assuming that the combination of the voltage vectors is V _n = (1,1,0), V ₁ = (1,0,0), V ₂ = (0,0,0), The durations t _1, t _2, t ₃ are calculated. As the calculation method, the well-known average voltage method is used in this embodiment.

That is, each phase current command value Iu (j) ^# , Iv
When two of (j) ^# and Iw (j) ^# having the largest absolute value are I ₁ ^* and I ₂ ^{* in} descending order, the durations t _1, t _{2 and} t ₃ are calculated by the following equations.

[Formula 14] t ₁ = | 2I ₂ ^* + I ₁ ^* | T / Vdc

[Equation 15] t ₂ = | I ₁ ^* -I ₂ ^* | T / Vdc

T ₃ = T− (t 1 ₊ t ₂ ) where T is the period and Vdc is the applied DC voltage.

Next, in step 136, the PWM signals by the set of voltage vectors are supplied for the durations t _1, t _2, t _3.
Is only output. For example, V ₆ = (1,1,0), V ₁ = (1,0,
0) and V ₀ = (0,0,0) are output in this order for durations t _1, t _{2 and} t ₃ . In other words, the voltage is applied to the U phase for t ₁ + t _{2, the} voltage is applied to the V phase for t _{1, and the} voltage is not applied to the W phase for the control period.

Each execution cycle of the DSP 14 and the CPU 23 is executed in the minimum control cycle, which is an integral multiple n.
_The current feedback loop is controlled by ₁ , the velocity feedback loop is controlled by the integral multiple n ₂ , and the position feedback loop is controlled by the integral multiple n _3, which is a criterion for the determination in steps 100, 110 and 120. The number of times is set. However, n ₁ <n ₂ ≦ n ₃ . By repeatedly executing the above cycle, FIG.
The position, speed, and current feedback control is performed at the timing shown in. However, the timing shown in FIG. 7 is detected by the timing of the program execution by the CPU 11.

By the servo control as described above, the positioning operation by stop, acceleration, constant speed, deceleration, and stop is executed. When the feedback loop of the position, velocity and current is displayed by a transfer function, it becomes as shown in FIG. That is,
In the speed feedback loop, proportional to the speed deviation
The target current Iq (n) obtained as a result of the integration calculation becomes the target current for the current feedback loop. At the same time, the target current Iq (n-1) becomes an input variable to the same-dimensional observer together with the rotation angle θ (n-1). Then, based on the disturbance torque T _L (n) ′ predicted and calculated by the same-dimensional observer, it is determined whether or not the moving body moved by the servo motor 33 collides with an obstacle. In the above embodiment, in the calculation by the same-dimensional observer, the commanded target current Iq (n) is the target current Iq from which the high-frequency component is removed by the low-pass filter A.

[Brief description of drawings]

FIG. 1 is a block diagram showing a configuration of a digital servo control device according to a specific embodiment of the present invention.

FIG. 2 is a flowchart showing a target position command procedure processed by a CPU 11 of the apparatus of the embodiment.

FIG. 3 is a flowchart showing a processing procedure by a DSP used in the apparatus of the embodiment.

FIG. 4 is a flowchart showing a processing procedure by a DSP used in the apparatus of the embodiment.

FIG. 5 is a block diagram showing a transfer function in a position, velocity, and current feedback loop used in the apparatus of the embodiment.

FIG. 6 is a flowchart showing a procedure for predicting disturbance torque by the same-dimensional observer and determining obstacle collision detection.

FIG. 7 is a timing chart showing timings of position, speed, and current feedback control.

FIG. 8 is a block diagram showing a transfer function of a control system.

[Explanation of symbols]

10 ... Digital servo control device 11 ... CPU 12 ... ROM 13 ... RAM 14 ... DSP (digital signal processor) (angle detection means, disturbance torque prediction means, collision determination means) 15a, 15b ... A / D converter 16 ... Current value counter (angle detection means) 17 ... Common RAM 20 ... ROM (angle detection means, disturbance torque prediction means, collision determination means) 25 ... Inverter 31 ... Servo motors 32a, 32b ... Current transformer (CT) 33 ... Pulse encoder step 104 ... Angle detection means Steps 300-304 ... Disturbance torque prediction means Steps 306-308 ... Collision determination means

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Shingo Kamiya 1-1, Asahi-cho, Kariya city, Aichi prefecture Toyota Koki Co., Ltd.

Claims (1)

[Claims] 1. An obstacle detection device in servo feed which has a position feedback loop, a velocity feedback loop, and a current feedback loop, and which gives a target value for each feedback loop by a digital amount, in accordance with a velocity deviation from the velocity feedback loop. Target current command means for sequentially outputting a target current to the current feedback loop, angle detection means for detecting the rotation angle of the servo motor at each sampling time, and angle detection for the current control cycle. From the rotation angle detected by the means, the rotation angle prediction value, the rotation angular velocity prediction value, the disturbance torque prediction value, and the commanded target current, the same dimension observer for the digital servo control system. Disturbance in the next control cycle is calculated The disturbance torque predicting means for predicting the torque is compared with the predetermined disturbance torque in the next control cycle, and when the disturbance torque exceeds a predetermined value, the moving object by the servomotor collides with the obstacle. An obstacle detection device in servo feed having a collision determination means for determining.