JPS5969279A - Control system of bilateral servo manipulator - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed Description of the Invention] [Field of Application of the Invention] The present invention relates to a mask and mask operated by remote control by a human.
This relates to a slave pirate servo manipulator.

[Prior art]

In the conventional mask/slave/pillateral servo manipulator, the master 1° and slave 12 are connected as shown in Figure 1.
The control method is the same, both are based on position control, and the input and output of each is connected, so they are often called symmetrical. In other words, the master 10 has human operating power 1
00 is added, the master's attitude 102 changes, and this becomes the slave's position command value. The slave whose position is controlled operates in accordance with its command value 102, and the slave's posture 104 is configured to further serve as a positional target for the mask. When the slave is restrained by the external force 106, the mask position target 104 is fixed.

Therefore, a reaction force 108 is generated from the master in response to the human operation 100, and the external force 106 applied to the slave is fed back to the human in a manner that is economical.

In reality, the above does not happen, and even if a human applies an operating force of 100 to the mask, the master's posture 102 does not change due to the friction of the mask, and even if the master's posture 102 changes slightly, the slave's posture 102 does not change. Slave posture 1 for friction
04 does not change, and the mask position target is fixed. Therefore, a large operational reaction force 108 is generated even though no external force 106 is applied to the slave.

As a countermeasure against this, it may be possible to install a torque detector on the joint axis of each mask/slave manipulator to directly measure and control the torque acting on the joint.
An effective control method for this has not yet been known.

A basic configuration as shown in FIG. 2 is conceivable as a control system for a mask/slave bilateral servo using a torque detector. In an actual manipulator, there are multiple operating axes, but for the sake of simplicity, the - axis will be explained here.

The master torque detector 20 attached to the joint axis of the mask measures the torque 202 acting on the mask joint axis, and the slave torque detector 22 attached to the slave joint axis measures the torque acting on the slave joint axis. Measure 204.

The master 24 is driven by the difference 206 between the torque 202 acting on the mask joint axis and the torque 204 acting on the slave joint axis, and the position of the slave 26 is controlled by the posture 102 of the master.

Slave pose 104 does not feed back to the mask. According to this method, even if there is friction on the slave, if no external force is applied to the slave, the torque 204 detected by the slave torque detector 22 is zero, and the influence of the friction on the slave is I can't handle the mask.

Furthermore, the effect of mask friction can be dealt with by removing the master friction compensation 208.

In this method, since the slave performs positional control, the main control is the mask control method, but this method has the following problems.

As shown in FIG. 3, from a control point of view, the torque detector 30 utilizes the shear strain caused by the deviation 306 between the detector manual angle 302 and the output angle 304. In response, an electrical torque detection signal 308 and a mechanical transmission torque 310 are output. In this figure, Ko is an electrical torque detection signal 30 from a deviation 306.
8 and KITl is the conversion gain from the deviation 306 to the mechanically transmitted torque 310.

It can be seen that the master isometric control block using the l·lux detector having such a meaning becomes a positioning control system using the human operation angle 302 as a command value, as shown in FIG. In the figure, the torque detector is to the left of the broken line. Referring to this block diagram, the electrical torque detection signal 308 from the torque detector has a torque control gain Kt
This is amplified by the electric torque 402, which is added to the mechanical transmission torque 310 of the torque detector to become an effective torque 404, which drives the mask mechanism inertia J.

This causes the mask to move at an angular velocity of 4o6, and the angle of the mask is 304. This angle 304 is controlled 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; when the human removes the mask, the deviation 306 always becomes zero; The loop will no longer be constructed.

Now, the transfer function G1 of this control loop from the human operation angle to the mask angle. ) becomes, and it can be seen that the control system has no damping at all. In other words, theoretically, when a person operates the mask, the angle of the mask will continue to vibrate. However, according to actual experiments, when a person lightly grasped the mask while operating it, no vibrations were generated, but when a person operated the mask with only a slight grip, steady vibrations occurred. Even if vibrations occur, the vibrations will stop when the person removes the mask, as the position control loop disappears.

Further, even when the mask was operated by firmly grasping it, no vibration occurred when the torque control gain Kt was small. This is thought to be due to damping due to loss of motion in the human arm itself.

However, when the torque control gain Kt is small,
The problem was that the movement was slow due to the friction of the mask.

[Purpose of the invention]

Therefore, an object of the present invention is to provide a control method for preventing vibration in a mask-slave bilateral servo manibulator having a torque detector on the joint shaft.

[Summary of the invention]

A feature of the present invention is that the torque control gain and the amount of dynamic friction compensation are changed depending on the operating speed of the mask.

[Embodiments of the invention]

The present invention will be explained below according to examples.

FIG. 5 is a diagram illustrating the present invention in detail.

An actual manipulator has seven degrees of freedom, but for simplicity, only one degree of freedom will be explained here.

In this embodiment, a microcomputer is used for the servo calculation, and the calculation section is the part surrounded by a broken line in the figure.

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

In addition, the torque acting on this joint is determined by the torque detector 22 attached to the corresponding joint of the slave 12.
It is detected as a torque detection signal 204. These were each taken into a computer by ND Nigonnoku overnight 50゜51, (MA8 ''' TOII, Q) 502,
(8LV-TORQ) becomes 504. Then, the torque deviation (TORQ-ER,) 506 is calculated as 11°.

(TOR,Q-Ell几)=(MA8-TORQ)-<
8LV-TORQ) Direct output to converter (M
A8-TEF) 508 is calculated as follows.

(MA8-T EF) = (TORQ, -Ell 几)XK
t +F mHere, Kt is the torque control gain, Fl
n is a dynamic friction compensation value. The torque control gain Kt and the dynamic friction compensation value pm are configured as a function of the master speed Xr,l. This will be detailed later.

The output 510 of the D/A converter is applied to a constant flow driver 53 to drive a 1) C motor 54 for mask driving. The output pulses of the incremental encoder 55 directly connected to this motor shaft are counted by the Up/[)own counter 56 and become the master position xrll. From this master position xlTl and positional information delayed by one sample, mask velocities x and ll are calculated. That is, xm-(1-e-")/T-X,n Regarding the torque control gain ICI mentioned above, o-
y□≦;
For xnl, K t= K However, Kb<Kg For the dynamic friction compensation value F m, 0 − y , <;−X m@v ,,, then F
When rl1=0° V In < x171, F m = ft > 0 0 xm -vlT+, F m =' +1 < 0. In this case, vnl + [(, R+ K
The actual value of L H'l+f11 is determined to be an appropriate value through experiments. Specifically, fl I F is set to a value such that no operational resistance due to dynamic friction is felt when the mask is moved at a constant speed. vIn is the switching speed between static IF friction and dynamic friction, and this example does not feel like 'f! 2. KH is selected so that vibration does not occur. Furthermore, regarding Kt, which is adjusted so that the operating force does not become discontinuous at the speed vm, in addition to the above, as shown in Fig. 6, at xm-〇, Kt = KR, and asymptotically approaches Kt on both sides. It may be a function.

Regarding the dynamic friction compensation value Fm, as shown in FIG.
” Any function may be used as long as it asymptotes to fl and asymptotes to F, = f2 as delta □ decreases.

The slave is a position control servo with a dynamic friction compensation value, and uses the master's position [X In] as a position command input. Slave position deviation (SLV-ERR) 550 is calculated from slave current position X3 and position command x, 1 (SI, V-ERR,
)=Xm-:(.

The slave speed command (SI, V-IEF') 552 is o
'Vt≦K p > Te (ST+V-E t mo 几)≦
If Vl, then (SLV-Vl(,Eli')=+(,
x (Sl, V,,ERR)0 Kp×(SLv-E
RR) -vt, then (SLV-VREF) = -V
If to Vt (Kp X (SLY-ERR).

(SLY-V几EF')=Vt Here, 1 (p is the position control gain, and vt is a parameter that defines the maximum movement speed of the slave.

The slave speed deviation (SLV-VEILR) 554 is
Slave speed xl! Then, (8LV-VER,R)-(SLV-VREF)
Xl] Slave torque command (SLV-Tfl, EF'
) 556 Ha,<SL%ji”REF)=K vX
(8LV-VERII,) 10F.

where I(v is the slave speed control gain).

Fs is the slave dynamic friction compensation value, which is a function of slave speed XB, similar to that described in Mask. That is, for OVg≦x8≦V, l, Fg = 0 0 V g <

It becomes.

Slave/L/re command value (SLV-TREF) 556
is converted into an analog signal 55 by the D/A converter 57.
8 and input to the constant current driver 58.

The slave drive DC motor 59 is driven by the output 560 of the constant current driver 58. The output pulses 562 of the incremental encoder 60 directly connected to this DC motor shaft are counted by the IJp/l) own counter 61 and become the slave position command XI+. The slave speed is the same as the master i, X, −(1e −m
T)/T, calculated by X8.

[Effects of the Invention] According to the present invention, in a master V-private lateral servo manipulator in which a force detector is attached to each joint axis, static friction, dynamic friction, inertia, etc. of the mask are all compensated for, and the friction of the slave is compensated for. It is possible to realize a piratesil servo manibulator that is unaffected, stable in terms of control, and therefore vibration-free and that can be easily operated with extremely small force.

[Brief explanation of drawings]

Claims (1)

[Claims] 1. A human-controlled mask slave manipulator,
A torque detector is installed on each joint axis of the manipulator, and the torque difference acting on each corresponding axis of the mask and slave is used as a reaction force.
In the pirate servo manipulator that feeds back to the master drive system, the mask is torque-controlled according to the output of the master torque detector, and the slave is controlled in position according to the posture of this mask, and the torque control gain of the mask is A bi-chiral servo manipulator control method characterized in that the amount of mask friction compensation is changed according to the moving speed of the mask. 2. The torque control gain is decreased according to the increase in the moving speed, and the The control method according to item 1, characterized in that the amount of mask friction compensation is increased in accordance with the increase in the movement speed. 3. The torque control gain is increased by 41 in accordance with the increase in the movement speed.
2. The control method according to item 2, wherein the mask friction compensation amount is increased to f'λ according to an increase in the moving speed. 4. Depending on whether the moving speed is equal to or higher than a predetermined value, the torque control gain is set to a first value or a second value smaller than the first value, and the mask friction compensation amount is set to a third value. or a second value larger than the third value.