JPS60263206A - Control device of manipulator - Google Patents

Abstract

PURPOSE:To reduce the non-linearity and time interference which a manipulator has, and to execute a good positioning control by adjusting a compensating characteristic of a non-linear compensating means, in the direction in which a feedback quantity detected successively decreases. CONSTITUTION:A non-linear compensating means B compensates the non-linearity which a manipulator A has and sets a control input to the manipulator A like a feedforward. A feedback means C sets a feedback quantity corresponding to a deviation of a target value and an actual value and adds it to the control input. The manipulator A is controlled thereby, and on the other hand, a compensating characteristic adjusting means D is provided, by which a feedback quantity by the feedback means C is deteced, a compensating characteristic of the non-linear compensating means B is adjusted in the direction in which the feedback quantity detected successively decreases, and the compensating characteristic is corrected.

Description

DETAILED DESCRIPTION OF THE INVENTION (Field of Industrial Application) The present invention relates to a control device for a manipulator having a nonlinear compensation function.

<Prior Art> As a conventional manipulator control device, there is one disclosed in, for example, Japanese Unexamined Patent Publication No. 169203/1983.

This method assumes that the control characteristics of a moving object (robot) are linear within a certain range, and controls the compensator by switching the control gain according to the posture, operating speed, and load, improving the compensation function. This is what I was trying to do.

<Problems to be solved by the invention> However, in general, the motion of a manipulator can be linear even if it is assumed to be a rigid body, and for manipulators with large arm inertia or manipulators with high operating speed, nonlinearity may occur. Therefore, a control method based on linear proximity can only be applied to manipulators with extremely small arm inertia and slow operating speed.

Therefore, the present invention aims to improve the strong non-linearity of a manipulator, even when the arm inertia is large, when performing high-speed operation, or when operating while gripping a heavy workpiece with sufficient actuator power (motor power). It is an object of the present invention to provide a manipulator control device that can reduce interference between shafts and perform good positioning control.

Means and operation for solving the above problems> Therefore, in the present invention, as shown in FIG. 1, the control input to the manipulator A is set in a feedforward manner by compensating for the nonlinearity of the manipulator A. nonlinear compensation means B;
Feedhacking means C is provided to set a feedhacking amount according to the deviation between the target value and the actual value and add it to the control input, and while controlling the manipulator A, the feedbanking amount by the feedbanking means C is controlled. A compensation characteristic adjustment means is provided to adjust the compensation characteristic of the nonlinear compensation means B in a direction that reduces the amount of feed hack detected sequentially, thereby optimizing the compensation characteristic.

<Embodiment> FIG. 2 shows a system configuration of an embodiment of the present invention.

! 1 is a manipulator to be controlled.

2 is a feedforward compensation controller, which determines the nonlinearity (inertial force) of the manipulator 1 from the target values θd and θd of the joint angle and joint angular velocity of the joint for each link of the manipulator 1.

Coriolis centrifugal force, viscous friction, and gravity) are compensated in a feedforward manner to determine the feedforward control human power UFF to the manipulator 1, and the manipulator tag force is linearized. Here, the amount of compensation is a previously stored parameter unique to the manipulator 1, that is, the center of gravity position Si of each link as seen from the coordinate system set for each link.
(Si,,Si,.

SL, ), link weight mi, link length 11. Moment of inertia matrix of each link J = -d+ag [' tx-
I my.

It2] (i = 1 to n, n: number of joints)
E is done.

3 is a feed hack controller, which has target values θd, θd and actual values θ of joint angles and joint angular velocities.

The feed bank amount UFB is determined according to each deviation from θ. Specifically, each deviation is multiplied by θ by 6, and these are added to determine the feedback amount UFB.

Then, the feedback amount U□ from the feedback controller 3 is added to the feedforward control human power UFF by the feedforward compensation controller 2,
The control human power U thus obtained is applied to the manipulator 1, and the portion that cannot be compensated for by feedforward compensation is controlled by feedhack control so that the deviation from the target value approaches zero.

4 is a parameter correction device, especially the manipulator 1
In order to effectively compensate for the control characteristics that change when the hand grasps a workpiece, the parameters of the tip link integrated with the hand, that is, the center of gravity position S,
, link weight m7. Link length x11. moment of inertia J
11, etc., the stored values of the feedforward compensation controller 2 are corrected and the compensation characteristics thereof are adjusted. Here, the correction method is to sequentially detect the feedback amount UFB by the feedback controller 3, and to compare the direction of the previous parameter correction and the feedback it'ur r.
-, the parameter value is increased or decreased by a predetermined value in the direction that the feedback amount U, 8 decreases.

This will be explained in more detail below.

As is well known, multi-joint manipulators have nonlinearity as shown in equation (1) below, and this nonlinearity is a problem in achieving high-speed, high-precision motion with linear feedback. It's coming.

J(θ) #+ff (θ,/)) +V (θ)t+
g, (θ')-τ...(1) Here, θ-[θ1. ...θn]t (θ8: each joint joint angle, n: number of joints) τ=[τ1. ...τll]' (τl: Driving torque to each joint) J(θ): Moment of inertia matrix f (θ, 6): Coriolis centrifugal force term V(θ)': Viscous friction g(θ) of each joint ): Gravity type Therefore, by determining the input torque in a feedforward manner as in the following equation (2), the nonlinearity of the manipulator can be removed to some extent, and it can be linearized as in the following equation (3). This allows linear feedbank control to be performed more effectively.

r=J (θ) [K1 θ102/)+GUFF]
10 fr (L /)) +V <e) /) +g <1
)) ・(21 (K+, IKz, G are constant matrices) θ=2/)+, θ+GUFF...(3)
Note that for θ and tide, target values θd and /)d of the joint angle and joint angular velocity given in advance are used.

Here, complicated calculations such as equation (2) are performed using
The center of gravity position of each link, Si = [Si,, Siy, Si,] t, based on the coordinate system set for each link.

Link weight mi, link length! ,, moment of inertia matrix J
, and a transformation matrix between link coordinate systems (Luh's algorithm).

However, when the manipulator grips the workpiece, the aforementioned parameter S regarding the tip link. 1m7゜7! ,,'
J changes, and the effect of nonlinear compensation deteriorates. This is especially noticeable when gripping a heavy workpiece.

Therefore, we estimated the parameter changes related to the tip link,
It is necessary to perform more accurate nonlinear compensation.

By the way, in many cases, considering the hand that grips the workpiece and the tip link, they can be considered symmetrical with respect to the link length direction, and below, the parameters related to the tip link are expressed as S, , = [0, 0, sz ]t (Z in the link length direction
Take the axis) 1m a, #,, J.

-diag ['l)+, I Il+ Iz]
As, six parameters l'n + Sz + ”
n + IIIXI I,,y,l1ll! (below 6
Let these parameters be P(i) (i=1 to 6). ) is estimated.

FIG. 3 shows a flowchart of the algorithm for parameter estimation, and FIGS. 4 and 5 show the state of parameter estimation. The explanation will be based on these figures.

The basic idea of parameter estimation is as follows. 6
First of all, one parameter p (11
Pay attention to the initial parameter value P given when no workpiece is being gripped. (1) first by a predetermined value ΔP in the positive direction.
(1) and the control result as a result of nonlinear compensation, that is, the feedback amount corresponding to the deviation between the target value and the actual value is the previous parameter value (the first search is P. (i))
The success or failure of the parameter search is evaluated based on whether the amount of feedback is smaller than when performing the same operation. If it is successful (that is, it is smaller than the previous time), move ΔP (i) further in the same direction, that is, in the positive direction, and if it fails at the start of the search, move ΔP (i) in the opposite direction, and if it continues to be successful and fails, the previous Assuming that there is a true parameter value between the parameter value and the parameter value at the time of failure, for example, use the intermediate value as the parameter estimate value, move on to the search for the next parameter P (i+1>, and repeat the same process.

FIG. 4 shows, in chronological order, the state of the feed bank control amount UFB among the control states when parameters are searched according to the above basic idea. The horizontal axis represents the parameter estimation time, for example, the time taken for one repetitive operation divided into 10, and the vertical axis represents the feedback control amount UFB.
shows the absolute value of It starts operating from a stationary state at time =0, and has positioned itself at the target by time =10. In the figure, the mark O indicates the value of the feed bank control amount U r B Tk) when the parameter was being corrected during the previous repetitive operation, and the mark X indicates the value of the feed bank control amount U r B Tk) when the parameter was being corrected during the current repetitive operation. Bank control amount U r m (k)
shows the value of

The estimation algorithm will be explained in conjunction with the estimation flowchart shown in FIG.

First, for the parameter P(i) that is 0 at time, P(
11+ A P (i) and finely move it (7o-34 in Figure 3), combined with other parameters PU) (jf-i),
It is assumed that nonlinear compensation is calculated (calculating equation (2) using Luh's algorithm), and the feedforward control input is set to OFF and output (S6).

At the next time, when =1, the feed bank amount U
UFI (1) l ' l UFI (111) is determined, and it is determined whether this ΔU (11) is positive or not, that is, whether the parameter has been corrected and (the absolute value of) the amount of feedback has decreased (S2).

If ΔU is negative, it is assumed that the parameter search has failed, and if this is the first search, change the direction of the slight movement (812 to 514).
, if it is a failure after success, the value of the true parameter P (il is assumed to be between the previous value and the corrected failed value this time, and P(1)'AΔP (i) is the parameter estimate ( S8), the search for P(i) is completed, and the search for another parameter P(il1) is started (S9).

Then, it is determined whether the search for all six parameters has been completed (510), and once the search has been completed, the search is performed again from the first parameter (S11). The reason for this is that there is a possibility that an instantaneous disturbance may be mixed in during the parameter search, resulting in an error in the parameter search, and the optimal parameter estimate can be obtained by constantly repeating the process.

Now, if ΔU(11) is positive in S2, it is assumed that the parameter search may have been successful.However, if the modification of other parameters P(i-1) was successful last time, then ΔU(
1) is naturally a pressure, so it is impossible to tell whether the search for parameter P (11 was successful) just by the sign or minus of ΔU(k).

Therefore, if ΔU (1) is larger than ΔU (0) at time = 0, it is determined to be a success, and if it is smaller, it is determined to be a failure (S3).

If the search fails, the process goes through the processes described above (37 to 511) and moves on to searching for the next parameter P (il1).

If successful, the search is repeated by further making a fine movement (ΔP fi) in the same fine movement direction (34 to S6).

Fig. 4 shows that the search for the parameter P (11 is started from =0 at time, and the parameter modification up to =4 is successful at time,
As a result of modifying the parameter at time = 5, the search failed at k = 6, and the estimated value of parameter P (kl was changed to P(i) 'A
ΔP (11.Then, the next parameter p(
The search for p (i
The search for p (il1) is completed with the estimated value of l1) being P (il1)', 4P (il1).

Figure 5 shows the six parameters P(1) (i=
1 to 6). However, when i in FIG. 4 is set to 1, only P(1) and P(2), which were searched for parameters, are shown, and P(3) to P(6) are omitted because they are constant values.

a parameter correction device 4 that executes the above algorithm;
Feedforward compensation controller 2 based on Luh's algorithm and positional deviation.

The feed bank controller 3, which performs proportional feedback control on speed deviation, can be configured with a microcomputer, for example, or a dedicated discrete I
Although it can be constructed using C or the like, FIG. 6 shows an example of a 6-axis manipulator constructed using a microcomputer.

In Figure 6, 11 is a microprocessor (MPU)
In the figure, the power supply circuit 2 oscillation circuit, recent circuit, etc. are omitted. 12 is six subdivisions, that is, six D/A converters;
13 is a current amplifier, 14 is a DC motor, and MPUI
The torque command from I is converted into an analog voltage by the D/A converter 12, and the DC motor 14 of each axis is driven via the current amplifier 13. 15 is a pulse generator; 16 is a waveform shaping circuit; the pulse generator 15 detects the angular velocity of each joint;
Enter in PUII. Reference numeral 17 denotes an amplifier down counter, which detects a joint angle from the angular velocity of each joint input via the waveform shaping circuit 16 and inputs it to the MPU II.

Here, inside the MPU II, software generates a target joint angle θ4° and angular velocity θ4 for each joint, performs feedforward compensation calculation and feedbank control calculation, and outputs a torque command for each axis.

In addition, in this example, only the parameter estimation of the tip link was performed, but it was applied to all links, corrected the model error,
It is also possible to perform better nonlinear compensation, and it is also applicable when all elements are included as parameters in the moment of inertia matrix of each link.

Further, in this embodiment, the feed bank amount tJFR is directly detected, but it may also be detected from the deviation between the target value and the actual value.If the feedback gain is not constant, this method provides more accurate control. is possible.

<Effects of the Invention> As explained above, according to the present invention, the control characteristics of the manipulator that change due to gripping of a workpiece, etc. are estimated using an extremely simple algorithm, and the corresponding compensation characteristics are obtained, so that the control characteristics The originally intended nonlinear manipulator can be linearized regardless of the fluctuation of Therefore, despite the strong nonlinearity of the manipulator, high-speed operation can be achieved.
Furthermore, even when an unknown workpiece or a heavy workpiece is gripped, the repeatable positioning accuracy can be improved.

[Brief explanation of drawings]

FIG. 1 is a block diagram showing the configuration of the present invention, FIG. 2 is a system configuration diagram showing an embodiment of the present invention, FIG. 3 is a flowchart of the parameter estimation algorithm, and FIGS. FIG. 6 is a diagram showing an example of a hardware configuration using a microcomputer.

Claims (1)

[Claims] nonlinear compensation means that compensates for nonlinearity of the manipulator and sets a control input to the manipulator in a feedforward manner; and sets a feedbank amount according to a deviation between a target value and an actual value and adds it to the control input. Feedbank means; further comprising compensation characteristic adjusting means for detecting the feedbank amount by the feedbank means and adjusting the compensation characteristic of the nonlinear compensation means in a direction in which the detected feedbank amount sequentially decreases. A manipulator control device characterized by: