JP2503414B2 - Master-slave manipulator control method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION A. Field of Industrial Application The present invention relates to a method for controlling a master / slave manipulator, which is improved so that the slave side follows the master side well even when the inertia moment on the slave side is large. It was done.

B. Outline of the Invention The present invention corrects the amount based on the deviation between the master position and the slave position in consideration of the amount based on the deviation due to the moment of inertia on the slave side, and uses this correction value as the speed command of the speed servo system. is there.

C. Prior Art FIG. 3 is a block diagram showing a control system of a master / slave manipulator according to the prior art. In the figure, ▲ ^* _θ
▼ is position command, that is, master position, θ _ANS is slave position, Is speed command, _ANS is slave speed, P _M is master position sensor gain, P _S is slave position sensor gain, Ke
Is the position servo gain, Ks is the slave speed sensor gain, Km is the speed servo gain, Kt is the motor torque constant,
J is the motor shaft converted inertia moment, F is the motor shaft converted friction resistance, Ns is the slave side reduction ratio, and E (s) is the position deviation between the master and slave.

In such a master / slave manipulator, position control of each joint on the slave side amplifies the position deviation E (s) between the position command ▲ ^* _θ ▼ and the slave position θ _{ANS to obtain} a speed command. Input to the speed servo loop as This is done by amplifying the deviation from the slave speed _ANS and applying a current to the motor, converting the current to torque and starting rotation, and further amplifying the torque with a reducer to move the joint. In addition, in FIG. Is a quantity indicating the response delay at this time.

D. Problems to be Solved by the Invention However, in the above-mentioned prior art, when the inertia moment on the slave side is large, the position and speed on the slave side lag behind the position and speed on the master side. It goes too far with me. FIG. 4 shows this. In the figure, ● indicates master position (radian), ■
The mark shows the slave position (radian), the mark shows the master speed (radian / second), and the mark shows the slave speed (radian / second).
Are shown respectively.

In this way, the position and speed of the slave side lag behind the position and speed of the master side, and both of them go too far, which is a fatal defect for this type of master / slave manipulator. There is a risk of collision with objects and surrounding buildings. In order to prevent this, the operating speed of the slave should be kept as low as possible, but this causes annoyance and fatigue of the person operating the master, and operability and work efficiency are significantly deteriorated.

An object of the present invention is to provide a control method of a master / slave manipulator capable of removing an overshoot of the speed on the slave side with respect to the master side even when the inertia moment on the slave side is large.

E. Means for Solving the Problems The present invention that achieves the above object is based on the following findings. That is, in the control system shown in FIG. 3, when the position deviation E (s) between the master and slave is calculated, Becomes Of these, the term relating to JP _M S ² represents the deviation due to the moment of inertia.

Therefore, in the present invention, an amount called inertial deviation compensation gain, which is an amount based on the deviation due to the inertial moment on the slave side, is introduced so that the term relating to JP _M S ² of the equation is eliminated, and in consideration of this, the speed servo system It is a speed command.

F. Action According to the present invention having the above-described configuration, the term relating to JP _M S ² is eliminated from the equation, and the position deviation E (s) becomes as follows.

G. Examples Hereinafter, examples of the present invention will be described in detail with reference to the drawings.

FIG. 1 is a block diagram showing a control system of the present invention. In this embodiment, as shown in the figure, the inertia deviation compensation gain is the amount based on the deviation due to the inertia moment of the reduction ratio N _M, the speed sensor gain K _SM and the slave side of the master of the master-side
We are introducing a new amount, K _JC . The inertia deviation compensation gain K _JC is an amount represented by the following equation.

In the present embodiment, the velocity _M on the master side is detected by the sensor and a differential operation is performed, and the inertia deviation compensation gain is added to the differential value.
K _JC is multiplied, the amount obtained by this multiplication is added to the amount obtained by multiplying the position deviation E (s) by the position servo gain Ke, and the amount obtained by this addition is used as the velocity command of the velocity servo system. That is,
In this embodiment, the amount obtained by the multiplication is feed-forwarded to the conventional speed servo system, and the block surrounded by the dotted line in FIG. 1 is added to the conventional block.

 The position deviation E (s) at this time is expressed by the following equation.

As is clear from the above equation, according to the present embodiment, the term regarding JP _M S ^{2 in the} equation is deleted.

FIG. 2 is a graph showing an experimental result when this embodiment is applied when the inertia moment on the slave side is large. As is clear from the comparison between FIG. 4 and FIG. 4, according to the present embodiment, the overshoot of the position and speed on the slave side is small.

H. Effects of the Invention As specifically described in connection with the above embodiments, according to the present invention, even if the moment of inertia on the slave side is large, it is possible to eliminate those overshoots of the slave with respect to the position and speed of the master. . That is, even when proportional integral or proportional integral derivative control is used in the speed servo system, slave follow-up (overshoot) can be improved. Therefore, it is possible to increase the operation speed without worrying about collision with the slave work object and the surrounding building, and to improve the operability and work efficiency of the master operator by improving irritability and fatigue.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram showing a control system of an embodiment of the present invention, FIG. 2 is a graph showing response characteristics of position and speed on the slave side to position and speed on the master side, FIG. 3 is a block diagram showing a control system according to the prior art, and FIG. 4 is a graph showing response characteristics of position and speed on the slave side to position and speed on the master side. In the drawing, ▲ ^* _θ ▼ is the position command, θ _ANS is the slave position, Is speed command, _ANS is slave speed, P _M is master position sensor gain, P _S is slave position sensor gain, Ke
Is the position servo gain, Ks is the slave speed sensor gain, Km is the speed servo gain, Kt is the motor torque constant,
J is converted to motor shaft inertia, F is converted to motor shaft friction, Ns is the slave reduction ratio, E (s) is the position error between the master-slave, N _M is the reduction ratio of the master side, K _SM is master The velocity sensor gain, K _JC, is the inertia deviation compensation gain.

Claims (1)

(57) [Claims] 1. In a control method of a master / slave manipulator in the case where a slave follows a master and a moment of inertia on the slave side is large, a master speed detected by a sensor is differentially calculated, and the slave side is set to the differentially calculated amount. Multiply the inertia deviation compensation gain K _JC , which is the amount based on the deviation due to the inertia moment of, and add the resulting amount to the amount obtained by multiplying the deviation between the master position and slave position by the position servo gain. In addition to the system speed command, the above inertia deviation compensation gain K _JC is given by the following equation: K _JC = J · P _M · N _S / P _S · N _M · K _SM · Km · Kt However, J; motor shaft conversion inertia P _M; master-side position sensor gain N _S; slave reduction ratio P _S; slave position sensor gain N _M; master speed reduction ratio K _SM; master speed Sensagei Km; Velocity servo gain Kt; Control method for master / slave manipulators, which is calculated by the torque constant of the motor.