JPS61173305A - Method of optimum control of moving body - Google Patents

Abstract

PURPOSE:To make positioning eliminating remaining vibration of a moving body at the time of starting and stopping quickly by giving negative feedback to a position command value by a correction value basing on status variables obtained successively by forecasting and correcting the position command value successively. CONSTITUTION:In a state equation that can forecast mechanical vibration of an arm, it is supposed that Z is state variable, A, B, C are coefficient matrix when the arm 4 is formula modeled, y' is calculative output variable for position y. Then, correcting position command value U' can be expressed by d/dtz=AZ+ BU', y'=CZ. In the next step corresponding to a correcting circuit 14, predetermined feedback coefficient matrix F is multiplied to a status variable Z obtained in the next but one step corresponding to an arm simulator in which initial value is zero. A command value U' that can eliminate vibration is calculated giving negative feedback to a command value U by the correction value FZ thus obtained. Thus, mechanical vibration can be suppressed by correcting the position command values successively.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to an optimal control method for a moving object, and in particular to an optimal control method useful for a positioning control system for a moving object such as a low-rigidity robot arm. .

[Conventional technology]

For example, an example of a ceiling-suspended, orthogonal coordinate type robot system as a moving body will be described with reference to FIG. 4. First, the system consists of a robot body 1 and a control device 2.

The robot body 1 consists of a base 3 fixed to a beam (not shown), an arm 4 of relatively low rigidity that moves horizontally in the direction of the arrow X while hanging from the base 3, and an arm 4 attached to the tip of the arm 4. A load 6 is attached to one wrist part 5.

The arm 4 is guided and supported by a guide rail 92 by a ball screw 8 rotated by a servo motor, and can freely move in the direction of arrow X. It is detected by a meter 10.

The control device 2 outputs a speed command value based on a position command value based on the teach data and a detection value of the linear potentiometer 10 to the servo motor 7 in a speed pattern as shown in FIG. Move to the position using the speed pattern.

By the way, in such a robot system, mechanical vibration occurs when the arm operates due to the low rigidity of the arm 4. Therefore, this mechanical vibration can be controlled by reducing the maximum value of the speed pattern shown in Fig. 5 below the required value. It was suppressed by keeping it in check.

In other words, if we take as an example the case where arm 4 is moved by a unit step command, the working point at the tip of arm 4 will naturally respond in the form of damped oscillation with an amplitude change as shown in Fig. 6 from the time of startup. However, as shown in the figure, it takes about 1.53 seconds for the response change to settle to a range φ within 10% of the command.

Therefore, conventionally, by keeping the speed command low,
The measures were taken to prevent the vibrations from increasing.

[Problem that the invention seeks to solve]

However, in such conventional methods, vibration generation is suppressed by relaxing the speed pattern, so if the load 6 is large (heavy) or the robot itself has weight restrictions, the arm stiffness may be increased above a predetermined value. If this is not possible, there is a problem in that high-speed movement and high-speed positioning are hindered.

This invention attempts to solve such problems.

[Means for solving problems]

Therefore, in the optimal control method for a moving object according to the present invention, in a positioning control system for a moving object, the moving object is expressed mathematically using a state equation that can predict its mechanical vibration state.
The mechanical vibration of the moving body according to the position command value is predicted by successively determining the state variables of the state equation, and the position command value is subjected to negative feedback with a correction value based on the state variables successively determined in the prediction. Sequentially corrects the position command value to suppress mechanical vibration.

〔Example〕

Embodiments of the present invention will be described below with reference to FIGS. 1 to 3 of the drawings.

FIG. 1 is a block diagram showing a portion of a robot positioning control system according to the present invention in functional blocks.

Note that this robot positioning control system includes, for example, a robot body 1 and a control device 2 shown in FIG.

In the figure, reference numeral 11 denotes an arm mechanism, which is composed of the arm 4 in Figure 4, a servo motor for driving the arm 4, a ball screw 8, etc. When the arm 4 moves to , the tip working point of the arm 4 is located at the position represented by y.

That is, this amount My represents the behavior of the tip of the arm 4, and means the mechanical vibration when the arm 4 is moved by the position command value U.

Note that the arm 4 is actually a servo that rotates according to the speed command value based on the deviation between the position command value U and the detected value (movement position of the arm 4) by the linear potentiometer 10 shown in FIG. It is driven by a motor 7.

12 is a compensation circuit according to the present invention, which expresses and obtains the rigidity of the arm mechanism 11. In other words, the arm mechanism 11 is expressed mathematically by a state equation that can predict the mechanical vibration state of the arm 4, and the mechanical vibration of the arm 4 according to the position command value U', which will be described later, is expressed by the state conversion number of the state equation. The arm simulator 13 makes a prediction by sequentially calculating 2, and the position command value U is sequentially corrected to U' by applying negative feedback to the position command value U using a correction value based on the state variable 2 obtained by the arm simulator 13. Correction circuit 1
It is functionally configured by 4.

In addition to the correction function, the correction circuit 14 also has a settling determination function, the details of which will be described later.

The state equation that can predict the mechanical vibration of the arm 4 is a state variable (state variable vector, in this case a vector representing "position") of 2. A and B are the coefficient matrices when the arm 4 is mathematically modeled using its -order differential (vector representing "velocity").

Assuming that the calculated output variable corresponding to C1 position 3 is 3', it can be expressed as Z=AZ+B u'y' =CZ with respect to the position command value U' as an input variable.

Here, the coefficient matrices A, B, and C are the arm mechanism 11
The load weight can be determined by the spring constant and the damper coefficient G.

In addition, the mathematical model of arm 4 is expressed as a transfer function as G(S)=((C/g)s+(&/m))/(s”
+(C/g)s+(k/II)) By using the above-mentioned state equation, real-time processing is possible by simplifying the calculation formula, and there are few errors.

Also,! Strictly speaking, l'=C2 is not a state equation but an output equation, and is a calculation formula necessary to obtain y' to be used for settling determination.

Hereinafter, the operation of the compensation circuit 12 shown in FIG. 1 will be explained with reference to the flowchart shown in FIG. 2.

As shown in Fig. 2, first input the position command value U, and then
In the next step corresponding to the correction circuit 14 in the figure, the initial value is zero, and the correction value FZ obtained by multiplying the state variable 2 obtained in the next step corresponding to the 7-modulator 13 by a predetermined feedback coefficient matrix F is calculated. Then, negative feedback is applied to the input position command value U to calculate a corrected position command value U' (initial value is 2=0, so u'=u) that can cancel out the vibration.

Next, the calculated U' is substituted into the state equation described above to obtain the state variable Z and the output variable 3'.

And the actual position (position displacement)! ! For example, y' corresponding to is within a predetermined settling range φχ.

Moreover, it is determined whether the vibration of the arm 4 based on the position command value U has stabilized by determining whether or not a predetermined time Tx has elapsed since the vibration has settled within that range. If it has not entered yet, or Tx has not elapsed even after entering, or if it has entered and exceeded φX again and the vibration has not stabilized, output the previously obtained U' as the position command value to move the arm mechanism 11. From then on, the U input process to the settling determination process are repeated again.

Also,! ! If the vibration has stabilized and does not exceed φX again during the time Tx after ′ enters the settling range φχ, the previously obtained U′ is returned to the input U and output.

By performing such processing, the position command value U
The generation of vibration when the arm 4 moves is suppressed and the arm 4 becomes stable quickly, and the position command value U remains valid after the vibration has settled, so the stability when the arm 4 is stopped is also improved. I can do it.

Furthermore, when the arm 4 is equipped with the compensation circuit 12 shown in Fig. 1 and moved by a unit step command, the end working point of the arm 4 responds in the form of damped oscillation with vibration changes as shown in Fig. 3 from the time of startup. It is now possible to settle to the setting range φ within 10% of the command in about 0.5 seconds, which shows that there is a remarkable effect (1/3 time reduction) compared to the conventional method (see Figure 6). .

In this example, the vibration settling judgment was made based on whether 0.63 seconds had elapsed since startup, and in this way, only the time was set by cut and error and used for the settling judgment. You can do it like this.

In addition, the settling blade chamber in the above embodiment was designed to be performed in view of the fact that the arm simulator 13 performs approximate calculations and there is a possibility that the blade becomes unstable again after settling. You can also build .

Furthermore, although the above embodiments have been described using a robot arm as an example of a moving object, the present invention can be applied to a positioning control system for any moving object.

〔Effect of the invention〕

As explained above, according to the present invention, vibrations are suppressed by correcting the position command value based on the results obtained by simulating the behavior of the moving object, so that the moving object can be moved at the maximum driving capacity of the positioning control system. Furthermore, residual vibrations at the time of starting and stopping can be quickly canceled out, making it possible to achieve high-speed positioning.

[Brief explanation of drawings]

FIG. 1 is a block diagram showing the parts of a robot positioning control system to which the present invention is applied in functional blocks. 2FM is a flow diagram for explaining the operation of the compensation circuit 12 in FIG. 1, FIG. 3 is a diagram for explaining the effect of the compensation circuit 12 in FIG. 1, and FIG. 4 is an example of a robot system as a moving object. The configuration diagram shown. FIG. 5 is a speed pattern diagram of the speed command value output by the control device 2 of FIG. 4, and FIG. 6 is a diagram for explaining the drawbacks of the prior art. 1... Robot body 2... Control device 4...
Arm 7... Servo motor 10... Linear potentiometer 11... Arm mechanism 12... Compensation circuit 1
3...Arm simulator 14...Correction circuit cylinder 1
WJ1? Figure 2: Figure 4: Figure 5

Claims (1)

[Claims] 1. In a positioning control system for a moving object, the moving object is expressed mathematically by a state equation that can predict its mechanical vibration state, and the mechanical vibration of the moving object according to a position command value is expressed as a state variable of the state equation. The mechanical vibration is suppressed by sequentially correcting the position command value by applying negative feedback to the position command value with a correction value based on the state variable successively obtained by the prediction. An optimal control method for a moving object characterized by: