JPH0378643B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a bilateral servo device, and particularly to a bilateral servo device such as a manipulator in which a non-negligible frictional resistance exists in the drive system.

[Conventional technology]

Manipulators are used to mechanically perform operations similar to those performed by humans in inaccessible or dangerous locations. For such applications, an articulated manipulator is generally used, and one of its manual operation methods is a master-slave method. This type of manipulator is
It consists of a master manipulator that is actually operated by the operator, and a slave manipulator that follows this master manipulator and operates in a similar manner to perform the actual work.The mechanisms on the master side and slave side have the same structure. be.

This method is said to be easier to operate than a method in which each joint of the manipulator is driven individually using a switch or joy stick. As a means to further improve the operability of this system, there is a method of transmitting the force applied to the slave manipulator by the load to the operator in the form of a reaction force via the master manipulator. According to this means, the feeling of force against the load can be transmitted to the operator, and the operation feeling is good. Moreover, it is extremely effective in complex work that requires controlling the amount of force. A servo mechanism with this reaction force transmission function is called a bilateral servo mechanism.

Here, with reference to FIG. 1, the basic structure of a symmetrical bilateral servo device, which has a simple mechanism among bilateral servo devices, will be explained.

The position signals of the master side arm shaft 1 to which the master side arm 10 is connected and the slave side arm shaft 2 to which the slave side arm 11 is connected are respectively taken out by detectors 3 and 4, and the servo deviation is detected by the subtractor 5. Can be changed to ε. The servo deviation ε is applied to the respective electric motors 8 and 9 via amplifiers 6 and 7, and on the slave side, a torque is generated that causes the slave side arm 11 to follow the master side arm 10, similar to a normal position servo, and the master side In this case, a torque proportional to the servo deviation ε is generated.
When a load torque is applied to this system, the servo deviation ε increases in order to maintain this load, and a large reaction force is also applied to the master side.

[Problem to be solved by the invention]

Here, if there is friction in the drive mechanism of the slave side arm 11, the drive torque of the slave side arm 11 will be required to overcome this friction torque. Therefore, the reaction force applied to the master side also increases by an amount corresponding to the friction torque. Also,
If there is friction in the drive mechanism of the master side arm 10,
In addition to the reaction force mentioned above, the operating force of the master side arm 10 is
A force is required to overcome the frictional force on the master side. Several kg
In actual manipulators that handle loads exceeding
It even reaches Kgf. Therefore, even when the slave manipulator is under no load, the operator's operating force is
This has the disadvantage that it is necessary to overcome the frictional force, which significantly impairs operability.

As a device for reducing the influence of such frictional force on the operating force, there is a bilateral servo device shown in FIG. 2. This device is the device shown in FIG. 1 with the configuration shown by the dotted line added.

Force detector 12 mounted on master side arm 10
Converts the operator's operating force into an electrical signal S,
This electric signal is guided to a differentiator 13 to obtain a differentiated signal S〓. Further, the differential signal S〓 is input to the Schmitt circuit 14, and a positive or negative constant voltage Vs is generated depending on the sign of the differential signal S〓. And on the master side,
A voltage Vs is applied to the motor 8 via the compensation amplifier 15 so that a compensation torque is generated against the frictional force in the direction in which the master side arm 10 is driven by an increase in the operating force. At the same time, on the slave side, a compensation torque is generated in the direction in which the slave side arm 11 is driven as the master side arm 10 is driven due to an increase in the operating force, so that a compensation torque is generated against the frictional force through the compensation amplifier 16. Apply voltage Vs to motor 9.

An example of this type of prior art is
-73365 etc.

JP-A-50-73365 discloses a method in which a comparator is provided on the force feedback path from the slave to the master, and when the amount of force feedback from the slave is less than the frictional force, the amount of force feedback is set to zero.

According to this method, when the slave is unloaded,
Master operation force can be reduced.

However, it is necessary to maintain high absolute accuracy of the force sensor. In addition, there was a problem that high accuracy could not be maintained due to temperature drift and hysteresis of the sensor. Furthermore, it was necessary to install a high-speed calculation device that calculates the dead weight torque in real time.

A method has also been proposed in which a master and slave detector are provided, a memory that rewrites the stored torque deviation only when the torque deviation is within a set range, and a bias circuit that adds the output of the memory to the slave drive system.

According to this method, when the slave is stationary, the slave torque is equal to the set range value.
It is said that when the slave drive system is controlled and the gripper grips an object, the gripping torque can be controlled to a value within a set range and that excessive torque can be prevented from being applied.

However, this method also has
I had the same problem as issue 73365.

In particular, in the conventional method, including the specific example above,
If an operation was made to suddenly weaken the operating force while the master side arm was being driven, the sign of the differential signal would be reversed, and the compensation torque would act in the opposite direction to the driving direction, that is, in the same direction as the frictional force. As a result, there was a problem in that the reaction force increased and the operability was impaired.

SUMMARY OF THE INVENTION An object of the present invention is to provide a bilateral servo device that improves operability by ensuring that compensation torque remains in a constant direction regardless of the operating force or even if the operating force suddenly decreases.

[Means to solve the problem]

In order to achieve the above object, the present invention provides a master operating mechanism operated by an operator, a slave operating mechanism that executes work, a position detector that detects the position of the drive shaft of each operating mechanism, and a position detector that detects the position of the drive shaft of each operating mechanism. means to drive the electric motor of the slave operating mechanism to follow the master operating mechanism based on the positional deviation of A differentiator that outputs a differentiator, and a compensation torque that counteracts the frictional force when driving each operating mechanism is calculated according to the magnitude and polarity of the output signal of the differentiator, and a command is given to each of the electric motors connected to each of the operating mechanisms. a speed detector for detecting the rotational speed of a drive shaft of a master operating mechanism; and a comparator for determining whether the detected rotational speed is within a predetermined range. , a switch that cuts off the output from the differentiator to the compensation torque generating means when the detected rotational speed falls outside the predetermined range;
The present invention proposes a bilateral servo device that includes means for maintaining the direction of compensation torque at that point in time.

The present invention also provides a speed detector that detects the rotation speed of the drive shaft of the master operating mechanism, a comparator that determines whether the detected rotation speed is within a predetermined range, and a speed detector that detects the rotation speed of the drive shaft of the master operating mechanism. means for cutting off the output from the differentiator to the compensation torque generating means when it is outside the predetermined range; and means for obtaining the absolute value of the rotational speed;
A bilateral servo device is proposed that includes a multiplier that multiplies the absolute value of the rotational speed by the output from the comparator and outputs the result to compensation torque calculation means as a correction value according to the rotational speed.

The present invention further includes a speed detector for detecting the rotational speed of the drive shaft of the master operating mechanism, a comparator for determining whether the detected rotational speed is within a predetermined range, and a comparator for determining whether the detected rotational speed is within a predetermined range. means for cutting off the output from the differentiator to the compensation torque generating means when the difference is within the predetermined range; means for obtaining the absolute value of the rotational position deviation; and multiplying the absolute value of the rotational position deviation by the output from the comparator. The present invention proposes a bilateral servo device including a multiplier that outputs a correction value according to the rotational position deviation to compensation torque calculation means.

[Effect]

In the present invention, since the direction of the friction force compensation torque is not changed when the detected rotational speed is equal to or higher than a predetermined value, even if the operating force of the master side arm is suddenly weakened, the friction force compensation torque The direction does not change frequently, improving operability.

Additionally, if the frictional force is proportional to speed or load torque, a correction term for speed or load torque can be added to more accurately compensate for varying friction.

〔Example〕

FIG. 3 is a block diagram showing an embodiment of the bilateral servo device according to the present invention. Here, the same members as in FIG. 2 are given the same reference numerals. In this embodiment, a speed detector 1 is added to the device shown in FIG.
7. A window comparator 18 and an analog switch 19 are added. That is, a speed detector 17 is provided connected to the motor 8 that drives the master side arm shaft 1, and only when the detected speed signal x ₁ exceeds a certain value, the window comparator 18 sends an output signal to the analog switch 19. Differentiator 13
The output is outputted to the Schmitt circuit 14. Third
The operation of the illustrated embodiment will now be described. However, since the basic operations and compensation principles described above overlap,
The explanation will be omitted.

The force detector 12 is mounted on the input shaft arm in order to detect the operating force applied to the master side arm 10 by the operator. The force detector 12 includes, for example, a strain gauge, a bridge circuit that detects a change in resistance value depending on the amount of strain, a preamplifier, and the like. The output signal S of the force detector 12 is guided to the differentiator 13 and becomes a differentiated signal S〓, which is sent to the analog switch 1.
9 is input. On the other hand, the speed detector 17 detects the rotational speed of the master side arm shaft 1 using a tachometer generator whose input shaft is interlocked with the master side arm shaft 1. The speed signal _x〓1 is input to the window comparator 18. Window comparator 1
8 has the input/output characteristics shown in FIG. 4, and its output signal V _G is input to the gate input terminal of analog switch 19. The analog switch 19 is turned on when the gate signal is 0, and turned off when the gate signal exceeds a certain threshold value V _G1 . The output signal _S〓1 of the analog switch 19 is guided to the Schmitt circuit 14. The Schmitt circuit 14 is
It has the characteristics shown in FIG. On the master side, the output signal Vs is passed through a compensation amplifier 15 to a subtracter 2.
given to 0. A subtracter 20 subtracts the signal Vs from the output signal of the amplifier 6 and provides the result to the motor 8. On the other hand, on the slave side, the output signal
Vs is provided to the adder 21 via the compensation amplifier 16. The adder 21 adds the output signal of the amplifier 7 and the signal Vs, and sends the result to the electric motor 9.
give to

a, b, c, d, e, f, g, h, i in Figure 6
These are operation waveforms of each part of the embodiment in FIG.

With reference to FIG. 6, the operation of the embodiment shown in FIG. 3 will be explained as time passes.

It is assumed that the master side arm 10 is in a stationary state at time t< _t0 , and at time _t0 , the operator applies force to the master side arm 10 in order to drive the load on the slave side arm 11.

As a result of the sudden force being applied to the arm 10, the force signal S applied to the master side arm 10 shows a sudden change in output, as shown in FIG. 6a.

The differential signal a〓 increases with time, as shown in FIG. 6b. At this time, as shown in Fig. 6c, since the speed x〓 ₁ of the master side arm axis is zero,
As shown in FIG. 6d, the window comparator 1
The output of 8 is V _G =0. Therefore, the analog switch 19 is in the active or on state, and its output S〓 ₁ (same as the signal S〓) is applied to the Schmitt circuit 14 . At time _t1 , when the analog switch output _S〓1 reaches _S〓10 , the output Vs of the Schmitt circuit 14 changes from _-Vs1 to + _Vs1 . Since the output Vs of this Schmitt circuit 14 is given to the motor 8 via the compensation amplifier 15 and the subtractor 20, the torque M ₁ of the motor 8 is
As shown in figure h, it changes from −M ₁₀ to +M ₁₀ .
At the same time, the torque _M2 of the electric motor 9 also changes from _-M20 to + _M20 , as shown in FIG. 6i.

When the operator further increases the operating force, the time
At t ₂ , S+M ₁₀ ≧F ₁ (F ₁ is the friction torque on the master side), and the master side arm shaft 1 starts to move. At this time, the slave side arm shaft 2, as shown in Fig. 6i, M ₂₀ < M _L + F ₂ (M _L is the load torque, F ₂ is the slave friction torque)
Don't move because of it. Therefore, as the master arm shaft 1 rotates, a positional deviation ε occurs between the master arm shaft 1 and the slave arm shaft 2, as shown in FIG. 6g. This deviation ε
are transmitted to the electric motors 8 and 9 via the subtracter 20 and the adder 21, respectively. As a result, as shown in FIG. 6h and i, the electric motor 8
The torque M ₁ of the motor 9 decreases, and conversely the torque M ₂ of the electric motor 9 decreases.
increases. A decrease in torque M ₁ in this case will be felt by the operator as an increase in reaction force.

If the master-side arm shaft continues to be driven even after time _t3 (the master-side operating torque S is further increased), the torque _M2 continues to increase along with the deviation ε. At time _t4 , as shown in FIG. 6i, when _M2 ≧ _ML + _F2 , the slave arm shaft 2 begins to move.
At this time, the reaction force of the master side arm shaft 1 is given by M _L + (F ₁ - M ₁₀ ) + (F ₂ - M ₂₀ ).

Torque generated in motors 8 and 9 by voltage Vs ₁ generated in steps by Schmitt circuit 14
If each circuit constant is selected so that M ₁₀ and M ₂₀ are as close to the friction torques F ₁ and F ₂ of each shaft as possible, the reaction force felt by the operator will be approximately equal to the load torque M _L applied to the output shaft. .

When the operator operates the master side arm 10 so that the slave side arm shaft 2 reaches a desired speed, an overshoot occurs in the operating torque S after time _t4 , as shown in FIG. 6a. Therefore, as shown in FIG. 6b, the sign of the differential signal S〓 of the force detector 12 is reversed, and as shown by the dashed line in FIG. 6e, at time _t5 , S〓 ₁ <-S ₁₀ , the output signal of the Schmitt circuit 14 also becomes -Vs ₁ , as shown by the dashed line in FIG. 6F, and its sign is inverted. As a result, the motor torque M ₁ decreases by 2M ₁₀ , as shown by the dashed line in FIG. 6h. Since the operator feels this decrease in torque as a stepwise increase in reaction force, it is difficult to continuously drive the master side arm 10 smoothly. Furthermore, in order to subsequently drive the master arm 10, a large operating force is required, as shown by the dashed line in FIG. 6a.

In the present invention, the speed detector 17, window comparator 18, and analog switch 19 shown in FIG.
When the rotational speed x〓 ₁ of the master side arm shaft satisfies the condition x〓, ≧x〓 _1t at time _t3 , as shown in Fig. 6c, then as shown in Fig. 6d, , changes the output signal Vs of the window comparator 18 from zero to _Vs1 . As a result, the analog switch 19 is turned off, and the input signal _S〓1 of the Schmitt circuit 14 becomes zero, as shown in FIG. 6e. Therefore, the sign of the output signal of the Schmitt circuit 14 is determined by the driving speed of the master side arm 10 as shown in _FIG.
It does not change until it becomes less than Therefore, the threshold value x〓 _1t of the window comparator 18 is set to a value corresponding to the lowest speed at which the operator normally operates, and the threshold value S〓 ₁₀ of the Schmitt circuit 14 is set to the value at which the operator normally starts the master side arm 10. Set to the value corresponding to the minimum force change rate at that time.

In this way, only when the operator applies force to start the master side arm 10,
The sign of the output signal of the Schmitt circuit 14 is switched, and a compensating torque is generated in the starting direction to counteract the frictional resistance. According to the data obtained by the inventor, the value of the minimum speed and minimum force change rate is 5 to 10 mm/sec when the peripheral speed of the operator's hand grasping the arm is 5 to 10 mm/sec.
It has been confirmed that if the minimum force change rate is about 0.4 to 0.8 Kgf/sec, the direction of the compensation torque can be switched smoothly enough, and moreover, it can always be applied in the opposite direction of the frictional force.

According to the embodiment in FIG. 3, it is possible to reduce the operating force against the frictional force on the master side when operating the master side arm. Furthermore, since the frictional force on the slave side is not fed back to the master side, force transmission from the slave side to the master side becomes accurate, and in addition, it becomes possible to widen the range of force that can be transmitted.

FIG. 7 is a block diagram showing another embodiment of the present invention. In FIG. 7, the same members as in FIG. 3 are given the same reference numerals.

This embodiment has a voltage regulator 22, an absolute value circuit 23, a multiplier 24, and an adder 25 in addition to the embodiment in FIG.
is added. The rotational speed signal x〓 ₁ of the master side arm shaft 1 is guided to the absolute value circuit 23 via the voltage regulator 22, and the absolute value signal |Vx| of the output signal Vx of the voltage regulator 22 is obtained. This absolute value signal |Vx
| and the output signal Vs of the Schmitt circuit 14 are input to the multiplier 24. Output from multiplier 24 |
The adder 25 adds Vx| The output signal of the adder 25 is proportional to the rotational speed signal x ₁ , as shown in FIG. The proportionality constant in this case can be arbitrarily set by the voltage regulator 22.

According to the embodiment shown in FIG. 7, effective friction compensation is possible even when the friction torque increases in proportion to the speed.

FIG. 9 is a block diagram showing a modification of the embodiment of FIG. 7. In this embodiment, the rotational position deviation ε is used as the input signal to the voltage regulator 22 instead of the speed signal x ₁ . Since this deviation ε is proportional to the slave side load torque, the output signal of the adder 25 V _SL
is proportional to the load torque. According to the embodiment of FIG. 9, effective friction compensation is possible even when the friction torque is proportional to the load torque.

It will be obvious that the embodiment of FIG. 7 and the embodiment of FIG. 9 may be combined.

Although the above embodiments have been explained using a symmetrical bilateral servo device as an example, the present invention can be similarly applied to asymmetrical bilateral servo devices such as a force reversal type and a force feedback type.

〔Effect of the invention〕

According to the present invention, the direction of the frictional force compensation torque can always be opposite to the frictional torque, improving the operability of the manipulator.

In particular, even if the friction torque is proportional to the speed or load torque, the compensating torque is increased in response to an increase in speed or load, resulting in improved operability.

[Brief explanation of drawings]

Fig. 1 is a block diagram of a conventional symmetrical bilateral servo device, Fig. 2 is a block diagram of a conventional symmetrical bilateral servo device that is an improvement over the conventional device shown in Fig. 1, and Fig. 3 is a symmetrical bilateral servo device according to the present invention. A block diagram of the first embodiment of the lateral servo device, FIG. 4 is an output characteristic diagram of the window comparator of the embodiment shown in FIG. 3, FIG. 5 is an output characteristic diagram of the Schmitt circuit of the embodiment shown in FIG. 3, and FIG. 6 a , b, c, d, e, f, g,
h and i are diagrams showing operating waveforms of each part of the embodiment in FIG. 3, and FIG. 7 is a block diagram of another embodiment of the present invention.
8 is an output characteristic diagram of the multiplier constituting the embodiment of FIG. 7, and FIG. 9 is a block diagram of a modification of the embodiment of FIG. 7. 1... Master side arm axis, 2... Slave side arm axis, 3, 4... Position detector, 5, 20... Subtractor, 6, 7... Amplifier, 8, 9... Electric motor, 1
0... Master side arm, 11... Slave side arm, 12... Force detector, 13... Differentiator, 14...
...Schmitt circuit, 15, 16...Compensation amplifier, 17...Speed detector, 18...Window comparator, 19...Analog switch, 21,2
5... Adder, 22... Voltage regulator, 23... Absolute value circuit, 24... Multiplier.

Claims (1)

[Claims] 1. A master operating mechanism operated by an operator, a slave operating mechanism that executes work, a position detector that detects the position of the drive shaft of each operating mechanism, and a position detector that detects the position of the drive shaft of each operating mechanism, and A means for driving the electric motor of the slave operating mechanism to follow the master operating mechanism, a force detector that detects the pressing force applied to the master operating mechanism, and a differentiator that differentiates the output of the force detector and outputs the torque change rate. and means for calculating a compensating torque to counteract the frictional force when driving each operating mechanism according to the magnitude and polarity of the output signal of the differentiator, and commanding and outputting it to each of the electric motors connected to each of the operating mechanisms. A bilateral servo device comprising: a speed detector that detects the rotational speed of the drive shaft of the master operating mechanism; a comparator that determines whether the detected rotational speed is within a predetermined range; and a switch for cutting off the output from the differentiator to the compensation torque generating means when the rotational speed exceeds the predetermined range, and means for maintaining the direction of the compensation torque at the time. Bilateral servo device. 2. The bilateral servo device according to claim 1, further comprising: means for obtaining the absolute value of the rotational speed; and compensation torque calculation means for multiplying the absolute value of the rotational speed by the output from the comparator. and a multiplier that outputs a correction value according to rotational speed. 3. The bilateral servo device according to claim 1, further comprising means for obtaining the absolute value of the rotational position deviation, and multiplying the absolute value of the rotational position deviation by the output from the comparator to obtain the compensation torque. A bilateral servo device, characterized in that the calculation means includes a multiplier that outputs a correction value according to the rotational position deviation.