JPS61251915A - Moving body control system - Google Patents

Abstract

PURPOSE:To control a driving means on a basis of the state variable of a state observing means to prevent the influence of disturbance by including this means as a model of the deviation space of an object to be controlled and feeding back the synthesized output of deviation of the state observing means and a deviation means to the state observing means. CONSTITUTION:An object to be controlled 6 of a moving body control system is provided with an actuator which drives a robot as the load and a detector which detects the state of this actuator. The control object 6 is operated by controlling a current, and a current value Y is applied to a deviation means 6a to output a deviation Ye between the current value Y and a command value Y0. The control object 6 and a state observing means 7 are constituted with a circuit or software of the model in the same deviation space, and the actual deviation Ye from the means 6a and an estimated deviation Ye from the means 7 are synthesized by a synthesizing means 8. The output from the means 8 is given to the means 7 from the first gain means 9b as a feedback gain, and the actuator is controlled on a basis of this feedback variable.

Description

[Detailed description of the invention] [Table of contents] Overview Industrial field of application Conventional technology Problems to be solved by the invention Means for solving the problems (Fig. 1) Working examples (al) Description of one embodiment (Fig. 2, Fig. 3, Fig. 4) (bl Description of other embodiment of the first invention (Fig. 5, Fig. 6) (C1 An embodiment of the second invention) (Fig. 7) (d) Description of another embodiment of the second invention (Fig. 8) (e+Detailed description of the present invention (Fig. 9) ffl Description of another embodiment (Fig. 10) ) Effects of the Invention [Summary] A moving object control system that controls a driving means for driving a moving object based on a movement command and a force command, which includes a state observation means configured as a model of a deviation space of a controlled object, By feeding back the combination of the deviation of the object and the deviation of the state observation means to the state observation means and controlling the drive means based on the state quantity of the state observation means, high-speed and smooth movement and force control is possible. It is something to do.

[Industrial application field]

The present invention relates to a mobile body control method for controlling a mobile body such as a lohosoto using movement commands and force commands, and more particularly to a mobile body control method for controlling a mobile body by observing the state of the system.

In recent years, robots have been developed to automate tasks performed by humans, and efforts are also being made to develop technology to control robots to perform operations equivalent to human hands and arms.

If robots could perform tasks similar to those performed by human hands and arms, they would be able to perform other advanced tasks and be extremely useful.

In general, robot control is based on movement control such as position control or speed control of an actuator (arm), and a work hand at the tip of the arm is positioned at a commanded position to perform work.

This kind of position or speed control alone lacks flexibility, so it is insufficient to make it perform the same work as a human arm or hand, and it is not suitable for tasks that are influenced by other objects. There is no. Therefore, it is necessary to control the actuator by detecting the force (or reaction force from the outside) exerted by the actuator.

[Conventional technology]

Conventionally, force is controlled by giving a certain rigidity to the actuator for position control, setting a virtual target value first, and creating a certain deviation between the virtual target value and the current value. , a method of outputting force is known.

Also, if the robot itself has a force detection function,
A method such as converting the detected force into a deviation and subtracting it from a virtual target value is used.

[Problem that the invention seeks to solve]

On the other hand, human arms and hands have a function of movement such as positioning.
Since it has the necessary force control function and tactile function, it is necessary to equip the robot 7) with these functions and to activate these functions in a timely manner according to the work.

For example, to fit two members together, the robot positions one member relative to the other member in a fixed position, and after accurately aligning the other member using the tactile function, a predetermined pressing force is used to fit the other member. Let's do it.

In order to perform such work, it is necessary to change the control mode of the actuator at the time of positioning, alignment, and fitting.

However, in the conventional method, force is given by the product of stiffness and positional deviation, so if the stiffness is large, even a small deviation will generate a large force, and the stiffness cannot be increased very much. On the other hand, if the rigidity is low, it is likely to vibrate and cannot be driven at high speed. In addition, it is susceptible to disturbances such as gravity, resulting in poor positioning accuracy. Therefore, in the conventional method, handling of combined position and force control is complicated, and there is a problem that fine position and force control cannot be performed.

For this reason, the present inventors proposed a control method capable of finely controlling the position and force in patent application No. 109980/1980 (issued May 30, 1980, 1) 1).

The content of the existing proposal is to detect the force applied to the actuator by a detection means, obtain the difference from the command force, and perform force control, and also detect the difference between the current position and the command position from the detection means. In addition, the actuator is controlled by using the sum of these values with a predetermined weight as the basic control amount.

The content of the existing proposal will be explained using Figure 1).

In the figure, 1 is a multi-jointed 5-axis robot, with a hand 1a at the tip and arms lb, lc, and I that operate the hand 1a.
d, Ie, and a head 1f, the hand 1a, arms lb, lc, 1d, and 1e are rotated by actuators to move the hand 1a, and 21.22 is a position detector for each actuator. 3 is a signal processing circuit that constitutes a part of the force and position detector, and 3 is a signal processing circuit that calculates the detected position 4 is a computer that monitors the operating state of the robot from the detected position X and detected force f, and outputs the target command position xo, target command force fo, and first and second weights a and b. 5 is a control device (second control means) which controls the drive current i of the drive source (motor) of each actuator according to the command position xo, the command force fO1, the first and second weights a, b, and the current Control is performed so that y in the following equation becomes zero depending on the position X and the detection force f.

ye=a・Δx+b・Δf −−−−1−−(1
) However, Δx = x O−x −−−−−−(
2) Δf = f o−f −−−−(
31 In the previously proposed method, force is given as a command, so position and force can be precisely controlled, and since the control amount is the combination (sum) of position deviation and force deviation, it can be realized with the same drive system. It is possible and convenient.

By the way, the condition of equation (1) is not satisfied, and the position X (angle θ
) and force (torque) f, and in reality, a steady solution that satisfies the conditions of equation (1) is determined in five cases depending on external conditions.

For example, as shown in Fig. 12, even if a command is given to push with a force of f=fo (=1>) at the position x-xo (-4), the object BD will be pushed at
If there is, robot 1 will be at position f at x=xs (2)
It operates to push with a force of .=fs (=3), making it possible to control the object BD in response to an external factor, and the same applies when the object BD moves.

The point of the previous proposal is that the position and force can be controlled appropriately using such external factors, but in reality, the control amount y is obtained by transforming the formula fl1, and the command position xo, the command Command value yo due to force ro and current position X,
This is the deviation ye from the current value y due to the current force f.

In order to make this deviation ye zero, if achieved using classical control theory, with simple output feedback 7, it would take time to settle the influence of disturbances, the follow-up frequency would be low, and smooth and high-speed operation would be difficult. It is. (Also, it is possible to improve the characteristics by adding a compensation circuit, but since setting a policy based on classical control theory is generally done by trial and error, this becomes rapidly difficult when the order of the control system becomes 3rd order or higher.) In this case, the order of the control system is 4th order (first invention) or 5th order (second invention), so it takes a lot of effort to improve the characteristics of the control system, and the characteristics have to be significantly improved. (Improvement is difficult.) Therefore, an object of the present invention is to provide a mobile body control method that can achieve smooth and high-speed mobile body control by applying modern control theory.

[Means for solving problems]

FIG. 1 is an explanatory diagram of the principle of the present invention, FIG. 1(A) is an explanatory diagram of the principle of the first invention, and FIG. 1(B) is an explanatory diagram of the principle of the second invention.

In FIG. 1(A), reference numeral 6 indicates an actuator (driving means) that drives a moving object (such as a robot arm) that is a controlled object, specifically a load, and a detector 21 that detects the state of the actuator. 22 (Fig. 1)), etc., which is operated by the drive current i given to the actuator and outputs the current value y. 6a is a deviation means, which outputs the current value y.
7 is a state observation means (hereinafter referred to as an observer), which is the same as the controlled object 6.
A model in the deviation space of is configured in a circuit or software, 8 is a synthesis means, and deviation means 6a
9a is a first gain means, which gives an observer gain G to the synthetic output of the synthesis means and feeds it to the observer 7; 9b 10 is a second gain means, which adds a feed bank gain F to the observed state quantity of the observer 7 to create a control current ie, which is input to the observer 7; 10 is a synthesis means, which generates a steady current is The drive current i is created by combining the control current ie and the control current ie. In this case, the steady current consists of an accelerating current io for moving the moving body in space and a current if for generating a steady force, and the accelerating current i is given by the calculator 4, and becomes zero after the movement is completed. (The following !lI theory is 10
= 0).

That is, in the first invention shown in FIG. 1(A), an observer 7 is provided, and the observer 7 executes the same operation as the controlled object 6 in the deviation space to observe the controlled object 6. ), and the drive control of the controlled object 6 is performed based on the state quantity.

Next, in FIG. 1(B), the same parts as those shown in FIG. 1(A) are indicated by the same symbols, and 1) is an integrating means, which integrates the actual deviation ye; Reference numeral 12 denotes a synthesizing means, which generates the control current ie by synthesizing the integrated actual deviation and the output of the second gain means 9b.

That is, in the second invention, in addition to the first invention, the actual deviation y
The controlled object 6 is determined by the integral amount of e and the state amount of the observer.
This is to perform drive control.

[Effect]

In the present invention, an observer known as modern control theory is used. In this modern control theory, when the amount to be feedhacked is given by the deviation from the reference value,
The optimal feedbank gain F can be uniquely determined for a given evaluation function, and there is also a guarantee that the gain is the optimal gain.

However, as in the system of the present invention, there are two command values, a movement command and a force command, and the actual command value is a composite output of these, y, so the position and force command values and reference values are Modern control theory cannot be applied to find the feedback gain because they do not necessarily match and the amount of deviation from the reference value cannot be obtained.

In particular, since the order of the system is high, it is difficult to find a stable gain, and even if a gain is found, there is no guarantee that it is optimal.

Therefore, in the present invention, the control current ie is input to the observer, and the actual system deviation ye and the estimated deviation ↑e of the observer 7 are fed back to the observer 7, so that the observer 7 can always reproduce the actual system in the deviation space. That's what I do. Observer 7 can easily observe the state quantities of the system (for example, actuator speed and position, load speed and position), so if you perform foot hacking using the state quantities (deviation amount) of Observer 7, you can achieve stable and high speed. Operation becomes possible. Note that, like the feedbank gain F, the observer gain G can be uniquely determined for a given evaluation function.

Further, in the second invention, the integrated amount of the actual deviation ye is also fed-banked, so that stable operation can be achieved even against disturbances such as friction of the actuator.

〔Example〕

(Description of an Embodiment of the First Invention FIG. 2 is a block diagram of an embodiment of the first invention, FIG. 3 is an analytical model diagram of a controlled object, and FIG. 4 is a model block diagram of an observer.

In the figure, the same components as those shown in FIG. 1 and FIG. current ie
14 is a difference section which takes the difference between the current force f and the estimated deviation force fe at the observer 7. 15 is a multiplication section which divides the difference (f-fe) of the difference section 14 into 1/ Multiply the current by kT to generate a steady force! 20 is a motor (actuator) that creates f.
32 is a differential circuit that drives loads such as a to 10, and the rotation angle (position) θ1 of the motor 20 and the rotation angle θ2 of the load.
The multiplier 32a takes the difference between the two and multiplies it to create the real side force (current force) f. 50 is a difference circuit, and the detection power from the difference circuit 32 (of No. 1 processing circuit 3) f and calculator (4
51 is a multiplier circuit that calculates the difference Δf between the command force fO from the output cover sofa 40) (one that executes the above-mentioned equation (3)), and 51 is a multiplier circuit that calculates the difference Δf between the difference (error) Δf of the difference circuit 50 and the computer ( 52 is a difference circuit that multiplies the weight b from the output sofa 42) of No. 4 and generates an output b・Δf, and 52 is a difference circuit that calculates the current position θ2 (counting position of the 20th counter 31 of the signal processing circuit 3) and the calculator (4). 53 is a multiplier circuit that calculates the difference Δθ from the command position θ20 from the output cover sofa 41) (one that executes equation (2)), and 53 is a multiplier circuit that calculates the difference Δθ from the difference circuit 52 (error) Δθ and the calculator (4). 54 is a summation circuit which multiplies the weight a from the output sofa 43) and generates the output a.Δθ, and the outputs b.Δf of the two multiplier circuits 51.53,
It adds a·Δθ according to equation (1) and outputs the control amount ye. In observer 7, M1 to M1
2 are multipliers, each of which multiplies the input by a coefficient assigned in the block and outputs the result, A1 to A7 are each arithmetic units, each of which adds or subtracts two inputs depending on the polarity and outputs the result. (2) T1 to rT4 are integrators that integrate the input and output the result, and D1 to D4 are differentiators that output the difference between two inputs. 90-93
is a multiplier, and the synthesis output of the synthesis means 8, that is, (?e-
Observer gain 01~ indicated in the block in ye)
94 to 97 are multipliers that are multiplied by G4 and input to each differentiator D1 to D4 of the observer 7.
The outputs of the integrators IT1 to IT4 are multiplied by the feed bank gains F1 to F4, and 982 to 98c are adders, which add the outputs of the multipliers 94 to 97 to obtain the control current i.
e.

First, the configuration of the observer 7 will be explained with reference to the analysis model diagram of the controlled object in FIG. 3 and the model configuration diagram of the observer in FIG. 4.

In the analytical model shown in FIG. 3, a load 1 is directly connected to the output shaft of the motor 20, and the loads 1 and 2 are coupled by a torsionally rigid spring.

At this time, the equation of motion becomes as follows.

J1θ+ +D+ f)1+K (θl-θ2)=kT
i−・−(5) J21)2+D2θ2+K(θ2−〇+)=f'−−−
----- (6 squares, Jl: inertia factor of load 1 (motor inertia factor) Dl: damping force acting on load 1 θ1: rotation angle of load 1 (detected by encoder 21 in the motor example).

J2: Inertia factor of load 2 (load inertia factor) D2: Damping force acting on load 2 θ2: Rotation angle of negative riI2 (load side inertia factor) kT:
Torque constant t of the motor 20: Current kTi flowing through the motor 20: Output torque f' of the motor 20: External force (self-weight, reaction force) of the load 2.

The force f transmitted from load 1 to load 2 can be expressed as follows.

f=K(θ1-θ2) -----
-(7) Here, the linear sum y of the position θ2 and the force f expressed by the following equation is taken as the output of the model 1.

, -3θ2 + b f ---
--181 The target value yo of the output y is the position θ2 as well as y.
target value (command value) θ2o and target value (command value) f of force f
It is assumed that it is expressed as a linear sum with o.

yo-aθ20 +b f o ---
---(91 Equations (5) and (6), when t<Q, θl
-θ1-θ2-θz=1=0 and Laplace transform: (Jl sz+DB S+K) θ1(S)=k
T I fsl + to θ2 fsl --------(
10) (J2 S2+D2 S +K) θ2(S)-
θ+ (s) 10f' +51 −−−− (1
)) becomes. Here, if we set, the equation (10) and the equation (1) become θ+f
sl-G+ (s) (kT Ifsl+ to G2 (sl
) −・−(13)G2 (Sl=G2 fsl (k
θself sl + f' (sl) --------(1
4).

On the other hand, when formula (8) is Laplace transformed with t<Q and y=0, yts+=a・G2 (sl +b −f (sl
--------(15).

Therefore, if equations (13) 2 to (15) are shown in a block diagram, the result will be as shown in FIG. 4(A). That is, the controlled object 6 is
Figure 4 (
It can be modeled using the block diagram in A).

If the block diagram on the Laplace transform in Fig. 4(A) is shown as a block diagram on the time axis, it becomes as shown in Fig. 4(B),
Therefore, the controlled object 6 includes multipliers M1-M12, arithmetic unit A
1 to A7, and integrators ITI to IT4, and the state quantities in the controlled object 6 (angular velocity ω1 of the motor 20, rotation angle
l, load angular velocity ω2, rotation angle θ2, force f, and output y) can be observed.

By the way, in order to make the output y follow the command value stably and at high speed, each state quantity (G1, G1, G2, G2) shown in FIG. 4 may be fed back.

However, such a feedhack amount needs to be a deviation from a steady solution, and in the present invention, since two command values are given, the steady solution is not necessarily fixed, and the equation of motion and external conditions are This is an ideal value.

Therefore, as it is, the observer cannot feed back the deviation because the steady-state solution is unknown.

In order to solve this problem, the observer 7 is made to reproduce the actual system in the deviation space as shown in FIG. 2, so that the deviation state quantity can be obtained.

In other words, it is possible to obtain the amount of deviation to be fed back even without knowing the steady-state solution.

In order to reproduce this observer 7 in the deviation space, the control current ie is input to the observer, the difference (9e-ye) between the deviation outputs of the actual system 6 and the observer 7 is taken by the synthesis means 8, and it is applied to the observer 7. By feed-hacking, the observer 7 can always reproduce the actual system in the deviation space.

At this time, it is assumed that the motor 20 is applied with a feedback current ie determined from the feed bank in each state, to which a current if for generating a steady force fs is added.

kTif=fs In the observer 7, the amount of deviation from the steady value 1e, θ1e,
ω2e and θ2e can be observed, and if feedback is performed using these state quantities, stable and high-speed operation is possible.

Specifically, the observer gain G1. ,
G2, G3, and G4 are multiplied by multipliers 90 to 93, and the difference is obtained by inputting the multipliers to each of the differentiators D1 to D4 of the observer 7, and the difference is obtained by operating in the deviation space.
The deviation state quantities, that is, the deviation angular velocities ω1e, ω2ez, the deviation rotation angle θle, are obtained from the integrators ITI to IT4 at the subsequent stages of 1 to D4.
s θ2e can be observed.

This observed deviation state quantity Q, e, ω2e, θIe−=
θ2e is multiplied by the feed bank gain Fl−F4 by the multipliers 94 to 97 of the second gain means 9b, and the adder 98a
The control current ie is obtained by adding the outputs of the multipliers 94 to 97 at ~98c.

The inverter 13 inverts the polarity of this signal, inputs it to the observer 7, and synthesizes the steady current is
The drive current l of the motor 20 is obtained by taking the difference between

At this time, the current if for generating a steady force is the rotation angle θ1 of the motor 20 and the load 2, as shown in equation (7),
The difference unit 14 calculates the difference between the measured force f multiplied by the difference in G2 and the estimated force fe in the deviation space of the observer 7, and the multiplier 1
It is obtained by multiplying 1/kT by 5.

In the above-mentioned Fig. 4, the external force f' is input to the observer, but since it can be regarded as f' e=Q in the deviation space, it is not considered in the observer 7 operating in the deviation space of Fig. 2. You don't have to.

In this way, since the observer 7 reproduces the operation of the controlled object 6 in the deviation space, even if the steady-state solution is unknown, it can feedbank the state deviation from the steady-state solution, and therefore can Variable control is possible, vibration does not occur even during high-speed operation, and smooth operation toward a steady solution is possible, realizing high-speed followability, reducing the influence of external vibration, and suppressing vibration. Achieves smooth operation.

fbl Description of another embodiment of the first invention FIG. 5 is a block diagram of another embodiment of the first invention, and FIG. 6 is an explanatory diagram of the observer in FIG. Components that are the same as those in FIG. 4 are indicated by the same symbols.

In the second embodiment, before applying modern control theory, a current if is caused to flow through the motor in accordance with the measured force f.

The controlled object 6 modified in this way is modeled as shown in FIG. 6 (A) and in the same manner as in FIG. 4 (B).

At this time, as the steady current is, it is only necessary to consider the accelerating current io for moving the position, and after the movement is completed, it is necessary to consider only the accelerating current io for moving the position.
=io=0. That is, it is assumed that 1o=o after the movement is completed.

The equations of motion in Figure 6 (A) are equations (5) and (6).
) in the same way as Eq.

J 1 θ ++D plate θ 1 + K (θ 1
−θ2)=kT(io+if)
−−−−−−(16) J2θ2+D2θ20K(θ2
-θ+)-f'-1~--(17).

Here, kTif=f=K(θ] −θ2 ) −−−−−
--(18), so the equation (16) is J1θl+D+θ+ = k Tio ---
−(19).

Therefore, the block diagram of FIG. 6(A) does not require the feed banks of θ2, θ1, and the feed bank of f, and can be simplified as shown in FIG. 6(B).

An observer 7 using this is shown in FIG.

In such other embodiments, the same operations as in the embodiment shown in FIG. 2 can be performed, and the same effects and effects can be achieved.

(C) Description of an embodiment of the second invention FIG. 7 is a configuration diagram of an embodiment of the second invention.

In the figure, the same components as those shown in FIG. 1(B) and FIG. 2 are indicated by the same symbols, and lla is a multiplier;
The integral amount of the deviation ye (output of the integrator 1) is multiplied by the feed hack gain F5 and outputted to the adding section 12.

In the embodiment shown in FIG. 7, the configuration of the embodiment of the first invention shown in FIG.
is added, the integral amount of the deviation ye is feedhacked, and added to the state quantity of the observer 7 (that is, the output of the second gain means 9b) to obtain the control current ie. Therefore, the operations of the observer 7 and the like are the same as those in FIG.

The purpose of feeding the integral amount of the deviation ye is to prevent a steady deviation from occurring even when there is friction in the motor 20 or when there is disturbance due to changes in the gravity of the arms 1a to 1e.

This allows stable control even when a motor with particularly high friction is used, and also allows stable control even when gravity changes depending on the position of the arm such as the multi-jointed robot robot 1-.

(d) Description of another embodiment of the second invention FIG. 8 is a block diagram of another embodiment of the second invention.
Components that are the same as those shown in FIG. 1(B), FIG. 5, and FIG. 7 are indicated by the same symbols.

In another embodiment of FIG. 8, the integral amount feedback of the deviation ye of the embodiment of the second invention of FIG. 7 is added to the configuration of the other embodiment of the first invention of FIG. 5. This embodiment has the same operation, effect, and effect as the embodiment shown in FIG.

(Detailed Description of the Present Invention FIG. 9 is a block diagram of an application example of the present invention, and shows an example in which the present invention is applied to the control device 5 of the articulated robot robot 1- in FIG. 1).

In the figure, the same parts as those explained in Fig. 1), Fig. 1, and Fig. 5 are indicated by the same symbols, and 2 is an actuator, which is a position detector consisting of a motor 20 and a non-contact rotary encoder. 22.21 are integrated,
The position detector 21 is directly connected to the shaft of the motor 20, and
The position detector 22 is directly connected to the output shaft 2a to which the transmission force of the motor 20 is applied via a torsion bar (flexible member) 24, and detects the load, that is, the robot's rotation angle θ1. This is for detecting the rotation angle θ2 of the arms 1a to 1e. In addition, the output shaft 2a and the load,
That is, a speed reducer may be provided between the arms 1a to 1e of the robot.

30 is a first counter of the signal processing circuit 3, which counts position pulses that are the output of the position detector 21;
is a second counter of the signal processing circuit 3, which counts position pulses that are the output of the position detector 22; 32 is a difference circuit; Taking the difference from the counting position, actuator 2
These components constitute the signal processing circuit 3. 40, 41, 42, and 43 are the output sofas of the computer 4, respectively, and the command forces fo,
Command position θ20. weights, for the output of a, 44
is a processor of the computer 4, which sets a command force fo, a command position θ2o, and a weight a to each output sofa 40 to 43 at the time of mode conversion, and operates according to a predetermined program.

First, the operation of the actuator 2 will be explained.Although the actuator 2 is only shown for one axis in the figure, it is provided corresponding to each arm [a] to [f], and the rotational force of the motor 20 is applied to the torsion bar 24. The signal is output from the output shaft 2a via the output shaft 2a to drive the arm.

The rotational position of the motor 20 is detected by a position detector 2I, and the rotational position of the output shaft 2a is detected by a position detector 22. Therefore, the position where the position of the output shaft 2a is detected is
If the position of the motor 20 is controlled by the output of the output device 22, accurate positioning can be achieved. On the other hand, the force (torque) is determined by the output difference between the two position detectors 21 and 22 and the rigidity of the torsion bar 24.

Therefore, when some force is applied to the output shaft 2a, the torsion bar 24 bends due to its flexibility.
Since a phase difference occurs between the outputs of the rain detectors 21 and 22, it is possible to detect the external force and its magnitude.

That is, the force can be detected by the difference in the count values of the counters 30.31 that count the position pulses of the rain detectors 21.22.

By using actuator 2 like this, it is possible to realize a compact force and position detector integrated with the motor, and rotary encoders are capable of highly accurate position detection, are resistant to noise, and are highly reliable. . As the flexible member, a spiral spring may be used in addition to the torsion bar.

First, the processor 44 of the computer 4 runs on the output sofa 40.
At 41.42.43, command force fo, command position θ2o, weight S, and a are sent. As a result, the control device 5 obtains the current position θ2 and the detection force f from the signal processing circuit 3, obtains the actual deviation ye from the difference circuit 50.52, the multiplication circuit 51.53, and the summation circuit 54, and obtains the estimation of the observer 7. The difference from the deviation ye is taken by the synthesis means 8 and fed back to the observer 7 via the first gain means 9a, and on the other hand, the control current ie is calculated from the state quantity of the observer 7 by the second gain means 9b.
is generated and inputted to the observer 7, and the difference between the steady current is (the sum of the accelerating current 10 and the current if that generates a steady force) is obtained by the synthesizing means 10, and the drive current i is applied to the motor 20. to drive the motor 20.

In an articulated robot, the motor 20 is required for each axis, and the control device 5 shown in FIG. 9 is provided for each axis.

If the weight a≠0, b≠0, the arms 1b to 1e of the robot 1 are moved to the specified position with the specified force via the motor 20.
control.

In this way, the control amount is determined by the position error Δθ as shown in equation (1).
and error Δf are given weights a and b, respectively. Therefore, when only position control is performed, by setting weights a≠0 and b=Q, equation (]) becomes y =a・Δθ −−−−−−(2
0), only position control can be performed.

Similarly, to perform only force control, weight a=O1b≠0
By doing this, equation (1) becomes y=b・Δf
Since -・-・-(21), only force control can be performed.

Furthermore, if a≠0 and b≠0, Equation (1) remains the same, so position-force combined control becomes possible, and the control mode can be easily changed by changing the weights a and b. I can do it.

Similarly, by changing the values of weights a and b, more detailed control modes can be provided. For example, in position-force combined control, when placing emphasis on position accuracy, weight a When placing emphasis on force control by setting the weight a to be large and the weight b to be small, fine control can be easily achieved by setting the weight a to be small and the weight b to be large. This is executed by changing the weights a and b as appropriate while the computer 4 grasps the operating state of the robot based on the current position θ2 and the detection force f.

Since changing the weights a and b changes the controlled object, the feed bank gain F and the observer gain G are changed according to the weights. In the above example, the first
The structure of the embodiment of the first invention and the structure of the second invention (7th invention) are shown to which other embodiments of the invention are applied.
8) may be applied, and in order to apply the configuration of the second invention, the feed bank gain F5 is also changed when changing the weights a and b.

+fl Explanation of another embodiment FIG. 10 is a block diagram of another embodiment of the present invention, in which the same components as those shown in FIG. ' is a detection signal processing circuit, which includes a counter (9th
31 in the figure) and an A-D (analog-to-digital) converter that converts the detected current i into a digital value i, 5' is a control device, and the drive voltage of the drive source (motor) of each actuator V, command position xo (θ2
0), command force fo (io), first and second weights a
, b, current position X (θ2), and detection force f (+) so that y in the following equation becomes zero.

y=a・Δθ+bΔt −−−−(22
) However, Δθ−θ2o−θ2 −1−−−−−(2
3) Δi= (for)/kT -----
(24) Note that kT is the torque constant of the motor 20.

A current detector 26 detects the current value i of the motor 20 and inputs it to the signal processing circuit 3'.

The control device 5' has the same configuration as the control device 5 of FIG. 9, and the difference circuit 50 is configured to take the difference between the detected current i of the signal processing circuit 3' and the command current io.

Therefore, the sum circuit 54 obtains the deviation ye of equation (22). Further, the actuator includes a position detector 22, a motor 20, and a speed reducer 23.

In this embodiment, a proportional relationship is established between the current i flowing through the motor 20 and the output torque T, and if the proportionality constant (torque constant) is kT, then T = k T −i −−−−
-(25).

Therefore, by detecting the current i flowing through the motor 20, the output torque T, that is, the force, of the motor can be detected, and the same control as in the above embodiment is possible.
, b can similarly change the control mode.

In addition, in this other embodiment, these position-force controls can be performed using the actuator configuration used in conventional position control systems, and the configuration is inexpensive and convenient, and conventional robots can be easily upgraded. Can be made functional.

Further, in each of the above embodiments, control of position and force was explained as an example, but control of speed and force may also be used. In this case, angular velocity ω0 may be given instead of the position command.

Similarly, although the observer 7 has been described as a hardware circuit, it may also be implemented by executing software using a microprocessor or the like.Furthermore, the control device 5.5' may be configured with a microprocessor, and the functions of the control device 5.5' may be implemented by software. It can also be done by execution.

Furthermore, the target is not limited to articulated robots, but may be other moving bodies such as Cartesian coordinate robots.

Further, in the above embodiment, an encoder type detector is used as the detector, but other force sensors such as a strain gauge may also be used.

Although the present invention has been described above using examples, the present invention can be modified in various ways according to the gist of the present invention, and these are not excluded from the present invention.

〔Effect of the invention〕

As explained above, according to the first aspect of the present invention, since the observer is made to operate in the deviation space of the controlled object and is controlled by the state quantity of the observer, firstly, the movement-force control system It is possible to obtain high-speed followability at
This has the effect of minimizing the influence of external vibrations (external vibrations) and enabling high-speed operation.Secondly, it suppresses imaging at resonance points and extends mechanical life.Thirdly, it has smooth It also has the effect of being able to realize a wide range of motions regardless of external conditions, and is particularly suitable for application to multi-joint types where load fluctuations are severe.

Further, according to the second invention, in addition to the effects of the first invention, there is also the effect that the operation can be performed without producing steady-state deviations in response to disturbances such as friction and gravity, and is even more effective against disturbances and external conditions. It enables smooth operation even in response to changes.

[Brief explanation of drawings]

Fig. 1 is a diagram explaining the principle of the present invention, Fig. 2 is a configuration diagram of an embodiment of the first invention, Fig. 3 is an analytical model diagram of the controlled object in the configuration shown in Fig. 2, and Fig. 4 is the configuration shown in Fig. 2. FIG. 5 is a diagram of the configuration of another embodiment of the first invention, and FIG.
An explanatory diagram of an observer in the diagram configuration, FIG. 7 is a configuration diagram of an embodiment of the second invention, FIG. 8 is a configuration diagram of another embodiment of the second invention, and FIG. 9 is a configuration diagram of an application example of the present invention. FIG. 10 is a block diagram of another embodiment of the present invention, FIG. 1) is an explanatory diagram of the previously proposed control method, and FIG. 12 is an explanatory diagram of the operation of the already proposed control method. In the figure, 1-robot, 1a-hand, 1b to 1e-arm, 2-actuator, 20-motor, 21.22-position detector, 4-computer, 5.5'-control device, 26-current detector , 6-controlled object, 6a-deviation means, 7-state observation means (observer), 8-synthesis means, 1)-=integration means.

Claims (2)

[Claims] (1) A controlled object including a driving means for driving a moving body, a deviation means for obtaining a deviation between a movement command, a force command, and the state quantity of the driving means, and a model of a deviation space of the controlled object. a state observing means, feeding back a composite output of the deviation of the deviation means and the deviation of the state observing means to the state observing means, and controlling the driving means based on the state quantity of the state observing means. A mobile object control system featuring: (2) A controlled object including a driving means for driving a moving body, a deviation means for obtaining a deviation between a movement command, a force command, and a state quantity of the driving means, and a model of a deviation space of the controlled object. a state observation means, a combined output of the deviation of the deviation means and the deviation of the state observation means is fed back to the state observation means, and an integrated amount of the state quantity of the state observation means and the deviation of the deviation means; A moving body control method characterized in that the driving means is controlled based on a combination of the following.