JP3086104B2 - Weight and center of gravity position correction device for force control work machine - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention
Regarding the center-of-gravity position correction device, in particular, for a work tool mounted on a force-controlled work machine via a force sensor, the parameters of the weight and the center of gravity of the work tool are obtained, and the detected values of the force sensor during work using these parameters The present invention relates to a weight / gravity center position correcting device for correcting the position.

[0002]

2. Description of the Related Art A work tool is attached to an arm tip of a mechanical section (robot main body) via a force sensor, such as a grinder, a deburring apparatus, a fitting apparatus, and the like. In a force control work machine configured to perform a force work by a work tool by changing a posture, a weight (gravity) of a work tool fixed to a force sensor and a holder thereof (hereinafter referred to as a tool) and a position of a center of gravity are set as parameters (gravity). It is necessary for control to set it as a (correction value). The reason is as follows.

When a tool performs a predetermined force operation on a workpiece, force data detected by the force sensor is a force applied to a detection center position of the force sensor. In contrast,
The force information to be known is the force acting on the tool from the work at the place where the tool and the work come into contact. Also, assuming that the tool is in the air and does not receive any external force, the tool has a weight due to gravity applied to its center of gravity. Therefore, a tool that is working in contact with a workpiece is subjected to a combined force of the acting force at the contact point and the force due to the weight of the tool itself, which is generated during the work. The force detected by the force sensor is a combined force. Therefore, in order to detect only the force information related to the work at the contact point of the tool, values relating to the weight of the tool and the position of the center of gravity are prepared in advance as gravity correction values, and the force data detected by the force sensor is used using this. I have to compensate. Therefore, it is necessary to set the weight and the position of the center of gravity of the tool as parameters for gravity correction.

As a conventional method for calculating the parameters of the tool weight and the position of the center of gravity, there is a method disclosed in Japanese Patent Application Laid-Open No. 62-74594. In the method of calibrating the force sensor of the robot apparatus disclosed in this prior document, a simple example is shown of how to take three postures required when obtaining the weight and the position of the center of gravity of the tool.

[0005]

According to the method described in the above prior art, it is not so easy for an operator to create the described three postures based on the guidance. That is, there is a case where it is impossible to calculate the weight and the position of the center of gravity of the tool depending on how to take the posture. In other words, the operator guides the tool and the arm, and the operator himself
Even if it is considered that three postures specified by a manual or the like are taken for the entire mechanism, two substantially same postures may be taken in relation to coordinates. In such a case, the three postures required for calculating the weight and the center of gravity are not taken, so that the weight and the center of gravity of the tool cannot be calculated. As described above, in spite of the fact that there is a method of setting the posture of the tool that cannot actually calculate the weight and the position of the center of gravity of the tool, in the above-described conventional apparatus, an operation method that the operator avoids this. There was a problem in not providing.

SUMMARY OF THE INVENTION It is an object of the present invention to obtain a required tool posture by simply performing the operation of setting the posture of the tool when obtaining the values of the weight and the position of the center of gravity of the tool with the force control work machine. The object of the present invention is to provide a weight / gravity center position correcting device for a force control work machine capable of calculating the center of gravity position with good accuracy.

[0007]

According to the present invention, there is provided an apparatus for correcting the weight and center of gravity of a force control work machine according to the present invention. A force control work machine that attaches a work tool via a force sensor that detects a moment about a point, operates a mechanism based on force control performed by a control unit, and causes the work tool to perform a predetermined force work. A first Cartesian coordinate system (base coordinate system) corresponding to the installation location of the mechanism,
A second Cartesian coordinate system (for example, a sensor coordinate system) corresponding to the force sensor is set, and force data applied to the work tool and posture data of the work tool are measured using the first and second Cartesian coordinate systems. Means, a posture setting means for guiding and setting the posture of the mechanism unit to which the work tool is attached to a plurality of postures, and a weight of the work tool from posture data and force data of the work tool measured using these coordinate systems.・ Weight and center of gravity position calculating means for calculating the position of the center of gravity, and further, force data and posture data relating to the work tool in each position when, for example, the operator guides and operates the mechanism unit to take a plurality of positions. And posture determination means for determining whether or not the posture data obtained by the measurement in each posture satisfies the conditions for calculating the weight and the center of gravity, and It provided with a posture determination notification means to notify the author. In the above configuration, preferably, the plurality of postures are at least three postures. Preferably, the posture determination means makes the determination based on a relative posture relationship of the second Cartesian coordinate system with respect to the first Cartesian coordinate system.
Further, this relative posture relationship is set by setting a conical guidance region that apexes the origin set on one axis of the first Cartesian coordinate system, and matches the origin of the second Cartesian coordinate system with the vertex. In this state, the axes of the second Cartesian coordinate system are included in the conical guiding area.

[0008]

In the present invention, when the operator guides and sets a plurality of postures of the work tool necessary in the process of calculating the weight and the position of the center of gravity of the work tool, when the operator adopts the posture, the posture becomes heavy. -Determine whether the posture is sufficient to satisfy the conditions that can be calculated in calculating the position of the center of gravity,
The determination result is notified. Thereby, the operator can immediately know whether or not the calculation of the weight and the center of gravity can be performed based on the posture of the mechanism section guided by the operator. The ability to obtain weight and center of gravity position parameters allows for accurate force feedback as required by a force controlled work machine.

[0009]

Embodiments of the present invention will be described below with reference to the accompanying drawings.

FIG. 2 is a block diagram showing the overall system of a force control work machine to which the weight / gravity center position correcting device according to the present invention is applied. The force control work machine includes a robot body (mechanical unit) 1 as a work machine that operates based on position and force control, a control device 2 for controlling the operation of the robot body 1, and a predetermined force work such as grinding. , A force sensor 4 for detecting a force and a moment applied to the work tool 3, and a teaching box 5 as a teaching device. It is natural that the robot body 1 performs position control when performing its operation, and also performs force control based on the necessity for work. Therefore, the robot body 1 is recognized as a force control work machine by its characteristics. The work tool 3 includes a tool holder, and is hereinafter referred to as a tool 3.

The robot body 1 is provided with an arm 6 on the upper side. The tool 3 is mounted on the wrist of the arm 6 via a force sensor 4, and the tool 3 has six degrees of freedom in performing the operation. It can be moved to any position in any posture. The robot body 1 has a Cartesian coordinate system (base coordinate system B: three orthogonal axes X _B , Y _B , Z) fixed to a pedestal 7 as shown in FIG.
_B ) is set. The arm 6 of the robot body 1
The force sensor 4 mounted on the wrist of the device measures the force in each of the three orthogonal axes (X _S , Y _S , Z _S ) and the moment around each axis, that is, the force in the six axes. Force sensor 4
2, a Cartesian coordinate system (sensor coordinate system S) unique to a force sensor having the origin at a force detection center (a reference point of moment) is set as shown in FIG.

The control device 2 is an arithmetic processing device constituted by a computer. The control device 2 controls the operation of the robot body 1 (controls the position and force) and calculates the weight and the center of gravity of the tool 4 according to the present invention. And a correction process. The teaching box 5 includes a plurality of command push buttons 8 necessary for teaching and guiding operation of the work operation of the robot body 1 and a display device 9 having, for example, a liquid crystal screen for displaying various information. By operating the teaching box 5, the operator can input data and instruction programs for instructing the robot main body 1 to perform any operation necessary for the power work.
It is stored in the storage device of the control device 2.

Next, the internal configuration of the control device 2 and the teaching box 5 will be described in detail with reference to FIG.

The robot operation control device 10 generates a signal for controlling the operation of a motor attached to each joint (movable axis) of the robot main body 1, and supplies the control signal to each motor to give the robot main body 1 a control signal. Take the necessary posture,
The tool 3 performs a necessary work operation. The control executed by the robot operation control device 10 handles position and force as control information, and includes so-called force control. In the force control, the force applied from the workpiece to the tool 3 is detected by the above-described force sensor 4, the detected force data is fed back and compared with a predetermined desired force reference value, and the desired force is determined based on the difference. Control is performed to cause this. The principle of the basic control of the robot main body 1 that operates based on the control of the position and the force, and the basic configuration of the robot operation control device 10 for performing this control are known. With force control,
It is necessary to use correct force information obtained by compensating for the gravity value of the tool 3 attached to the force sensor 4 and its holder. In order to perform the compensation calculation of the gravitational value of the tool 3, it is necessary to previously obtain the values of the weight and the position of the center of gravity of the tool as parameters (gravity correction values). The gravity value of the tool 3 differs for each tool. In actual grinding work, etc.,
Since the tool is exchanged according to the work content, it is necessary to relatively frequently perform the work of obtaining the parameters of the weight and the position of the center of gravity of the tool.

A force input device 11, a posture input device 12, a weight / gravity position calculating device 13, and parameters of the weight and the position of the center of gravity are stored as functional elements constituting a device for obtaining parameters of the weight and the position of the center of gravity of the tool 3. Storage device 14 is provided. The storage device 14 is a storage device consisting essentially of a RAM and a ROM provided in the control device 2 and stores programs used for controlling the operation of the robot body 1 and various control parameters. It is also used as a means for storing the parameters of the weight and the position of the center of gravity obtained by the center of gravity calculating device 13. Various control parameters and programs stored in the storage device 14 are read into the robot operation control device 10 at appropriate timing as needed.

The force input device 11 inputs force data detected by the force sensor 4 provided on the robot body 1. The force data detected by the force sensor 4 is expressed in the sensor coordinate system S described above. The force data detected by the force sensor 4 is the weight of the tool 3 itself when the tool 3 is not performing force work, and the force applied to the tool 3 from the work and the tool itself when the tool 3 is performing force work in contact with the work. Are combined. The posture input device 12 inputs posture data detected by a position displacement sensor provided at each joint of the robot body 1. The position displacement sensor is, for example, an encoder mounted in a motor of each joint of the robot body 1. The posture of the robot body 1 generally calculates the direction cosine of each axis using the detection output of the position displacement sensor. Based on this, it is expressed in the base coordinate system B.

The weight / center-of-gravity position calculating device 13 includes a force data input by the force input device 11 and a posture input device 12.
Has a calculation function for obtaining a weight parameter and a center-of-gravity position parameter of the tool 3 mounted on the wrist of the arm 6 of the robot main body 1 via the force sensor 4 using the posture data captured in Step 1. As the weight / center-of-gravity position calculating device 13, any device having such a calculating function can be used. For example, as one calculation method for obtaining a weight parameter and a center-of-gravity position parameter, the present inventors use A weight / gravity center position correction device in which weight parameters are previously obtained in a vector format (Japanese Patent Application No. 3-279).
774). In this weight / gravity center position correction device, since the weight parameter of the tool used for gravity correction is obtained by a vector amount expressed in a base coordinate system or the like, it is obtained by a scalar amount as in the previous calculation method. In comparison, the installation state of the force control work machine can be inevitably included in the weight representation of the work tool, which has the advantage that no means or adjustment process for adjustment is required. In any case, in the weight / gravity center position correcting device according to the present invention, a force input device 11 for capturing force data output from the force sensor 4, a posture input device 12 for capturing posture data output from the position displacement sensor, force data and posture data And a weight / centroid position calculating device 13 for calculating a weight parameter and a center-of-gravity position parameter by using the above. The condition that force data and position data on at least three postures are used in calculating the weight parameter and the center-of-gravity position parameter. As long as this condition is satisfied, an arbitrary weight / center-of-gravity position correcting device can be used for the configuration of the means for obtaining each parameter of the weight and the center of gravity.

The operator guides the robot body 1 to take, for example, a first posture, a second posture, and a third posture sequentially, and uses the force data and posture data obtained in each posture to use the weight / centroid position. The arithmetic unit 13 calculates a weight parameter and a center-of-gravity position parameter relating to the tool 3. weight·
The weight parameter and the center-of-gravity position parameter regarding the tool 3 obtained by the center-of-gravity position correcting device 13 are stored in the storage device 14.
Is stored in The weight parameter and the center-of-gravity position parameter stored in the storage device 14 are read into the robot operation control device 10 when a force operation is actually performed using the tool 3, and the force data detected by the force sensor 4 is weight-compensated. Used as data.

A feature of the present invention is that, when the operator takes the first posture, the second posture, and the third posture by guiding the robot main body 1, it takes three different postures correctly. Information can be provided to the operator at an appropriate timing, and the operator can determine whether the posture of the robot body 1 taken while guiding the robot body 1 is appropriate or inappropriate. In that it has a configuration that can be accurately recognized. Therefore, in order to be able to accurately recognize whether the posture of the robot body 1 taken by the operator by performing the guiding operation is appropriate or inappropriate, the control device 2 is provided with a posture calculation device 15 and a posture determination device 16. In addition, a posture guidance instruction device 17 and a posture determination device 18 are provided in the teaching box 5. The posture guidance instructing device 17 in the teaching box 5 is for giving a command for guidance operation input by the operator to the robot operation control device 10.

The operator operates the teaching box 5, that is, uses the attitude guidance instructing device 17 and the attitude determination notifying device 18 in the teaching box 5 to calculate an appropriate posture necessary for calculating the weight parameter and the center of gravity position parameter. To the robot body 1. Regarding the posture guidance instructing device 17 and the posture determination notifying device 18, specifically, the posture guidance instructing device 17 includes a plurality of push buttons 8 for guidance as described above, and the posture determination informing device 1
8 is configured to include the screen of the display device 9.

In order to calculate the parameters of the weight and the position of the center of gravity, the robot body 2 is operated based on the robot operation control device 10 in accordance with a command signal output from the posture guidance instruction device 17 by the operator.
, The posture of the robot body 1 is calculated, and the posture determination device 16 determines whether or not the posture at that time is sufficient to calculate each parameter of the weight and the position of the center of gravity. Posture determination device 1
As a result of the determination in step 6, if the posture satisfies the condition, the force data (force value) detected by the force sensor 4 at that time is taken in through the force input device 11 and the posture data output from the position displacement sensor Command to allow the user to input the data through the posture input device 12, the force input device 11, the posture input device 1
2. A message to the weight / gravity position calculating device and a message to take the next posture are given to the operator through the posture judgment notifying device 18. By repeating such an operation, data sufficient for calculating the weight parameter and the center of gravity position parameter based on at least three postures are prepared, the weight / center of gravity position calculating device 13 calculates the weight / center of gravity position, and the calculation result is obtained. It can be stored in the storage device 14. Note that the posture determination device 16 makes a determination based on, for example, a relative posture relationship between a sensor coordinate system (second Cartesian coordinate system) and a base coordinate system (first Cartesian coordinate system).

A method of taking a posture in the robot body 1 for calculating the weight and the position of the center of gravity will be described with reference to FIGS. It is necessary to take at least three types of postures for calculating the weight and the position of the center of gravity. Relative Z _B axis of the base coordinate system shown in FIG. 4 in this embodiment, Z _S axis of the sensor coordinate system in the first position enters the angle within theta (FIG. 3 (A)), the second
In the posture, the X _S axis of the sensor coordinate system enters an angle within θ (FIG. 3B), and in the third posture, the Y _S axis of the sensor coordinate system enters an angle within θ (FIG. 3C). ). The above guidance may be performed by guiding each axis of the sensor coordinate system into the conical guidance area 19 shown in FIG. 4 in a state where the origin of the base coordinate system B and the origin of the sensor coordinate system S coincide with each other. When the robot main body 1 is installed vertically, the operator may visually make each axis substantially vertical.
The above θ is desirably in the range of 0 ° ≦ θ <25 °.

The flow of the whole process will be described with reference to FIG.
First, the posture number (posture number) is set to 1 (first posture setting: step S1), and the teaching box 5 is set.
A guidance instruction message for guiding the operator to the first posture is displayed on the screen of the display device 9 (step S2). The operator manually operates the robot body 1 to take the first posture while depressing the guidance button (step S3) (step S4). When the operator determines that he or she has taken the first posture, the operator releases the depression of the guidance button (step S3) and depresses the posture determination button (step S5). When the attitude determination button is depressed, the attitude of the robot body 1 at this time is measured and calculated (step S6). Next, posture determination is performed (step S7). The first orientation is checked by Z _S axis of the sensor coordinate system. A unit vector indicating the direction of the _ZS axis is calculated with reference to the base coordinate system, and is calculated as (za _x , za
_y, when the za _z), determination is made by the following equation.

[0024]

[Number 1] za _z> cosθ

If the first posture does not satisfy the condition of (Equation 1), an error message is displayed using the posture judgment notifying device 18 (step S8). When the first posture satisfies (Equation 1), the force is detected by the force sensor 4 and output to the force input device 11 (step S9). Further, posture data obtained by the position displacement sensor is output to the posture input device 12 (step S10). Next judgment step S11
If the posture number is less than 3, the next posture (second posture) is set (step S12), and the above processing is repeated for the second posture. However, in the determination of the position, since the posture No2 (second posture) is determined by X _S axis, a unit vector indicating the direction of the X _S axis (xn _x, xn _y, xn
_z ) is determined by the following equation.

[0026]

## EQU2 ## xn _z > cos θ

Similarly, posture No. 3 (third posture) is Y
Since the determination is made on the _s axis, when the unit vector indicating the direction of the Y _s axis is (yo _x , yo _y , yo _z ), the determination is made by the following equation.

[0028]

[Equation 3] yo _z > cos θ

When the force data and the posture data up to the posture No. 3 have been obtained, the weight / gravity position calculating device 13 calculates the weight / gravity position using these data (step S13), and stores them in the storage device 14. It is stored (step S14). As an example of the calculation of the weight and the position of the center of gravity, as described above, for example, there is a method disclosed in Japanese Patent Application No. 3-279774.

In the above description of the embodiment, the method in which the operator guides the robot main body 1 by the push button 8 for guidance of the posture guidance instructing device 17 of the teaching box 5 has been described. Alternatively, the robot body 1 may move to a desired posture, or the robot body 1 may automatically adopt three postures to calculate each parameter of the weight and the position of the center of gravity.

Although the determination of the posture has been described using the sensor coordinate system, any coordinate system can be used as long as it is provided for convenience after the wrist of the robot body 1.

[0032]

As is apparent from the above description, according to the present invention, when the operator guides the robot body and takes at least three postures suitable for calculating the weight / gravity position for weight compensation. The robot is configured to notify the operator whether the posture set by the guidance is appropriate, so that the posture can be set accurately, the operability is good, and the accuracy of the calculation result can be guaranteed. In this case, the control performance is improved.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an internal configuration of a weight / gravity center position correcting device for a force control work machine according to the present invention.

FIG. 2 is a diagram showing an overall system configuration of the force control work machine.

FIG. 3 is a diagram showing each of three guided postures.

FIG. 4 is a diagram showing a guidance allowable range in a base coordinate system.

FIG. 5 is a flowchart showing a flow of overall processing in posture guidance.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Robot main body 2 Control device 3 Work tool 4 Force sensor 5 Teaching box 6 Arm 8 Push button 9 Display device 10 Robot motion control device 11 Force input device 12 Posture input device 13 Weight / gravity position calculation device 14 Storage device 15 Posture calculation device Reference Signs List 16 attitude determination device 17 attitude guidance instruction device 18 attitude determination notification device 19 conical guidance area

──────────────────────────────────────────────────続 き Continuation of the front page (72) Inventor Shinichi Sarugaku 7-1-1 Higashi Narashino, Narashino-shi, Chiba Hitachi Keiyo Engineering Co., Ltd. (56) References JP-A-3-55189 (JP, A) Sho 62-84991 (JP, A) JP-A-5-116081 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) B25J 3/00-3/04 B25J 9/10-9 / 22 B25J 13/00-13/08 B25J 15/04 B25J 19/02-19/06 B23Q 15/00-15/28 B23Q 3 / 155,3 / 157 B24B 27/00 B24B 41/00-51/00 G05D 15/01

Claims (4)

(57) [Claims] 1. A work tool is attached to a mechanism via force detecting means, and the mechanism is operated by force control by a control means to perform a force work on the work tool, and corresponds to an installation position of the mechanism. A first Cartesian coordinate system to be set and a second Cartesian coordinate system corresponding to the force sensor are set, the force applied to the work tool using the first and second Cartesian coordinate systems Means for measuring data and posture data of the work tool, posture setting means for guiding and setting the posture of the mechanism unit to which the work tool is attached to a plurality of postures, and the work tool obtained in each posture. Weight / gravity position calculating means for calculating a weight / gravity position for correction of the work tool using force data and the posture data, and wherein each of the plurality of postures has the weight / gravity position. Attitude determination means for determining whether or not the posture satisfies the condition for obtaining the heart position, and posture determination notification means for notifying the operator of the determination result. Center of gravity position correction device. 2. The weight of the force control work machine according to claim 1.
In the center-of-gravity position correcting device, the plurality of postures are at least three postures, wherein the weight / center-of-gravity position correcting device for a force control work machine is characterized. 3. The weight and weight of the force control work machine according to claim 1.
In the center-of-gravity position correcting apparatus, the posture determining means makes a determination based on a relative posture relationship of the second Cartesian coordinate system with respect to the first Cartesian coordinate system. Position correction device. 4. The weight and weight of the force control work machine according to claim 3.
In the center-of-gravity position correcting device, the posture relationship is the first position.
A conical guidance area that sets the vertex at the origin set on one axis of the Cartesian coordinate system is set, and the origin of the second Cartesian coordinate system is matched with the vertex. A weight / gravity center position correcting apparatus for a force control work machine, wherein each axis is included in the conical guidance area.