JPH0683971B2 - Bilateral Servo Manipulator Control Method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION Field of Application of the Invention The present invention relates to a master
The present invention relates to a slave bilateral servo manipulator.

[Prior art]

In the conventional master / slave / bilateral servo manipulator, the master 10 and the slave 12 have the same control method as shown in FIG. Many are called symmetrical shapes. That is, the posture 102 of the master in which the human operating force 100 is applied to the master 10 changes, and this becomes the position command value of the slave. The position-controlled slave operates so as to follow the command value 102, and the posture 104 of the slave further serves as a position target for the master. When the slave is restrained by the external force 106, the position target 104 of the master is fixed. Therefore, human operation 1
A reaction force 108 from the master is generated with respect to 00, and the external force 106 equivalently applied to the slave is fed back to the human.

In reality, this is not the case, and even if a person applies an operating force 100 to the master, the master's posture 102 does not change due to the friction of the master, and even if the master's posture 102 slightly changes, Due to the friction, the attitude 104 of the slave does not change, and the position target of the master is fixed. Therefore, a large operation reaction force 108 is generated even though the external force 106 is not applied to the slave.

As a countermeasure for this, it is conceivable to provide a torque detector on the joint axis of each manipulator of the master and slave to directly measure and control the torque acting on the joint.
The effective control method has not yet been known.

As a master / slave bilateral servo control method using a torque detector, a basic configuration as shown in FIG. 2 can be considered. In an actual manipulator, there are a plurality of operating axes, but here, for simplicity, only one axis will be described.

With the master torque detector 20 attached to the master joint axis, the torque 202 acting on the master joint axis is measured,
Slave torque detector mounted on slave joint shaft
22 is used to measure the torque 204 acting on the slave joint axis.

The master 24 is driven by the difference 206 between the torque 202 acting on the master joint axis and the torque 204 acting on the slave joint axis, and the position of the slave 26 is controlled by the attitude 102 of the master. The slave posture 104 does not feed back to the master. According to this method, even if there is friction in the slave, if no external force is applied to the slave, the torque 204 detected by the slave torque detector 22 is zero,
The effects of slave friction do not appear in master operation. In addition, the effect of master friction is
You can give it. A master-slave type servo manipulator provided with a torque detector is disclosed in, for example, Japanese Patent Laid-Open No. 55-
101387, JP-A-55-101388, JP-A-55-120992,
It is proposed in JP-A-55-120993 or JP-A-56-147197 (JP-A-58-51086).

In this method, since the slave is merely position control, the center of control is the master control method, but this has the following problems.

As shown in FIG. 3, in terms of control, the torque detector 30 uses the strain generated due to the deviation 306 between the detector input angle 302 and the output angle 304, and the torque declining according to the deviation 306. Electrical torque detection signal 308 and mechanical transmission torque 310 are output. In this figure, K _e is the conversion gain from the deviation 306 to the electrical torque detection signal 308, and K _e
It is a conversion gain from the _m offset 306 to the mechanical transmission torque 310.

An equivalent control block of the master using the torque detector having such a meaning is as shown in FIG.
It can be seen that the positioning control system uses 302 as the command value. In the figure, the left side of the broken line is the torque detector. Explaining this block diagram, the electrical torque detection signal 308 from the torque detector is amplified by the torque control gain K _t to become an electrical torque 402, which is added to the mechanical transmission torque 310 of the torque detector to be effective. Torque 404,
The master mechanism inertia J is driven. As a result, the master moves at the angular velocity 406, and the angle of the master becomes 304. This angle 304 is controlled so as to match the human operation angle 302.

However, the characteristic feature is that such a position control loop is configured only when the input side of the torque detector is held by a human being, and when the human being acts as a master, the position shift loop will be shifted.
306 is always zero and the loop is no longer constructed.

Now, the transfer function G _(s) of this control loop from the human operation angle to the master angle is Therefore, it can be seen that it is a control system that does not require damping. That is, theoretically, when operated by a human, the angle of the master will continue to vibrate. However,
According to actual experiments, when a human grasped the master lightly and operated it, vibration did not occur, and when the human grasped the master tightly, steady vibration occurred. Even if vibration occurs, the vibration stops when the person releases the master because the position control loop disappears.

Further, even when the master was grasped and operated, the vibration did not occur when the torque control gain K _t was small. This is considered to be due to the damping caused by the motion loss of the human arm itself. However, when the torque control gain K _t is small, there is a problem that the movement is heavy due to the influence of the friction of the master.

[Object of the Invention]

Therefore, it is an object of the present invention to provide a control method for preventing vibration in a master / slave bilateral servo manipulator having a torque detector on a joint axis.

[Outline of Invention]

A feature of the present invention is that the torque control gain and the dynamic friction compensation amount are changed according to the operating speed of the master.

Example of Invention

 The present invention will be described below with reference to examples.

FIG. 5 is a diagram for explaining an embodiment of the present invention. An actual manipulator has seven degrees of freedom, but for simplicity, only one degree of freedom will be described here. In this embodiment, a microcomputer is used for servo calculation, and the calculation part is the part surrounded by the broken line in FIG.

By the torque detector 20 attached to the joint of the master 10 operated by a human, the torque acting on this joint is detected as the torque detection signal 202.

The torque detector 22 attached to the corresponding joint of the slave 12 detects the torque acting on this joint as a torque detection signal 204. These are N
_It is taken into the computer by the _D converters 50 and 51, and (MAS
_- TORQ) 502, (SLV _- the TORQ) 504. Then, the torque deviation (TORQ _- ERR) 506 is calculated. (TORQ _- ERR) = (MAS
_{- TORQ) - (SLV - TORQ} ) D / A output value to the converter (MAS _- TREF) 508 is calculated as follows.

_{(MAS - TREF) = (TORQ} - ERR) × K t + F m where _{K t} is a torque control gain, _{F m} is the dynamic friction compensation value. The torque control gain K _t and the dynamic friction compensation value F _m
Is configured as a function of the master speed _m . This will be described later in detail.

The output 510 of the D / A converter is added to the constant current driver 53 to drive the master driving DC motor 54. The output pulse of the incremental encoder 55 directly connected to this motor shaft is counted by the UP / D _OW n counter 56 and becomes the master position x _m . The velocity _{m of} the master is calculated from the position x _{m of the} master and the position information delayed by one sample. That is, _m = (1−e ^−sT ) / T · x _m Regarding the above-mentioned torque control gain K _t , when −v _m ≦ _m ≦ v _m , K _t = K _{H m} <−v _m , v _m However, in the case of < _m , _Kt = K _L However, in the case of K _L <K _H dynamic friction compensation value F _m , in the case of −v _m ≦ _m ≦ v _m , F _m = 0 v _m < _m , F _m = f ₁ > 0 in _m <-v _m, and summer and _{F m} = f ₂ <0. In this case, the actual values of v _m , K _H , K _L , f ₁ and f ₂ are set to appropriate values by experiments. Specifically, f ₁ and f ₂ are values such that no operation resistance due to dynamic friction is felt when the master is moved at a constant speed. v
_m is a switching speed between static friction and dynamic friction, and in this embodiment, And K _H is K _H chosen so that it is no longer felt the resistance at the time of start-up is determined so that the vibration does not occur. Furthermore, besides above for K _t the operating force in the velocity v _m is adjusted so as not discontinuous, as FIG. 6, _m
= 0, K _t = K _H , and it may be a function that asymptotically approaches K _L on both sides thereof.

Regarding the dynamic friction compensation value F _m , as shown in FIG. 7, _m =
At 0, F _m = 0, and when _m increases, it gradually changes to F _m = f ₁ , and when _m decreases, any function asymptotic to F _m = f ₂ is acceptable.

The slave is a position control servo with dynamic friction compensation, and the position x _m of the master is used as a position command input. Slave position deviation (SLV _- ERR) 550 is slave current position x _s and position command x _m , (SLV _- ERR) = x _m -x _s Slave speed command (SLV _- VREF) 552 is -v _l ≤ if _{- (ERR SLV) v l,} K p × (SLV - VREF) = K p × (SLV - ERR) K p × (SLV - ERR) <- v if _{l, (SLV - VREF) =} - v l If v _l <K _p × (SLV _- ERR), then (SLV _- VREF) = v _l, where K _p is the position control gain and v _l is the parameter defining the maximum speed of movement of the slave.

Slave speed deviation (SLV _- VERR) 554 is the slave speed
if _{s, (SLV - VERR) =} (SLV - VREF) - s Slave torque command (SLV _- TRER) 556 _{is, (SLV - TREF) = K} v × (SLV - VERR) + F s where, _{K v} Is the slave speed control gain. F _s is the slave dynamic friction compensation value, which is a function of slave speed _s , similar to that described for the master. That is, in the -v _s ≦ _s ≦ _{v s,} and summer and _F s = ₀ v s _{<In s F s = f 3> 0} s < In _{-v s F s = f 4 <} 0.

The slave torque command value (SLV _- TREF) 556 is converted into an analog signal 558 by the D / A converter 57 and input to the constant current driver 58. The output of this constant current driver 58
The DC motor 59 for slave drive is driven by 560. The output pulse 562 of the incremental encoder 60 directly connected to the DC motor shaft is counted by the Up / Down counter 61 and becomes the slave position command x _s . The slave speed is calculated by _s = (1−e ^−sT ) / T · x _s as in the master.

〔The invention's effect〕

According to the present invention, the force detector is attached to each joint axis, in the master-slave bilateral servo manipulator, the static friction of the master, dynamic friction, inertia, etc. are all compensated, without being affected by the friction of the slave, It is possible to realize a bilateral servo manipulator that is stable in terms of control, therefore has no vibration, and can be operated easily with extremely small force.

[Brief description of drawings]

FIG. 1 is an explanatory diagram of a conventional symmetrical bilateral servo manipulator. FIG. 2 is an explanatory diagram of a bilateral manipulator using a torque detector. FIG. 3 is a control explanatory view of the torque detector. Figure 4 shows
FIG. 6 is a control block diagram of a master using a torque detector.
FIG. 5 is a diagram for explaining an embodiment of the present invention.
FIG. 6 is a relationship diagram between the master speed and the torque control gain. FIG. 7 is a relationship diagram between the master speed and the dynamic friction compensation value.

Claims (4)

[Claims] 1. A method of controlling a human-controlled master / slave manipulator, wherein a torque detector is provided on each joint axis of the manipulator, and a torque difference acting on each corresponding axis of the master and slave is used as a torque control gain and a master friction compensation amount. Based on the calculation, the reaction force is fed back to the drive system of the master to drive it, the slave position is controlled according to the position of the master, and the torque control gain and master friction compensation amount are transferred to the master. A bilateral servo manipulator control method characterized by being changed according to speed. 2. The torque control gain is decreased in accordance with an increase in the moving speed, and the master friction compensation amount is increased in accordance with an increase in the moving speed. Bilateral servo manipulator control method. 3. The torque control gain is gradually reduced in accordance with an increase in the moving speed, and the master friction compensation amount is gradually increased in accordance with an increase in the moving speed. The method of controlling a bilateral servo manipulator according to the second item of the above. 4. Depending on whether or not the moving speed is a predetermined value or more,
4. The bilateral servo manipulator control method according to claim 3, wherein the values of the torque control gain and the master friction compensation amount are changed.