JPS585802A - Controlling system for kinetic mechanism with multifreedom - Google Patents

Abstract

PURPOSE:To change the speed of the titled mechanism and to improve its accuracy by feeding back the fluctuation torque generated by the influence of mutual interference, centrifugal force, Coriois force, gravity, etc. of working shafts on the basis of signals relating to the position and speed corresponding to each working shaft. CONSTITUTION:A positional signal (x) of each working shaft of a multifreedom kinetic mechanism is obtained by passing a signal outputted from a position detector 1 fitted to the working shaft through a waveform shaping element 2 and a speed signal (x') is obtained by passing the positioned signal (x) through a speed operation element 3. A main operating element 4 operates a torque command U on the basis of these positional signal (x), speed signal (x') and the value of a command from a storing element 5. The torque command U is amplified by an amplifier 6 and applied to a servomotor of a multifreedom kinetic mechanism 7. Namely, a fluctuating torque component influenced by centrifugal force and Coriolis's signal (x) and the speed signal (x') and these are fed back.

Description

DETAILED DESCRIPTION OF THE INVENTION According to the present invention, a multi-degree-of-freedom motion mechanism is realized by connecting a plurality of motion axes, such as an industrial WDF. It relates to continuous orbit control at high speed and with high precision.

In general, the equation of motion of a self-centered motion mechanism driven by a thermomotor is the displacement (position) of the first motion axis.
If we set z wz (1,'l*-・,#鳳)1 as l, then mu"i+B(x,i)+C(x4)mu...
Formula (1)% Formula% 1) Muro c) Inertia matrix of nX Otori that changes with axO value,
伽) B (x, i) is a vector that changes depending on X and the i0 value that represents the speed], during the eccentric force: Iv ori 0
(c) C(x, g) is an n vector that changes depending on the gravitational acceleration g and the XO value, and the variable torque caused by the gravitational influence, (A:) * is an n-vector indicating the torque generated by the shutter for driving each operating axis.

There are two main ways to control 90 types of multi-degree-of-freedom motion mechanisms:
There is.

One of them is something that can be easily realized with a simple control device.
The driver drives the motor 4 based on pre-stored command values, and performs speed and position feedback control for each shaft operation axis so as to reduce the deviation between the position of each operation axis 01A and the command value. In other words, the tip of the motion mechanism is continuously controlled along the desired trajectory. Since the ζO control method configures a control system for each operating axis, the control device becomes simple.
There is an advantage. However, when performing movement and elevation that involves movement of a large amount of displacement at high speeds, load fluctuations occur due to interference between the operating axes], or due to the effects of centrifugal force, Coriolis, and gravity, meandering or overshooting occurs. occurs, making it difficult to perform high-speed and highly accurate continuous trajectory control.

The x')o method calculates the fluctuating torque caused by interference between each operating axis and the effects of centrifugal force, Coriolis, and gravity from the position, velocity, and acceleration signals without corresponding to each operating axis. The variable torque is canceled by feeding back the calculated value, and then the acceleration, velocity, and position are feedback-controlled to continuously control the tip of the motion mechanism along the target trajectory (<4h).

In other words, the inertia matrix Mω in the previous equation (1) is separated into a term 8ω that changes depending on the 0 position of the pot motion axis and a term that does not change (Mω − birdω).
Using the formula (U of 11 to 1 = Mu, ωaM 10B (cei) 10 (xmg) - Ka
x −Ks 'x −K@ x+v = formula (
2). Here, 4 is an acceleration feedback matrix (nXm).

The velocity feed Δ ivy row n (n x n ), and the position feedback matrix (n x tortoise).

Ma is the command value.

In this case, the relationship between the command value Ma and each state, ``X s
If each feedback gain matrix is selected so that the characteristic machine of y, Nashi, and Fin is on the left half plane, a system that is stable and follows the specified value ma is constructed. In this way, this control method has the effect of canceling out the fluctuating torque, and has the advantage of realizing highly accurate continuous trajectory control compared to the former method.

However, since this control requires acceleration signals corresponding to each axis of motion, there are the following problems.

Generally, the acceleration signal is obtained from an acceleration detector, or it is obtained by twice differentiating the position signal or dividing the velocity signal Q1ml, but when obtaining it with an acceleration detector, the detector itself is very expensive. In addition, in a small multi-degree-of-freedom motion mechanism, the acceleration detector becomes a large load, which increases the speed of motion. On the other hand, when determining acceleration by differentiating a position signal or a speed signal, Fi requires that the original signal, the position signal and speed signal, be a highly accurate signal that does not contain noise. As a result, the acceleration signal appears to be extremely inaccurate, making it difficult to perform high-precision continuous cage control.

The purpose of the present invention is to realize high-speed, high-precision continuous trajectory control in view of the drawbacks of the prior art described above. From this, we can feedback the variable torque component caused by mutual interference of the operating axes, centrifugal force, Coriolis, gravity, etc.

FIG. 3 is a control FVS diagram of an embodiment of the present invention. To explain with reference to the figure, the position signal Xa of the operating axis of the multi-degree-of-freedom motion mechanism is obtained by passing the signal from the position detector 1 attached to each operating axis through a waveform shaping element, and the speed signal amount is This position information value is passed through the speed calculation element 3 to obtain it. These position 4M 1 Gain matrix K.

(nXn matrix) and velocity feedback dyne matrix K
1 (mXn matrix), the position signal X and the speed signal amount described above, the torque finger *U (n vector) given to the circa motor that drives each operating axis is calculated according to the following equation (3)K.

U=B Cx g x ) +C'Cx t t )
+ Aer)” (-K(X'-K2x + v) This torque command υ is amplified by the amplifier 6 and the multi-degree-of-freedom motion mechanism 7
data is applied to the motor. That is, from the position signal X and the speed signal i, there is a variable torque B (!

i) and calculate the variable torque C(x, g) due to the influence of gravity, feed/4-tsect these, and generate the position signal X.
Since the position of the shaft motion axis 'fs'l*・-, #nK changes, the inertia matrix M(b) is calculated and the command signal M and speed feedback signal -K are preset in the ζO matrix Mω.
The signal obtained by multiplying li and the position feedback signal - by the addition value of x is packed. By configuring such a feed barrier flow, the following relational expression (4) is established between the predetermined command signal M and each state X, X, and Violet.

Case + 1i + Fable
Therefore, by selecting the characteristic machine of equation (4) to be located on the left half plane, the nonlinear characteristics due to centrifugal force, Rioli's force, and gravity can be canceled without using the conventional applied acceleration signal. A stable and predetermined command signal V
A system that follows is constructed.

In this embodiment, in each feedback 247 matrix,
As shown in the following equation (5), K is selected so that all elements other than the diagonal elements are zero1. Therefore, the command belief proverb (vs e Vs @...,
The ***-th element 71 of [-] does not have any influence on the other operating axes since the data for driving the ill-th operating axis is an instruction to the motor.

As explained above, according to the control method of the present invention, the centrifugal force peculiar to a multi-degree-of-freedom motion mechanism; Interference is significantly reduced, and a control system can be constructed that follows the command value at high speed and with high precision 1KK.

[Brief explanation of the drawing]

The figure is a control boot diagram showing one embodiment of the present invention. In the drawing, l is a position detector, 2 is a waveform shaping element, 3 is a speed calculation element, 4 is a main calculation element, 5 is a storage element, 6 is an amplifier, 7 is a multi-degree-of-freedom movement mechanism, I is a position signal, i is the speed signal, K1 is the m*fee y A t ck r in matrix, and 4 is the turn! Feedback dyne matrix, U#i torque command to the 4-torer. is a command value signal.

Claims (1)

[Claims] In a method for controlling a multi-degree-of-freedom motion mechanism configured to connect a plurality of motion axes and drive each motion axis by feeding back a position signal and a speed signal corresponding to each motion axis , the fluctuation torque due to the influence of gravity, Rioli's force, and centrifugal force is calculated from the above position signal and speed signal, and the inertia matrix determined by the position of each operating axis is calculated from the above position signal. A control method for a multi-degree-of-freedom motion mechanism, which is characterized in that the inertia matrix calculated from , , and is multiplied by the addition value of the command note, the speed feed batter signal, and the position feed pack signal, and then fed back.