JPS6177906A - Controller of manipulator - Google Patents

Abstract

PURPOSE:To reduce greatly the arithmetic load in a positioning mode by discontinuing the nonlinear compensation and applying the feedback control only at a point close to the final target position or speed where the nonlinearity can be ignored. CONSTITUTION:A DC motor 4 of each joint shaft of a manipulator is driven by the output of a microcomputer 1 via a D/A converter 2 and a current amplifier 3. Then a tachogenerator 5 detects a joint angle speed d/dt(theta) and at the same time a counter 6 detects a joint angle theta respectively. Here the final target joint angle of the manipulator is compared with an actual angle theta. When the actual angle theta is set within a prescribed range, the final target joint angle speed is compared with the actual speed. Then it is regarded that the angle speed is slowed satisfactorily down to the final target value when the actual speed is kept within the prescribed range. Then the feedback control degree U is calculated from the deviation between the target angle and angle speed and these actual values. Thus a command torque T is calculated and delivered.

Description

[Detailed Description of the Invention] <Industrial Application Field> The present invention provides nonlinear compensation l! The present invention relates to a control device for a manipulator having the following functions.

<Prior Art> Conventionally, in manipulator control devices, nonlinear compensation has been performed in addition to feedback control. In other words, when the manipulator to be controlled has a multi-joint structure, its movement is always affected by nonlinear forces such as moment of inertia, Coriolis centrifugal force, and gravity, so it is necessary to By calculating the nonlinear compensation amount from the joint angle and joint angular velocity of the manipulator and nonlinearly compensating the control output (torque command) to the manipulator, the nonlinear effect of the manipulator is canceled out and linear operation is realized (Japanese Patent Application Laid-Open No. 55-41585).

<Problems to be Solved by the Invention> However, in such conventional manipulator control devices, even if high-speed calculation algorithms (for example, Luh's algorithm) are used in calculations for nonlinear compensation, multi-joint If there are multiple degrees of freedom, the calculation time will increase significantly, and the feedback control period will have to match the nonlinear compensation period, resulting in a long control period.
There is a problem in that precision servo control during positioning, which does not require much nonlinear compensation, results in coarse control, which takes time for positioning, leading to a decrease in manipulator operating performance.

SUMMARY OF THE INVENTION An object of the present invention is to provide a manipulator control device that can improve the operational performance of a manipulator during positioning.

In order to achieve the above object, the present invention, as shown in FIG. to M (7) IJ '+B Feed bank control means A that performs feedback control of the output;
In order to compensate for the nonlinearity of the manipulator M, a nonlinear compensation means B for nonlinearly compensating the control output is provided, as well as a nonlinear compensation stop for stopping the operation of the nonlinear compensation means A when the actual value reaches around the final target value. The present invention is characterized in that a means C is provided.

A second feature is that a feedback control period switching means is further provided for shortening the feed hack control period of the feedback control means A in synchronization with the operation of the nonlinear compensation stop means C.

Thirdly, the present invention is characterized in that it is further provided with a feed bank control gain switching means E that switches to a feed bank control gain that matches the control period in synchronization with the operation of the feedback control period switching means.

In other words, during high-speed operation when the manipulator's nonlinearity is strong, linear feedbank control is performed while nonlinear compensation is performed as before, and when positioning near the final target value where nonlinearity is not a problem, nonlinear compensation is performed. In this case, the control is stopped and controlled using only feedback control (first invention), and furthermore, the feedback control gain is switched to match the feedbank control period (third invention).

<Example> Examples will be described below.

In the figure showing an example of the hardware configuration, 1 is a microcomputer (CPU), its output calculation block, and 13 is a manipulator.

FIG. 4 is a flowchart showing the operating procedure, which corresponds to the first invention.

This flow is repeated every predetermined control cycle, but first, in step 1 (SL in the figure), the current joint angle θ and joint angular velocity θ of the manipulator are detected and taken in as data.

In step 2, the final target joint angle TT of a certain movement is compared with the actual value θ to determine whether it is within a predetermined range (A is a predetermined value). If YES, in the next step 3, the final target joint angle TT is determined. The joint angular velocity 77 (usually zero) is compared with the actual value θ to determine whether it is within a predetermined range (B is a predetermined value).

If the result is NO1 in step 2 or NO in step 3, it is assumed that high-speed operation is in progress (see Figure 7), and the process proceeds to step 11 based on the deviation between the target values θr, θr and the actual values θ, θ at that time. Feedback control i1u determined by
(see FIG. 3), and in the next step 12, a nonlinear compensation amount (command torque) T is calculated and output from U, θ, and υ by nonlinear compensation calculation.

If YES in step 3, it is assumed that the speed has become sufficiently low that it has approached the final target value (see Figure 7), and step 21
Proceed to and calculate the target values θr, θr and actual values θ, θ at that point.
The feedback control amount U determined based on the deviation from
In this case, without performing any nonlinear compensation operation,
In the next step 22, TwA'u+8'(A',
B' is a predetermined value), the command torque T is calculated and output.

The flowchart in FIG. 5 corresponds to the second invention, and when performing feedback control and nonlinear compensation, the control period is changed to D in step 13 following step 11.12.
TO, but if you want to stop nonlinear compensation and perform only feedbank control, change the control cycle from DTO to DTI (DTI<D
TO) to advance the control H cycle.

The flowchart in FIG. 6 corresponds to the third invention, and when the control period is shortened by switching to DTI in step 23, the feedback control gain is adjusted accordingly in the next step, step 24. (See Figure 3).

Next, the action will be explained.

First, we will discuss nonlinear compensation that removes the strong nonlinearity of the manipulator.

As is well known, multi-joint manipulators have nonlinearity and inter-axis interference as shown in equation (1) below.
This becomes a problem in achieving high-speed, high-precision operation using linear feedback.

J (θ) I9i + f (θ, tide) + V (θ) tide + g (θ) = T ... (1) Here, θ = [θ1. ...θ. ] also (θ,: joint angle of each joint, n: number of joints) T = (T,,i-T,)' (Tl: driving torque to each joint) J(θ): moment of inertia matrix f (θ, θ): Coriolis centrifugal force term V <O>: Viscous friction g of each joint (θ) Double force term Therefore, by setting the command torque as in (2) 2 below, for example, the nonlinearity of the manipulator can be reduced. .

Inter-axis interference can be removed to some extent, linearization can be performed as shown in (3) 2 below, and linear feedback control can be performed more effectively.

T=J (θ) (F, θ+F z /l + Gu
) + f (θ, σ) + V (θ) Shio 10g (θ)...
・(2)/j=Fze1F, 0 + Gu
−(31 Here, F l+ F z, G is a constant matrix, and F,, F,, G are selected as diagonal matrices in order to specifically remove interference. U is a feedback control amount. .

Therefore, in the nonlinear compensation calculation block, the specification values of the manipulator, that is, the center of gravity position of each link expressed in the coordinate system fixed to each axis, the link weight, the moment of inertia,
Friction coefficient, transformation matrix between link coordinates, gain F1, Ft
, G, and the joint angle θ and joint angular velocity of the manipulator 6. and the feedback control m1ku which is the control output of the linear feedback controller, and
) is calculated using Luh's algorithm (an algorithm that can calculate equations in the form of formula (1) at high speed), and the command torque T, which is a nonlinear compensation amount, is determined and output to the motor of each axis of the manibulator. do. As a result, the manipulator is shaped into a line, and each axis can be regarded as substantially independent.

Now, if we consider the compensation calculation of equation (2) with a general 6-axis multi-joint manipulator, there are approximately 600 multiplications and additions in total, which is 20 to 50 m5 even with the current calculation processor.
However, the control period of the linear feedback controller must be synchronized with this, and since the mechanical resonance frequency of the manipulator is generally around 10 Hz, it can be said that the control period is sufficiently short. This results in problems such as decreased positioning accuracy and increased settling time.

By the way, as can be seen from equation (11), the nonlinear force of the manipulator is strong when θ and θ are large, and it does not appear much at low speeds.In other words, as shown in Figure 7, the When it approaches, the speed has slowed considerably, the change in J(θ) is small, and nonlinear compensation is no longer necessary.

Therefore, in order to simplify the compensation calculation, IT7-01<A,
If it enters the region of +N lower -θ1<H (A.

B is determined appropriately from the nonlinear force characteristics of the manipulator)
, Haboshio = 0, T = A'u + B'' ... (4) Here, A'-J CIJ lower) G.

B'-J　<II lower) rear +f (#r, 0)+g<far) constant Only feedback control is performed at (first invention).

As a result, the nonlinear compensation calculation that occupied a considerable part of the calculation in the control system is eliminated, and the calculation load is drastically reduced.

In the second invention, the computational load drastically reduced in the first invention is used to shorten the control period of the linear feedback controller. In order to perform precise positioning in a short settling time, it is necessary to detect deviations from the target value quickly and take action quickly. )
By doing so, as shown in FIG. 8, the settling time is shortened and the position repeatability is also improved.

In the third invention, in addition to the second invention, the control gain is switched to +Kg (see FIG. 3) to match the changed feedback control cycle.

For example, considering the example in Figure 10, in a closed loop system, θ-
(F, +CrKZ)θ-(Fl+GKZKl)θ-GK
, θr −(5). Here, we convert this to Z transformation fi (Z=6'^(, Δ
t=control period), and since Kz is a function of Δt, the control gain that matches the control period is +1. The meaning of switching to Km is obvious.

Incidentally, FIG. 9 shows the difference in control performance between a case where the gain is not switched and control is performed using a gain matched with the DTO period, and a case where control is performed with a gain matched with a D71 period.

<Effects of the Invention> As explained above, according to the present invention, during high-speed operation when the manipulator is highly nonlinear, linear feedback control is performed while nonlinear compensation is performed as before, and the final target position where nonlinearity does not pose much of a problem can be achieved. and when approaching the speed,
Since nonlinear compensation is stopped and control is performed only by feedback control, the effect of greatly reducing the calculation load during positioning can be obtained.

Secondly, by shortening the feedback control period at the same time as stopping nonlinear compensation, it is possible to improve the position repeatability and shorten the settling time.

Thirdly, is there a feedbank system that is suitable for shortening the feedback control period and at the same time? By switching to the Im gain, it is possible to obtain the effect that the control performance can be further improved by selecting the optimum gain.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing the configuration of the present invention, FIG. 2 is a block diagram showing an example of a hardware configuration using a microcomputer, FIG. 3 is a control system block diagram, and FIGS. A flowchart of the operation procedure corresponding to the third invention, FIGS. 7 to 9 are diagrams for explaining the operation, and FIG. 10 is a block diagram of the control system when nonlinear compensation is stopped. ll...Linear feedback controller 12...
Non-linear compensation calculation block 13...Maneviator patent applicant Nissan Motor Co., Ltd. agent Patent attorney Fujio SasashimaFigure 4Figure 5Figure 6Figure 7

Claims (3)

[Claims] (1) Feedback control means that performs feedback control on the control output to the manipulator while comparing the target value and actual value at each point in time, and nonlinear compensation means that nonlinearly compensates the control output to compensate for the nonlinearity of the manipulator. A manipulator control device having:
A control device for a manipulator, comprising a nonlinear compensation stop means for stopping the operation of the nonlinear compensation means when the actual value reaches around the final target value. (2) Feedback control means that performs feedback control on the control output to the manipulator while comparing the target value and actual value at each time, and nonlinear compensation means that nonlinearly compensates the control output to compensate for the nonlinearity of the manipulator. A manipulator control device having:
nonlinear compensation stop means for stopping the operation of the nonlinear compensation means when the actual value reaches around the final target value; and feedback for shortening the feedback control period of the feedback control means in synchronization with the operation of the nonlinear compensation stop means. 1. A control device for a manipulator, characterized in that a control period switching means is provided. (3) Feedback control means that performs feedback control on the control output to the manipulator while comparing the target value and actual value at each point in time, and nonlinear compensation means that nonlinearly compensates the control output to compensate for the nonlinearity of the manipulator. A manipulator control device having:
nonlinear compensation stop means for stopping the operation of the nonlinear compensation means when the actual value reaches around the final target value; and feedback for shortening the feedback control period of the feedback control means in synchronization with the operation of the nonlinear compensation stop means. A control device for a manipulator, comprising: a control period switching means; and a feedback control gain switching means for switching to a feedback control gain that matches the control period in synchronization with the operation of the feedback control period switching means.