JPH03110605A - Servo controller - Google Patents

Abstract

PURPOSE:To improve the control performance by providing a function which simultaneously performs compensation to apparently erase the disturbance load with the positive feedback of an estimated disturbance load as well as speed feedback compensation using the estimated speed. CONSTITUTION:The position control operation part 3 of a servo controller 1 compares the data of a position command 5 with the position detection signal 6 of a control object and uses the data of the estimated speed 10 to perform the operation required for control and outputs a driving force command 10a. Meanwhile, the driving force 8 and the position detection signal 6 are inputted to a disturbance load speed observer 4, and the observer 4 outputs the estimated disturbance 9 and the estimated speed 10. The estimated disturbance load 9 is added to the output 10a of the position control operation part 3 to obtain the driving force 8. Since disturbance torque elimination and estimated speed feedback are simultaneously performed in such a manner, stable control is realized even when the speed is so low that speed ripple is apt to occur.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a position, speed or force control device for a robot manipulator, machine tool, or XY child table. [Summary of the Invention J] The present invention applies to control objects that are mechanically driven and have position detection means, such as robot arms, machine tool spindles, work tables, etc., that are caused by Coulomb friction, elasticity, viscosity, or load fluctuations. Simultaneously estimate the disturbance load on the controlled object, such as fluctuations in acceleration force and external forces from the outside world, and the motion speed of the controlled object, using the driving force (or driving current) generated by the actuator and the detected current position data. It is characterized by having a state estimation calculation unit (disturbance load/velocity estimation observer), and has the function of simultaneously performing compensation that apparently eliminates the disturbance load by positive feedback of the estimated disturbance load and velocity feedback compensation using the estimated velocity. This is a servo control device characterized by improving control performance by having the following. See (Figure 1). [Prior Art] Known or unknown disturbance torque occurs on the motor shaft of the mechanical drive system of robots, machine tools, etc., which may interfere with machine drive, cause vibration, or cause an error in tracking the target trajectory. There have been problems that have caused various deteriorations in controllability, such as cases in which the Conventionally, in the field of position/velocity control, as shown in the example in Fig. 2, control objectives are achieved by constructing a feedback control loop that utilizes a feedback signal of the position signal 6 from the position detector or the speed signal 14 of the controlled object. has been achieved. At this time, with respect to the disturbance torque 7, the deviation is eliminated by making use of the disturbance removal effect that the feedback loop inherently has, and by making the loop gain as large as possible. In addition, as seen in the transfer function block diagram of the conventional example in FIG. methods have been taken. However, the stable region of the control system is limited by the characteristics of the controlled object, so the simple gain 11.12.1
There is a limit to the improvement of 3, and sufficient characteristics cannot be obtained. Furthermore, many control methods have been developed to deal with known disturbances by configuring control systems that take into account changes in dynamic characteristics, but it has been difficult to maintain the same control characteristics against unknown disturbances. Therefore, as shown in FIG. 5, a disturbance torque observer 4 is installed that treats the disturbance torque 7 as a part of the controlled object model 2 and estimates the disturbance torque 7 using the actuator's generated current or generated torque 8 and motion speed 14. A method for configuring and applying it to speed control systems has been developed. On the other hand, when configuring not only a speed control system but also a position control system, the
As can be seen in FIG. 2, it is known that controllability can be improved by using a speed feedback loop in combination. [Problem to be solved by the invention] Many robots and machine tools do not have a speed detection means such as a tacho generator, but only have a position detection means, in order to make them smaller and lighter. In this case, it is necessary to use the differential value of the position detection data as the speed detection value for control position, but differentiation using an analog circuit is not practical because it easily picks up high frequency noise. Furthermore, speed detection circuits that generate a voltage proportional to the pulse frequency are widely used for controlled objects that have a position detection means that generates pulses that represent the position, but because they include ripples at low speeds, There were problems with accuracy and caused vibrations. In order to eliminate the ripples in the speed detection circuit, a method of inserting a low-pass filter has been used, but a delay occurs and the controllability deteriorates. Also,
Digital servo control devices often use a method of approximating the first-order difference between the current position and the position one sample before as speed data, but this also tends to cause vibration and noise because ripple increases at low speeds. . As described above, for a position control system having only one position detection means, a control device uses a speed signal from a controlled object and a drive torque signal of the control system to output an estimated disturbance torque using an observer and uses it in the position control system. There was a practical problem. [Means for Solving the Problems J] In order to solve the above problems, the present invention provides an observer (
This is a control device that performs servo control by calculating the estimated disturbance load and estimated speed using a state estimator), gives positive feedback to the estimated disturbance load, and compensates for the estimated speed by speed feedback, and has a position detection means and a speed detection means. A state estimation calculation unit (disturbance torque/velocity observer) is configured to input the driving force (or drive current) and detected position value for a controlled object that does not have a ,
This servo control device uses software (CPU device) to perform positive feedback of the estimated disturbance load and simultaneous speed feedback using the estimated speed. [Operation 1 The position control calculation unit 3 of the servo control device 1 compares the data of the position command 5 and the position detection signal 6 of the controlled object, performs the calculation necessary for control using the data of the estimated speed 10, and starts the drive. Output as a force command. On the other hand, the disturbance load speed observer inputs the driving force 8 and the position detection signal 6, and outputs an estimated disturbance load 9 and an estimated speed 10. The estimated disturbance load 9 is added to the output of the position control calculation section 3, and the driving force 8 is obtained. The controlled object 2 is driven and controlled by the difference between the driving force 8 and the disturbance load 7. [Embodiment] An embodiment of the present invention will be described using an example in which the controlled object is a rotating system. In FIG. 1, the position control calculation section 3 of the servo control device 1
A position command signal 5 and a position detection signal 6 from the position detector of the controlled object 2 are output. On the other hand, the disturbance torque
For speed observer 4. A position detection signal 6 and a driving torque (driving force) signal 8 are input, and the observer 4 performs calculations based on these two signals and outputs an estimated disturbance torque (load) signal 9 and an estimated speed signal lO. do. The estimated speed signal 10 is input to the position control calculation section 3, and the estimated disturbance torque signal 9 is added to the output 10a of the position control calculation section 3. The position control calculation unit 3 compares the position command 5 and the position detection signal 6, and uses the estimated speed signal 10 to calculate the driving force for moving the controlled object to a predetermined position, and generates a driving torque command signal lOa. Output. As described above, this drive torque signal 10a is added to the estimated disturbance torque signal 9 to obtain the drive torque signal 8. The drive of the controlled object 2 is controlled by the difference between this drive torque signal 8 and disturbance torque such as acceleration fluctuations or sliding friction of the controlled object. Note that the servo control device l, which consists of the position control calculation section 3, disturbance torque speed observer 4, etc., uses a CPU device to perform calculation processing by software, but the position control calculation circuit and the observer circuit respectively output calculations. You can also do this. Next, the principle of disturbance torque estimation will be explained. The first equation is an equation of motion regarding the controlled object, where motor generated torque T1, moment of inertia J, angular position θ, and disturbance torque T1, · represent time differentiation. In order to obtain a disturbance torque speed estimation observer, the controlled object is expressed by state equations such as the second and third equations, and the first equation is expressed in terms of the state based on the definition of the fourth equation and the assumption of the fifth equation. Equations 6 and 7 are expressed as equations. Even with the assumption of the fifth equation, it is clear from the literature regarding disturbance torque observers that the obtained observer will eventually follow disturbance torque changes to a certain extent. Equation of motion: 'l=Jθ+TL Equation of state: X=AX+Bu y=cx Definition: X, = θ, u = T a (1 equation) (2 equation) (3 equation) (4 equation) Xg=θ, y=x . ±θ 8th
The 10th and 11th equations expressed in the form of the 9th equation are obtained. Each state quantity has the meaning shown in the 12th formula. Cross=tez+'1lTu (Formula 9) (Formula 10) (Formula 11) Ll and La are parameter constants that determine the eigenvalues of the above system, and are determined so as to ensure system stability. Next, in order to realize the disturbance torque speed estimation observer obtained by Equation 1O and Equation 11 with software on the control device, discretization is performed, and by performing the transformation defined by Equation 13, Equation 14, 16th and 17th expressions expressed in the form of 15th expression
The parameters in the 0 formula that give the formula are the 18th to 27th formulas.
Expressed by the formula. X (kT) = C Z (kT) + D ・U (kTl (Formula 15) (Formula 16) (Formula 17) %Formula%) () y (kTl is written as: jw, u (kTl is written as um. x + (kTl to Xo, x, fkT) to x 11
x, x s (kT) is written as X. θ(kT) is expressed as θ5, δ(kTl is expressed as bk, and TL(kT) is expressed as T Lk. αth 0 (α−β) −L2(e −” −e 0th) (Formula 20) (Formula 21) (Formula 22) (Formula 24) (Formula 25) (Formula 26) (Formula 27) % formula % J is the nominal value of the moment of inertia. ) Disturbance torque speed in the discretized form expressed by Equations 16 and 17 The flowchart shown in Fig. 6 shows how the estimation observer is incorporated into the control device, and the disturbance torque speed estimation described above is simply expressed as an observer.After initializing the data, the servo control device repeats the process repeatedly. Enters a loop, creates sequential position command data, waits for sampling time, and performs position detection.Based on the detected value and previous input, the observer estimates the disturbance torque and speed, and transfers it to the servo calculation routine. The servo outputs the calculation result as a driving torque and updates the number of repetitions k. Figure 7 shows the calculation routine of the observer. Zi++ (t =, 0.1, 2...) is the disturbance torque speed estimation It is an intermediate variable in the observer, and the data value is updated every sampling time in the loop of the flowchart with the initial value as 0.
Z corresponds to z ((k+t)) in the formula and corresponds to Z+1
This is a variable after one sample of (k), and is updated sequentially within this routine. Equation 28 shows the formula for realizing control compensation using estimated disturbance torque and estimated speed. The integral of the difference between the command position and the detected position is multiplied by a gain, and the estimated speed and detected position are each multiplied by a gain. The estimated disturbance torque is fed back negatively, and the estimated disturbance torque is fed back positively. (Formula 28) % Formula %: TLk: Estimated disturbance torque Tmm: Mechatorque Figure 8 is the routine for realization. As a result, the transfer function block shown in FIG. 4 is realized. [Effects of the Invention] Taking a robot as an example, when the robot grips an object with unknown weight, the dynamic characteristic parameters of the robot change by an unknown amount. Furthermore, parameters such as elasticity, viscosity, and friction of mechanical drive systems, not just robots, change depending on environmental temperature, humidity, and the like. Torque fluctuations that occur during driving due to these changes act as disturbance torque on the control system.The zero control device can maintain constant control characteristics of the control system at all times by apparently eliminating this disturbance torque. In general, speed ripples in position/speed control systems are caused by detection noise, drive torque ripples, etc. in addition to disturbance torque. By using speed feedback using estimated speed, detection noise during speed detection can also be removed. By simultaneously performing disturbance torque removal and estimated speed feedback, stable control can be achieved even at low speeds where speed ripples are likely to occur.

[Brief explanation of drawings]

FIG. 1 is a block diagram of an embodiment of the present invention, showing a disturbance torque speed estimation observer and a control device using the observer. FIGS. 2 and 3 are conceptual diagrams of a conventional control device. FIG. 4 is a block diagram showing a transfer function of an embodiment of the present invention;
FIG. 5 is a block diagram showing the transfer function of the embodiment of FIG. 2. FIG. 6 is a flowchart showing the control system of the embodiment of the present invention, and FIGS. 7 and 8 are flowcharts of the observer and servo calculation routine of FIG. 6. Servo control device Controlled object position control calculation section Disturbance load (torque)/speed estimation observer (state estimation calculation section) Position command signal Position signal Disturbance load (torque) Driving force (torque) signal Estimated disturbance load (torque) signal Estimated speed signal 11 ・ l 2 ・ l 3 ・ l 4 ・ ・Integral compensation gain・Velocity feedback gain・Position feedback gain・Speed

Claims (1)

[Scope of claims] In a servo control device that controls a controlled object consisting of a mechanical drive system having a position detection means, the disturbance load of the controlled object and the motion speed of the controlled object are simultaneously estimated by calculating from an input signal. It has a state estimation calculation means that outputs a disturbance load and an estimated speed signal, and a position control calculation means that calculates from a position command signal, a position detection signal, and the estimated speed signal and outputs a driving force command signal, A servo control device characterized in that a disturbance load signal is positively fed back to a position control calculation means, and the estimated speed signal is compensated for by speed feedback.