JP2681966B2 - Bilateral control method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION A. Field of Industrial Application The present invention relates to a bilateral control method for a master / slave type servo manipulator, which is improved so that reaction force torque generated in a master arm does not burden the operator. Is.

B. Outline of the Invention In the bilateral control method according to the present invention, the reaction torque generated in the master arm by the bilateral control is the reaction torque based on the torque of the slave arm, the position of the master arm, and the position of the slave arm. Since it is proportional to the product of the servo gain processing data obtained from the deviation, if the position deviation is small, even if the load on the slave arm is large, the reaction torque will be small and it will not burden the operator. If there is a large position deviation and the reaction torque is large, the operator recognizes the collision.If the operator moves the master arm too fast, the position deviation will be large and the reaction torque will be large and the speed will increase. Braking the master arm so that it is not too far.

C. Conventional technology In the bilateral control system of the master / slave type servo manipulator, when a load is applied to the slave side, a feedback torque is generated on the master side as a bilateral reaction force.

Conventionally, this reaction torque is mastered by multiplying the slave torque input ST by the torque sensor by the reaction force gain in the reaction force gain processing unit 1 and appropriately limiting it in the reaction force limit processing unit 2 as shown in FIG. It is given by driving the motor MM.

It should be noted that FIG. 2 shows the configuration for a specific axis,
The servo gain processing unit 3 multiplies the position deviation between the master position input MP and the slave position input SP by the position sensor by the servo gain, and subtracts the slave speed input SS by the speed sensor as a feedback value from the obtained servo gain processing data, The PI compensation processing unit 4 performs PI compensation on the obtained difference and supplies the result to the slave motor SM as a motor drive current value.

Also, to prevent the operability from decreasing due to the master's own weight and friction, the master torque input M.
The deviation between the self-weight compensation value obtained by calculating T and the master position input MP in the self-weight compensation processing unit 5 is multiplied by the friction gain in the friction gain processing unit 6, and the obtained value is used as the master speed input MS by the speed sensor. And the self-weight compensation value are added, and the reaction torque from the slave is subtracted from this added value to obtain the master motor M.
It is the motor drive current value supplied to M.

Further, in order to prevent the reaction torque from being generated by the slave's own weight, the own weight compensation processing unit 7 calculates the own weight compensation value from the slave position input SP, subtracts the own weight compensation value from the slave torque input ST, and multiplies the reaction force gain. ing.

D. Problems to be Solved by the Invention As is clear from FIG. 2, in bilateral control,
When a load is applied to the slave arm, the drive current of the slave motor SM increases in an attempt to maintain the position at this time, and as a result, the slave torque input ST increases.

Conventionally, when the slave torque input ST increases, even though it is within the range of the reaction force limit, the reaction force torque directly increases, so that the master operator feels a large load, which is contrary to the operability. The effect was negative.

In view of the above-mentioned conventional technique, an object of the present invention is to provide a bilateral control method for applying a reaction torque so as to reduce the burden on the operator.

E. Means for Solving the Problems The bilateral control method according to the present invention, the reaction force torque generated in the master arm, the reaction force torque based on the torque of the slave arm, the position of the master arm and the position of the slave arm It is characterized in that it is proportional to the product with the servo gain processing data obtained from the deviation.

F. Action In the above configuration, if the positional deviation between the master arm and the slave arm is small, the reaction torque becomes small even if the load on the slave arm is large. Therefore, it does not burden the operator.

On the other hand, if the slave arm collides with something and the position deviation is large, a large reaction torque is generated, and the operator is made to recognize the collision. Further, when the operator moves the master arm too fast, the positional deviation becomes large, and the master arm is braked so that the reaction torque is not too large and too fast.

G. Embodiment An embodiment of the present invention will be described in detail with reference to FIG.

The embodiment shown in FIG. 1 is one in which the present invention is applied to the conventional example shown in FIG. 2, and the present invention is realized by a portion 8 surrounded by a broken line.

That is, the reaction force gain processing unit 1 is applied to the slave torque input ST.
Is multiplied by the reaction force gain, and the result is multiplied by the servo gain processing data determined by the position deviation between the master arm and the slave arm obtained by the servo gain processing unit 3 to obtain an absolute value, and the reaction force command value is multiplied. I am trying. Reference numeral 9 is an absolute value processing unit, and 10 is a multiplication unit.

As in the conventional case, the self-weight compensation processing unit 7 calculates a self-weight compensation value from the slave position input SP so that the reaction force torque is not generated by the self-weight of the slave, and the slave torque input S.
The weight compensation value is subtracted from T. Further, the reaction force limit processing unit 2 appropriately limits the reaction force torque to drive the master motor MM.

On the other hand, in order to prevent operability from decreasing due to the weight and friction of the master, the master torque input value from the torque sensor
The friction gain processing unit 6 calculates the difference between MT and the self-weight compensation value obtained by calculating the master position input MP in the self-weight compensation processing unit 5.
Multiply the friction gain with, add the master speed input MS by the speed sensor and the weight compensation value to the obtained value, subtract the reaction torque from the slave from this added value, and supply it as the motor drive current value to supply to the master motor MM. There is.

Further, as the follow-up control of the master arm and the slave arm, the servo gain processing unit 3 multiplies the position deviation between the master position input MP and the slave position input SP by the servo gain,
Subtract the slave speed input SS as a feedback value from the obtained servo gain processing data, and add P to the obtained difference.
The I compensation processing unit 4 performs PI compensation, and supplies the result to the slave motor SM as a motor drive current value.

Due to the above, even if the slave torque input ST becomes large, for example, even if the deviation between the slave torque input ST and the self-weight compensation value becomes large, if the position deviation between the master arm and the slave arm is small, it will result in the master arm. The reaction torque becomes smaller. Therefore, even if a heavy load such as a torque wrench is applied to the slave arm,
Slave motor SM trying to maintain the position at this time
The drive current of S increases, and as a result, the slave torque input S.
Even if T increases, it does not burden the operator of the master arm.

On the other hand, when the slave arm collides with an object, the positional deviation between the master arm and slave arm increases and the reaction torque increases, so the reaction force of the collision is correctly returned to the operator, and the collision can be recognized. Therefore, the operability can be improved.

Further, it is dangerous for the operator to suddenly move the master arm, but in such a case as well, the position deviation becomes large, so the reaction torque is fed back and braking is applied to the sudden operation of the master arm.

In the above embodiment, the multiplication unit 10 is replaced by the reaction force gain processing unit 1.
Although it was placed in the latter stage, it may be in the former stage. The reaction unit gain processing unit 1 may have a variable gain, and the reaction unit gain may be changed in proportion to the servo gain processing data, so that the multiplication unit 10 may be omitted.

H. Effect of the Invention According to the present invention, even when a heavy load such as a torque wrench is applied to the slave arm of the master / slave type servo manipulator, the reaction torque is not returned to the master arm and the burden on the operator is increased. do not become.

On the other hand, when the slave arm cannot move due to collision with an object, the reaction torque is returned to the master arm, and the operator can recognize the collision or the like. Further, even when the master arm is rapidly moved, the reaction torque is returned to the master arm, and the sudden operation of the master arm can be braked.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an embodiment of the present invention.
The figure is a block diagram of a conventional example. In the drawing, MP is master position input, SP is slave position input, MM is master motor, ST is slave torque input, 1
Is a reaction force gain processing unit, 3 is a servo gain processing unit, 8 is an improvement unit, 9 is an absolute value processing unit, and 10 is a multiplication unit.

Claims (1)

(57) [Claims] 1. A reaction force torque generated in a master arm by bilateral control is composed of a reaction force torque based on a torque of a slave arm and servo gain processing data obtained from a deviation between a position of the master arm and a position of the slave arm. A bilateral control method characterized by being proportional to a product.