JPH0685137B2 - Position / force control method - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to a position / force control method for controlling a hand driving actuator of a robot or the like based on position and force information, and particularly, a control mode is easily and stably changed. Then, the present invention relates to a method for controlling the position / force of an actuator such as a robot having flexibility for work.

In recent years, robots have been developed in order to automate the work performed by humans, and further efforts have been made to develop a technique for controlling the robots to have a function equivalent to that of human hands and arms.

If the robot can be made to perform the same function as a human hand or arm, it will be possible to perform such advanced work and it will be extremely convenient.

Generally, the robot is basically controlled by controlling the position of an actuator (arm), and the work hand at the tip of the arm is positioned at a command position to perform work.

Since such position control alone lacks flexibility,
It is inadequate to perform the same function as a human arm or hand, and it is not adaptable to work affected by other objects. Therefore, it is necessary to control the actuator by detecting the force exerted by the actuator (or the reaction force from the outside).

[Conventional technology]

Conventionally, as a force control device, a position control actuator has a certain rigidity, a virtual target value is set in advance, and a certain deviation is provided 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,
The detected force is converted into a deviation and subtracted from the virtual target value.

[Problems to be solved by the invention]

On the other hand, since human arms and hands have necessary force control functions and tactile functions in addition to the positioning function, it is necessary for the robot to have these functions so that these functions can be activated in a timely manner according to the work. . For example, in order to fit two members, one member in a fixed position is positioned with the other member by a robot, and the members are accurately aligned by a tactile function, and then they are fitted with a predetermined pressing force.

To perform such work, it is necessary to change the control mode of the actuator during positioning, during positioning, and during fitting.

However, in the conventional method, since the force is given by the product of the rigidity K and the positional deviation, if the rigidity is large, even a slight deviation causes a large force, and the rigidity cannot be increased so much. On the other hand, if the rigidity is low, vibration is likely to occur and driving at high speed is impossible. In addition, it is weak against external disturbances such as gravity, and the positioning accuracy deteriorates. Therefore, with the conventional method, the handling of the combined position-force control is complicated, and fine position / force control cannot be performed.

Therefore, in the conventional position / force control method, although both position control and force control can be performed, there is a problem that changing the control mode is difficult and complicated.

[Means for solving problems]

The present invention provides a control method for a robot that can easily and stably control a stable control mode based on position and force information.

Therefore, in the present invention, the actuator, the detection means for detecting the force and the position of the actuator, the difference between the command force and the detection force of the detection means with the first weight, and the command position. A control means for driving and controlling the actuator by a sum of a difference between the detection position of the detection means and a second weight, and a control means for controlling the sum to zero. Is the command force to be the detection force of the detection means when changing either the first or the second weight,
It is characterized in that the command position is replaced with the detection position of the detection means.

[Action]

In the present invention, the force applied to the actuator is detected by the detection means, and the force control can be performed by obtaining the difference between the command force and the difference between the current position and the command position from the detection means. The position control is possible, and the sum of these given weights is used as the basic control amount.
Therefore, it is possible to change the control mode of position / force control only by changing the weight, and when changing the weight,
The command force is replaced with the current detected force, and the command position is replaced with the current position to prevent vibration of the actuator when changing the mode (weight), thereby enabling smooth mode conversion.

〔Example〕

 Hereinafter, the present invention will be described in detail with reference to Examples.

FIG. 2 is a block diagram of an embodiment of the present invention, in which 1 is a robot, which has a hand 1a at the tip, arms 1b, 1c, 1d, 1e for operating the hand 1a, and a base 1f. And the arms 1b, 1c, 1d, 1e are rotated by the actuator,
What moves the hand 1a, 21 and 22 are position detectors of each actuator, which constitute a part of the force and position detectors,
What will be described later, 3 is a signal processing circuit, and the position detector 22
Which outputs a detection force f which is the difference between the detection position p of the robot and the detection output of the two detectors 21 and 22, and 4 is a computer (first control means), which detects the robot position from the detection position p and the detection force f. A device for monitoring the operating state and outputting the target command position p ₀ , the target command force f ₀ and the first and second weights a and b.
Is a control device (second control means), which controls the drive current i of the drive source (motor) of each actuator to the command position p ₀ , command force f ₀ , first and second weights a and b, and the current position. It is controlled by p and the detection force f so that y in the following equation becomes zero, which will be described later with reference to FIG.

y = a · Δp + b · Δf (1) However, Δp = p ₀ −p (2) Δf = f ₀ −f (3) FIG. 1 is a detailed view of the main part of the configuration of the embodiment shown in FIG. ,
The same parts as those shown in FIG. 2 are shown by the same symbols, 2 is an actuator, and the motor 20 and the position detectors 22 and 21 are integrally formed. To do. Reference numeral 30 is a first counter of the signal processing circuit 3, which counts the position pulse output from the position detector 21, 31 is a second counter of the signal processing circuit 3,
For counting the position pulse output from the position detector 22,
Reference numeral 32 is a difference circuit, which is used for counting the counting position of the first counter 30 and the second position.
The force f applied to the actuator 2 is detected by taking the difference from the counting position of the counter 31 of the above, and these constitute the signal processing circuit 3. Reference numerals 40, 41, 42 and 43 denote output buffers of the computer 4, respectively, command force f ₀ and command position.
For output of p ₀ , weights b and a, 44 is a processor of the computer 4, and each output buffer 40 to 43 at the time of mode conversion.
The command force f ₀ , the command position p ₀ , and the weights b and a are set to the command force, which is operated by a predetermined program. Reference numeral 50 denotes a difference circuit, which obtains a difference Δf between the detected force f from the difference circuit 32 of the signal processing circuit 3 and the command force f ₀ from the output buffer 40 of the computer 4 (which executes the formula (3)). ,
Reference numeral 51 denotes a multiplication circuit, which multiplies the difference (error) Δf of the difference circuit 50 by the weight b from the output buffer 42 of the computer 4 to output b.
A device for emitting Δf, 52 is a difference circuit, and calculates the difference Δp between the coefficient position (current position) p of the second counter 31 of the signal processing circuit 3 and the command position p ₀ from the output buffer 41 of the computer 4. Numeral 53 (which executes the equation (2)), 53 is a multiplication circuit, which multiplies the difference (error) Δp of the difference circuit 52 and the weight a from the output buffer 43 of the computer 4 to obtain the output a · Δp. What is emitted, 54 is a sum circuit, which adds the outputs b · Δf and a · Δp of the two multiplication circuits 51 and 53 according to the equation (1),
A control circuit 55 outputs a control amount y, and controls a drive current i so that the control amount y becomes zero.

FIG. 3 is a detailed configuration diagram of an embodiment of the actuator in the configuration shown in FIG. 1. FIG. 3 (A) is a cross-sectional view thereof, and FIG. 3 (B) is a detailed view of essential parts for explaining the principle of operation. is there.
In the figure, 23 is a speed reducer, which is configured by a well-known gear type speed reducer, is provided to reduce the rotational speed of the motor 20 to increase torque (force) and improve positioning resolution, and 20a is A shaft of a motor, which is hollow inside and serves as a rotation axis of the motor 20, 21a is a slit disc, an optical slit is formed around the shaft, and the shaft 20a of the motor 20 is provided with the shaft 20a. Those which rotate by rotation, 21b and 21c are optical sensors, which are provided corresponding to the slit circumference of the slit disk 21a, and the light receiving element 21c receives the light of the light emitting element 21b through the slit of the slit disk 21a. The position detector 21 is configured by an optical non-contact rotary encoder. 22a is a slit disc, has the same configuration as the slit disc 21a, provided on the shaft 23a of the speed reducer 23, 22b, 22C is an optical sensor, the same configuration as the optical sensor 21b, 21c. Therefore, the position detector 22 is also composed of an optical non-contact rotary encoder. 24 is a torsion bar, which constitutes a flexible member, connects the shaft 23a of the speed reducer 23 and the shaft 20a of the motor 20, connects the shaft 20a, and is provided in the hollow portion of the shaft 20a. The rotational force is transmitted to the shaft 23a of the speed reducer 23.

First, the operation of the actuator having the configuration shown in FIG. 3 will be described. The rotational force of the motor 20 is transmitted from the shaft 20a to the shaft 23a of the speed reducer 23 via the torsion bar 24 and the output shaft 2a.
Will be output. The rotational position of the motor 20 is detected by the position detector 21, and the rotational position of the output shaft 2a is detected by the position detector 22. Therefore, if the position of the motor 20 is controlled by the output of the position detector 22 that detects the position of the output shaft 2a, accurate positioning can be performed. On the other hand, the force (torque) is detected by both position detectors.
Determined by the output difference between 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 bends due to the flexibility of the torsion bar 24,
Since a phase difference occurs between the outputs of both detectors 21 and 22, the external force and its magnitude can be detected by this. That is, the detection can be performed by the difference between the count values of the counters 30 and 31 of FIG. 1 that count the position pulses of both detectors 21 and 22.

By using such an actuator 2, it is possible to realize a small force and position detector integrated with a motor, and a rotary encoder can detect a position with high accuracy and is strong against noise and highly reliable. As the flexible member, a spiral spring may be used instead of the torsion bar.

Referring back to FIGS. 1 and 2, the operation will be described.

Although the figure shows one axis, the same applies to the other axes.

First, the processor 44 of the computer 4 outputs the output buffers 40, 41,
The command force f ₀ command position p ₀ and the weights b and a are set in 42 and 43. As a result, the control signal 5 obtains the current position p and the detection force f from the signal processing circuit 3, executes the equation (1) according to the configuration of FIG. 1, and outputs the drive current i to control the motor 20. Since the control circuit 55 of the control device 5 controls the drive current i so that the controlled variable y becomes zero, if the weights a ≠ 0 and b ≠ 0, based on the designated force and the designated position. ,motor
The arms 1b to 1e of the robot 1 are controlled via 20.

In this way, the control amount is the position error Δp as shown in the equation (1).
And the force error Δf are weighted with a and b, respectively. Therefore, when only position control is performed, by setting weights a ≠ 0 and b = 0, the equation (1) becomes Since y = a · Δp (4), only position control can be performed.

Similarly, to perform only force control, weights a = 0, b ≠ 0
By this, the equation (1) becomes y = b · Δf (5), so that only the force control can be performed.

Further, if a ≠ 0 and b ≠ 0, the expression (1) remains unchanged, so that the position-force combined control becomes possible, and the control mode can be easily changed by changing the weights a and b. You can

Similarly, by changing the values of the weights a and b themselves, a more detailed control mode can be given. For example, in the position-combined control, if the position accuracy is to be emphasized, When the weight is set to be large and the weight is set to be small and the force control is to be emphasized, fine control can be easily realized by setting the weight a to be small and the weight b to be large. This is executed by the computer 4 changing the weights a and b at appropriate times while grasping the operation state of the robot by the current position p and the detection force f.

On the other hand, the relationship between the position p and the force f when y = 0, which is the sum, is set as y = 0 in the equation (1), and Becomes

Therefore, as shown in FIG. 4 (A), the relationship between the position p and the force f in the state of y = 0 when a = a ₁ and b = b ₁ is Even if the target position p ₀ and the target command force f ₀ are given, the balance is adjusted so that y = 0 at the coordinates (p ₁ , f ₁ ) on the straight line α even if the target position p ₀ and the target command force f ₀ are given. May be kept and is not always balanced at coordinates (p ₀ , f ₀ ).

When the weights a and b are changed in a state where the commanded target position p ₀ and the target force f ₀ are not balanced, the condition when y = 0 is (p ₀ , f ₀ ) Exists on a straight line β having different inclinations.

Therefore, when the weight is changed, the optimum value moves from (p ₁ , f ₁ ) to (p ₂ , f ₂ ) on the straight line β, and the output fluctuates,
The actuator vibrates due to only the mode change due to the change in weight, causing chattering, and stable operation is not guaranteed.

Therefore, in the present invention, before changing the mode by changing the weight, the processor 44 of the computer 4 sets the value of the second counter 31,
That is, the current position p ₁ and the current force f ₁ of the difference circuit 32 are read,
The output buffers 41 and 40 are set.

As a result, the above equations (2) and (3) become Δp = p ₁ −p (8) Δf = f ₁ −f (9), and therefore the equation (1) is y = a · (P ₁ −p) + b (f ₁ −f) (10).

Therefore, as shown in FIG. 4 (B), even if the weights a and b are changed, the condition at the time of y = 0 from the equation (10) is centered on the current state (p ₁ , f ₁ ). It is on a different straight line β, there is no shift of the optimum value, and chattering can be prevented.

In this way, it is possible to change the control mode of position / force control simply by changing the weight, and it is possible to control so that chattering does not occur even if the weight is changed and the control mode is changed.

Next, another embodiment of the present invention will be described.

FIG. 5 is a block diagram of another embodiment of the present invention. In FIG.
The same parts as those shown in the figure are designated by the same reference numerals, and 3'is a detection signal processing circuit, which is a counter for calculating the output pulse of the position detector 22 of the motor 20 (31 in FIG. 1). , Etc.) and the detected current i are converted into digital values i
Having an (analog / digital) converter,
Reference numeral 5'denotes a control device, which controls the drive voltage v of the drive source (motor) of each actuator to the command position p ₀ (θ ₀ ) and the command force f _0.
(I ₀ ), the first and second weights a and b, and the current position p
(Y) and the detection force f (i) are used to control y so that y in the following equation becomes zero, which will be described later with reference to FIG.

y = aΔθ + bΔi (11) where Δθ = θ ₀ −θ (12) Δi = (f ₀ −f) / KT (13) where KT is the torque constant of the motor 20.

Reference numeral 6 denotes a current detector for detecting the current value i of the motor 20, which will be described later with reference to FIG.

FIG. 6 is a detailed configuration diagram of the control device 5'in the configuration of FIG. 5, in which the same components as those shown in FIG. 1 are designated by the same symbols, and 56 is a difference circuit, The difference Δi between the command force (current value) i ₀ from the computer 4 and the detected force (current) i is taken. Reference numeral 57 is a difference circuit, which is the difference Δθ between the command position θ ₀ from the computer 4 and the current position θ. Is taken. The multiplication circuits 51 and 53 are the weights b and a from the computer 4 and the difference circuit 56,
The outputs Δi and Δθ of 57 are multiplied according to the equations (12) and (13), and the sum circuit 54 outputs the controlled variable y according to the equation (11). The actuator 2 ′ is composed of a position detector 22, a motor 20, and a speed reducer 23.

FIG. 7 is an explanatory diagram of the current detector 6 in the configuration of FIG.

The electric model when the voltage v is applied to the motor 20 is as shown in FIG. 7A, Ro is the external resistance of the motor, and the motor 20 can be represented by the counter electromotive voltage v, the inductance L, and the resistance. Therefore, in order to detect the current i flowing through the motor 20, the voltage applied to the external resistor Ro may be detected as shown in FIG. 7 (B). That is, the input resistance Rs, the operational amplifier 60, and the feedback resistance Rf are used.

Here, if Rs >> Ro, the current flowing through the input resistor Rs is negligibly smaller than the current flowing through the external resistor Ro, so if the voltage across the external resistor Ro is appropriately amplified by the operational amplifier 60, It is possible to detect the current flowing from the output v ₁ to the external resistance Ro, and thus the current flowing to the motor 20.

Referring back to FIGS. 5 and 6, the operation will be described.

As in FIGS. 1 and 2, only one axis is shown in the figure, but the other axes are also the same.

First, the computer 4 sets the command current (force) i ₀ , the command position θ ₀ , and the weights b and a in the output buffer. The controller 5'obtains the current position .theta. And the detected current value i from the detection signal processing circuit 3 ', and executes the equation (11) according to the configuration of FIG.
A control device 5 ', which generates a drive voltage v and controls the motor 20.
The control circuit 55 controls the drive voltage v so that the controlled variable y becomes zero.

Here, a proportional relationship is established between the current i flowing through the (DC) motor 20 and the output torque T, and when the proportional constant (torque constant) is kT, T = kTi (14).

Therefore, if the current i flowing through the motor 20 is detected, the output torque T of the motor, that is, the force can be detected, and the same control as that of the embodiment shown in FIGS. 1 and 2 can be performed. The control mode can be similarly changed by changing the weights a and b.

That is, when a ≠ 0, b = 0, position control a = 0, b ≠ 0, force control a ≠ 0, b ≠ 0, a combined mode (stiffness mode) of both can be realized.

Also in this other embodiment, the position command and the force command are replaced with the current position and the current force before changing the weight, as in the above-described embodiments, and mode conversion without chattering is possible.

Further, in the other embodiment, these position-force control can be performed with the structure of the actuator used in the conventional position control system as it is, the structure is inexpensive and compact, and the existing robot can be easily and highly functionalized. it can.

In actual work, by changing the weight, position control is performed when carrying a member (positioning), position-force combined control is performed at the time of work such as engagement and fitting, and both position-force control is performed. The ratio of can be changed according to the work content, and chattering at the time of change can be prevented.

In the above-described embodiment, the encoder type is used as the detector, but other force sensors such as a strain gauge may be used.

Although the present invention has been described with reference to the embodiments, the present invention can be variously modified according to the gist of the present invention, and these modifications are not excluded from the present invention.

〔The invention's effect〕

As described above, according to the present invention, the actuator, the detecting means for detecting the force and the position of the actuator, and the difference between the command force and the detecting force of the detecting means are given the first weight. Control means for driving and controlling the actuator by the sum of the difference between the command position and the detection position of the detection means with a second weight, and controls so that the sum becomes zero. With the first or second control means.
When any one of the weights is changed, the command force is replaced with the detection force of the detection means, and the command position is replaced with the detection position of the detection means. The control mode of the force control system can be changed, the control mode can be easily changed, the work contents of the robot and the like can be enhanced, and chattering that is likely to occur when the control mode is changed by changing the weight can be prevented. Even if the control mode is changed, a smooth transition is possible, and the effect of stable operation is also obtained. Further, it has a practically excellent effect that it is easy to realize.

[Brief description of drawings]

FIG. 1 is a schematic view of an essential part of an embodiment of the present invention, FIG. 2 is a general schematic drawing of an embodiment of the present invention, FIG. 3 is a schematic drawing of an embodiment of an actuator having the structure shown in FIG. 1, and FIG. FIG. 5 is an operational characteristic diagram of the present invention, FIG. 5 is a configuration diagram of another embodiment of the present invention, and FIG.
FIG. 7 is an explanatory diagram of a main part of the configuration shown in FIG. 7, and FIG. 7 is an explanatory diagram of the current detector having the configuration shown in FIG. In the figure, 1 ... Robot, 1a ... Hand, 1b-1e ... Arm, 2 ... Actuator, 20 ... Motor, 21, 22 ...
Position detector, 4 ... Calculator, 5, 5 '... Control device, 6
...... Current detector.

Claims (1)

[Claims] 1. An actuator, a detection means for detecting the force and position of the actuator, a difference between the command force and the detection force of the detection means with a first weight, the command position and the detection. A control means for driving and controlling the actuator according to a sum of a difference between the detected position of the means and a second weight, and the control means for controlling the sum to zero. A position / force characterized in that the command force is replaced with the detection force of the detection means and the command position is replaced with the detection position of the detection means when changing either the first or the second weight. control method.