JPH0373282A - Control method for master arm - Google Patents

Abstract

PURPOSE:To compensate a gravity torque without operating the gravity torque at a gravity torque compensation processing part by subtracting a torque command value immediately after making a PI compensation processing with its feedback from the torque signal which is the output signal of a torque sensor detecting the torque acting on each axis of a master arm. CONSTITUTION:A torque command value immediately after making a PI compensation processing to be fed to the motor of each axis of a master arm is subtracted with its feedback from the torque signal which is the output signal of a torque sensor detecting the torque acting on each axis of the master arm. The gravity part of the master arm is thus compensated and the operational force of the master arm can be lightened enough.

Description

DETAILED DESCRIPTION OF THE INVENTION Field of Industrial Application The present invention relates to the fi11 method of a master arm, and is useful when applied to compensate for the dead weight of the master arm.

B. Summary of the Invention The present invention provides self-weight compensation by feeding back a torque command value obtained by processing a torque signal, which is an output signal of a torque sensor that detects the torque acting on each axis of a master arm, and subtracting it from the torque sensor. It was designed to do this.

C. Conventional technology Master slave manipulator The machine has a master arm, which is a multi-jointed artificial arm, and a slave arm, which is similar in shape to the master arm, and when the operator operates the master arm, the slave arm becomes the master arm. The slave arm processes the object by controlling it to follow the position of the object.

FIG. 4 is a block diagram showing an example of the configuration of this type of master-slave manipulator. As shown in the figure,
This master/slave manipulator consists of a master arm/slave arm, a sensor section attached to each axis of each arm, a sensor input converter section, a control device section, and a drive section. Among these, the control device section inputs sensor input values, performs internal processing, and outputs a torque command value. The drive unit converts this torque command value (voltage) into a current command value and outputs it. This current command value drives the motor built into each axis of the master arm/slave arm,
The generated torque is transmitted to each shaft via a transmission gear. The position, speed, torque, etc. of the arm at this time are detected by the sensor section and supplied to the sensor input conversion section.

Some Kakaru master/slave manipulators are designed to perform self-weight compensation to cancel the torque due to the master arm's own weight in order to reduce the operating force of the master arm. 5 and 6 show the f! of the master arm of this type of master-slave manipulator. FIG. 2 is a block diagram showing the jIIIU system. FIG. 5 is a block diagram before operation, that is, in the hold mode, and FIG. 6 is a block diagram during operation, that is, in the normal operation mode. As shown in Fig. 5, in the hold mode control system, the position input value is set in the hold value setting section 1 as a target value, and this target value and the position signal that is the output signal of the position sensor are used for position control. The servo gain processing section 3 multiplies the positional deviation from the signal processed by the conversion processing section 2 by the servo gain, and the speed engineering conversion processing section 4 converts the data multiplied by the servo gain and the speed signal, which is the output signal of the speed sensor. A speed type PI compensation processing unit 5 performs speed type PI compensation processing on the speed deviation with the signal obtained by processing, and the resulting data is used as a torque command value.

The dead weight compensation processing section 6 calculates the dead weight torque of the master arm based on a signal based on the position signal processed by the position engineering conversion processing section 2. The offset value holding unit 7 stores the torque signal, which is the output signal of the torque sensor, processed by the torque engineering conversion processing unit 8 (the difference between No. 2 and the signal representing the dead weight torque processed by the dead weight compensation processing unit 6). In this hold mode, no external force other than self-weight torque is acting on the slave arm, so the torque sensor has no offset value and the self-weight compensation process is properly performed. In this case, the torque deviation is zero, but since this torque deviation 1. is usually not zero, this torque deviation is held as an offset value, and corrections that take the torque deviation into account are performed in the operation mode described below. It has become.

As shown in Figure 6, in the control system depending on the operation mode,
The self-weight compensation processing unit 6 converts the torque signal detected by the torque sensor during operation into a signal processed by the torque engineering conversion processing unit 8.
The signal representing the self-weight torque processed in 2 and the offset value held in the offset value holding section 7 are subtracted, and this data is multiplied by a friction compensation gain in the wear compensation gain processing section 9 to form a torque command value. At this time, the final torque command value supplied to the motor of the master arm is determined from the torque command value immediately after being multiplied by the friction compensation gain, and a signal representing the self-weight torque which is an output signal of the self-weight compensation processing section 6 and the slave arm. The reaction force torque signal representing the load acting on the motor is subtracted, and a signal representing the speed obtained by processing in the speed engineering conversion processing section 4 is added.

D Problems to be Solved by the Invention In the control system shown in FIGS. 5 and 6, a signal representing the dead weight torque for compensating the master arm's dead weight is generated by the dead weight compensation processing section based on the position signal detected by the position sensor. 6
Since the torque is obtained by calculating the dead weight torque, there is a drawback that the configuration associated with this calculation process is complicated.

The present invention has been made in view of the drawbacks of the prior art described above.

It is an object of the present invention to provide a control method for a master arm that can perform self-weight compensation without performing calculation processing.

E. Means for Solving the Problems The configuration of the present invention that achieves the above object is a master arm control system that performs self-weight compensation to cancel the torque due to the master arm's own weight, in which each of the master arms A feature is that the torque command value immediately after pH compensation processing to be supplied to the shaft motor is fed back and subtracted from the torque signal that is the output signal of the torque sensor that detects the torque acting on each shaft of the master arm. do.

F Effect In the present invention having the above configuration, the torque command value immediately after the PI compensation process is an amount due to the master arm's own weight torque, so if this amount is subtracted from the torque acting on the master arm, the master arm's torque is calculated. Dead weight needle is compensated,
The operating force of the master arm was light enough.

G. Embodiment Embodiments of the present invention will be described in detail below with reference to the drawings. Note that parts that are the same as those in the prior art are given the same numbers and redundant explanations will be omitted.

FIGS. 1 and 2 are block diagrams showing a control system of a master arm that realizes an embodiment of the present invention. FIG. 1 is a block diagram before operation, that is, in a hold mode;
FIG. 2 is a block diagram during operation, that is, in normal operating mode.

As shown in FIG. 1, in the hold mode, the control system feeds back the torque command value immediately after PI compensation processing in the PI compensation processing section 5, and converts the torque signal, which is the output signal of the torque sensor, into torque engineering. The torque deviation, which is the difference between the signal and the signal obtained by processing in the processing section 8, is determined. Since the torque command value at this time is a chair number representing the dead weight torque of the master arm, the offset value of the torque sensor after performing dead weight compensation is held in the offset value holding section 7.

As shown in Figure 2, in the control system in the operation mode,
The PI compensation processing unit 5 converts the torque signal detected by the torque sensor during operation into a signal processed by the torque engineering conversion processing unit 8.
After subtracting the feedback torque command value and offset value immediately after PI compensation processing, the nonlinear gain processing section 10 multiplies the nonlinear gain, and the signal multiplied by this nonlinear gain is processed by the speed engineering conversion processing section 4. A signal representing the speed is subtracted, and a PI compensation processing section 5 performs PI compensation processing. The final torque command value supplied to the motor of the master arm is PI
It is obtained by subtracting the reaction torque signal from the torque command value immediately after the compensation process.

In such a control system, since the torque command value immediately after the PT compensation process is performed by the PI compensation processing section 5 corresponds to the magnitude of the self-weight torque caused by the self-weight of the master arm, this torque command value is converted into torque engineering. Sufficient self-weight compensation can be performed by subtracting it from the signal representing the torque of the master arm after being processed by the processing section 8.

FIG. 3 is a characteristic diagram showing the nonlinear gain characteristics in the nonlinear gain processing section 10. In the figure, the horizontal axis (y input Ti
, and the gain is plotted on the vertical axis. As shown in the figure, in this nonlinear gain characteristic, an input dead zone is provided in the vicinity of the input Ti=0, and the output gain in this input dead zone is already zero; In this case, the output gain is -a. By providing the input dead zone in this way, it is possible to prevent vibration of the master arm when the input T1 is near zero, and the starting force of the master and arm is determined by the parameter of the output gain a of the nonlinear gain characteristic and the input dead zone Wb+7) parameter. Be able to adjust it.

Effects of the Invention As specifically explained in conjunction with the above embodiments, the present invention provides a torque sensor that feeds back the torque command value immediately after PI compensation processing and detects the torque acting on each axis of the master arm. Since the output of (torque No. 43, which is No. 1) is subtracted, the dead weight torque can be compensated without calculating the dead weight torque in the dead weight compensation processing section.

[Brief explanation of drawings]

FIG. 1 is a block diagram showing a control system according to an embodiment of the present invention in hold mode, and FIG. 2 is a block diagram showing a control system according to an embodiment of the present invention in continuous mode. FIG. 3 is a characteristic diagram showing nonlinear gain characteristics, FIG. 4 is a block diagram showing an example of the configuration of a master/slave manibrake, and FIG. 5 is a block diagram showing a control system according to the prior art even in hold mode. FIG. 6 is a block diagram showing a control system according to the prior art in an operation mode. Is 5 in the drawing PI? 11! Compensation processing section 8 is a torque engineering conversion processing section. Patent applicant Meidensha Co., Ltd. Agent

Claims (1)

[Scope of Claims] A control method for a master arm that performs self-weight compensation to cancel the torque due to the master arm's own weight, which immediately after PI compensation processing is performed to supply motors for each axis of the master arm. A master arm control method characterized in that a torque command value is fed back and subtracted from a torque signal that is an output signal of a torque sensor that detects torque acting on each axis of the master arm.