JPH03270865A - Position and force control device for working machine with multiple degree of freedom - Google Patents

Abstract

PURPOSE:To provide a tool with precise desired compliance by considering deflection characteristics of the arm, etc., of a working machine as factors for spring constant, measuring the deflection characteristics under certain specified conditions, and setting the assumed spring constant including the rigidity of working machine on a position/force control device concerned. CONSTITUTION:A working machine having multiple degrees of freedom is operated under certain specific conditions, and the rigidity of the machine is measured by a rigidity measuring means 61 using the force data sensed by a force sensing means 5 and the position data sensed by a position sensing means. With this rigidity value taken into consideration, the spring constant set on a spring constant setting means 70 is corrected by a correction processing part 60 for generation of the desired compliance. This spring constant obtained as the result from correcting shall be set on a spring constant multiplying means 31 of a position/force control device concerned.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a position and force control device for a multi-degree-of-freedom work machine, particularly for a work machine that performs operations such as deburring, grinding, mold polishing, etc. The present invention relates to a position and force control device suitable for performing copying work while pressing a work tool against a workpiece.

[Conventional technology]

Conventionally, for multi-degree-of-freedom work machines such as industrial robots, a virtual compliance control method has been proposed that allows the work machine to perform a predetermined task by controlling position and force (
JP-A-62-35915, etc.). Generally, in a working machine equipped with this control system, a force sensor is disposed on the wrist of the arm, and a working tool is attached via this force sensor.

Then, control parameters such as virtual mass, spring constant, viscous damping coefficient, etc. that determine the motion characteristics of the work tool are set in the control device, and these set values, reference position and orientation commands supplied from the outside, and the reference position and orientation commands applied to the work tool are set. Based on information on the position and posture of the work tool detected from angle information, arm angle information, etc., a speed command value for the work tool is calculated, and this command value is transmitted from the control device to the drive circuit and sent to the work tool. Supplied to the drive unit. As a result, the working tool can move following the shape of the workpiece and perform a predetermined work.

In virtual compliance control, the spring force is calculated based on the deviation between the position where the work tool should move specified by the control device and the detected current position of the work tool, and the virtual spring constant set as a control parameter. Outputs a command value that causes By using the above-mentioned control system realized by software in this way, it is possible to give a working tool the motion characteristics as if it were attached to a real mechanical spring mechanism.

[Problem to be solved by the invention]

For example, by applying the control method according to the prior art to an articulated robot to control its position and force, it is possible to perform a predetermined task while pressing a work tool attached to the end of the arm against a workpiece.

However, articulated robots have relatively low rigidity, and when a work tool is strongly pressed against a workpiece, the joints of the arms, etc., may flex. According to the above-described virtual compliance control method, by arbitrarily setting the spring constant, it is possible to provide the power tool with appropriate compliance. However, as described above, since the robot itself has flexure in the articulated robot, the spring constant differs from the spring constant set by the control device, resulting in a problem that the desired compliance cannot be obtained.

An object of the present invention is to provide a control device that can control position and force by controlling, for example, virtual compliance control.
Multi-degree-of-freedom work that makes it possible to provide the desired compliance to the work tool by measuring the stiffness from the deflection that occurs in the arm, etc. of the multi-degree-of-freedom working machine that is controlled by this control device and making corrections to the control parameters. The purpose is to provide a machine position and force control device.

[Means to solve the problem]

A position and force control device for a multi-degree-of-freedom work machine according to the present invention includes a force detection means for detecting a force of at least one axis applied to a work tool provided at the end of an arm of a multi-degree-of-freedom work machine; and a spring constant multiplier that virtually generates a spring element. In a position and force control device that controls the position and force of a power tool while imparting spring characteristics to the power tool using a multiplier, multi-degree-of-freedom control is performed using force data detected by the force detection device and position data detected by the position detection device. a stiffness measuring means for measuring the stiffness determined by a portion of a working machine that causes deflection; a spring constant setting means for setting a spring constant that produces a desired compliance;
A spring constant for achieving the compliance is calculated using the stiffness of the multi-degree-of-freedom working machine obtained by the stiffness measuring means and the spring constant set in the spring constant setting means, and this spring constant value is used as the spring constant. The feature is that the spring constant calculation means is configured to be set in the multiplication means.

Another apparatus for controlling the position and force of a multi-degree-of-freedom work machine according to the present invention has the basic device configuration described above, and includes a storage means for storing a plurality of stiffness values measured by a stiffness measurement means, and a storage means for storing a plurality of stiffness values measured by a stiffness measuring means. stiffness switching means for switching any one stiffness value given to the spring constant calculation means from among the plurality of stiffness values stored in the spring constant calculation means; and controlling the switching operation of the stiffness switching means based on position data from the position detection means. stiffness selection means, the stiffness measuring means measures the stiffness value for each predetermined partial work range obtained by dividing the series of work ranges, and the storage means stores the stiffness values of each of the plurality of partial work ranges,
When performing work on a series of work ranges, the stiffness selection means determines each partial work range, switches the stiffness switching means to a stiffness value corresponding to the determined partial work range, and provides the stiffness value to the spring constant calculation means. The characteristic point is that it commands.

[Effect]

In the position and force control device according to the present invention, a multi-degree-of-freedom working machine is operated under predetermined conditions, and the rigidity is thereby determined using force data detected by the force detection means and position data detected by the position detection means. The stiffness of the multi-degree-of-freedom work machine is measured by the measuring means, and the spring constant set in the spring constant setting means is further corrected so as to generate the desired compliance, taking this stiffness into account. do. The spring constant obtained as a result of correction is
It is set in the spring constant multiplier of the position and force control device.

In a multi-degree-of-freedom work machine, if its posture changes significantly, its rigidity also changes, so if the work range is large, divide the work range into a plurality of predetermined partial work ranges in advance, and perform each partial work. The stiffness is determined for each range, and when the work is executed, the stiffness selection means determines the partial work range, and a command is issued to the stiffness switching means to take out the corresponding stiffness value.

〔Example〕

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

FIG. 1 shows the overall configuration of a position and force control device according to the present invention, which is a multi-degree-of-freedom working machine and is applied to a robot that performs a specific task. In FIG. 1, reference numeral 1 is a robot body taught to perform a predetermined work, and an arm 2 having a plurality of joints is attached to a support base 3, and the movement of each joint of the arm 2 allows the arm to The tip moves to the position required for the work, and the posture of the entire arm changes to the position required for the work. A six-axis force sensor 5 is attached to the wrist part 4 located at the tip of the arm 2, and a work tool 7, which is a hand effector for performing a predetermined operation on the workpiece 6, is attached to the tip of the force sensor 5. It is being

8 is a controller, and this controller 8 has a built-in control means composed of a computer, etc., and controls the robot body 1 using predetermined calculation formulas for controlling position and force and according to a predetermined procedure. It has the function of controlling position and force.

The controller 8 gives a command signal to the robot body 1 through a signal line 9 to instruct the robot body 1 to perform a predetermined operation, and also obtains operation information of the motor etc. from the robot body 1.
They are electrically connected to each other so as to receive a detection signal regarding the force and moment applied to the working tool 7 from the force sensor 5, that is, a force signal 10. Reference numeral 11 denotes an operating device that is carried by the operator and operated as needed, and is provided with a numeric keypad and a plurality of operating switches that can give various commands to the controller 8. By operating the various operation switches of the operating device 11, the operator can, for example, teach working motions, set working conditions, set the force for pressing the working tool 7, set a desired spring constant, etc. .

Next, the configuration of the control device according to the present invention realized by the controller 8 will be explained based on the block diagrams of FIGS. 2 and 3. The control method of the present invention applied to the robot body 1 is realized by software or other hardware circuits using a computer included in the controller 8, and FIGS. The control circuit elements and their relationships are shown in block diagram form for clarity.

In FIG. 2, a plurality of drive motors (not shown) for moving each joint of the arm 2 are built into the robot main body 1. An angle meter 21 for measuring the amount of drive is attached to these drive motors, and the axis angle data of each joint can be obtained by this angle meter 21.

First, the configuration of the position/force control calculation system will be explained.

The force and moment signals output from the force sensor 5 placed at the tip of the arm are sent to the force acting nose 22, and the force acting nose 22 transforms the sensor coordinate system into the reference coordinate system (absolute coordinate system). The force t at the contact point between the workpiece and the work tool 7 is calculated by performing gravity compensation by subtracting the gravity needle of the work tool 7 along with the conversion. Then, this force f is compared with the angle target value fr set by the angle target value setting unit 26 in a force deviation calculation unit 27, and the force deviation Δt is calculated. Further, the angle data of each axis detected by the angle meter 21 mentioned above is sent to the position calculation section 23, and the position and orientation X of the power tool 7 in the absolute coordinate system are calculated by the position calculation section 23 from these angle data. Ru. Then, this position and orientation X are compared with the position target value 7 (r set by the position target value setting unit 24) in the position deviation calculation unit 25, and the position deviation ΔX is determined.

Note that the position target value in the position target value setting section 24 and the angle target value in the angle target value setting section 26 are set based on position information given by teaching or the like.

As mentioned above, the force deviation Δf obtained by the force deviation calculation unit 27
and the positional deviation ΔX regarding the position and orientation obtained by the positional deviation calculation unit 25 are inputted to the position/force control calculation unit 30, respectively.

The position/force control calculation unit 30 includes a spring constant multiplier 31, a subtracter 32, and a characteristic compensation calculation unit 33. The output Δf of the force deviation calculation section 27 is given to a subtracter 32,
The positional deviation ΔX obtained by the positional deviation calculation section 25 is manually input to the spring constant multiplication section 31. In the spring constant multiplier 31, a virtual spring constant matrix K is set. Spring constant multiplier 31
calculates Δλ by multiplying the input positional deviation ΔX by the set virtual spring constant matrix K.

In this embodiment, the values of each axis in the spring constant matrix are not fixed values, but are changed to appropriate values as necessary. A virtual spring constant in the direction in which the work tool 7 is pressed against the workpiece 6 is calculated in the work processing section 50 as described later. Δλ is applied to the output of the spring constant multiplier 31 to a subtracter 32 . The subtracter 32 calculates ΔX to Δf-,
The calculation result of this subtracter 32 is given to a characteristic compensation calculation section 33. The characteristic compensation calculation unit 33 calculates the controller gain K.
c is set, and Δf manually calculated by this Kc
−, ΔX receives control characteristic compensation, and characteristic compensation calculation unit 3
3, the speed command value M is calculated.

The speed command value Ma obtained in this way by the position/force control calculation section 30 is supplied to the drive command section 41, and the speed command value Ma is supplied to the drive command section 41 by the drive command section 41. converted into value.

The converted drive command values are supplied to each drive motor of the robot body 1, and the motors are operated according to these command values.

The work processing unit 50 has a program therein that issues commands to the control device having the above-described configuration to cause the control device to perform actual work on the robot main body worker, and uses the taught commands and data. etc., to operate the control device. In relation to the control device, the work processing section 50 takes in the output t of the force performance nose section 22 and the output X of the position calculation section 23 as input signals. Further, the work processing section 50 includes the angle target value setting section 26, the position target value setting section 24, and the spring constant multiplication section 3.
1 so that each control parameter can be changed and set. In particular, the work processing section 50 includes a stiffness measurement correction processing section 60, which is a main part of the present invention and has a configuration as shown in FIG.

The stiffness measurement correction processing unit 60 shown in FIG. 3 measures the stiffness of the robot body 1 and uses a virtual spring given as a control parameter to obtain desired compliance with a control device that controls position and force. It has a function of calculating a constant and changing the virtual spring constant K set in the spring constant multiplier 31 to an optimal value. The stiffness measurement correction processing section 60
A stiffness measuring section 61 into which each output signal of the force calculating section 22 and the position calculating section 23 is input, a storage section 62 storing the measured stiffness value measured and output by the stiffness measuring section 61, and a spring constant calculating section. 63.

The spring constant calculation section 63 is given a spring constant value given from a spring constant setting section 70 in which a target spring constant is set in advance, and the measured stiffness value stored in the storage section 62. A spring constant that takes into account the rigidity of the robot body 1 is determined by these values, and this value is set in the spring constant multiplier 31.

Next, a correction operation by the stiffness measurement correction processing section 60 having the above configuration will be explained. First, the control device performs control so that the work tool 7 of the robot body 1 is pressed against the workpiece 6 twice with different forces. The pressing direction at this time is the i direction, and the first pressing is performed by applying the force applied to the working tool 7 detected by the force acting nose part 22 during operation to f, II,
The position of the work tool 7 detected by the position calculation unit 23 is set as Xti, and the force applied to the work tool 7 detected by the force acting nose part 22 during the second pressing is 112, and the work tool detected by the position calculation unit 23 is Let the position of 7 be XI2. These displacement data and position data are input to the stiffness measuring section 61.

The rigidity measurement unit 61 uses these force and position data to determine the rigidity of the robot body 1, for example, using the following equation.

The stiffness thus determined by the stiffness measurement section 61 is stored in the storage section 62. Further, the spring constant setting section 70 is set to a spring constant for generating a desired compliance in the power tool 7. Although a storage section 62 is provided in FIG. 3, the storage section 62 is not necessary if the obtained stiffness value is immediately used for spring constant calculation.

The spring elements of the entire control system including the rigidity of the robot body 1 are shown in FIG. 4.

In FIG. 4, the spring constant set in the spring constant setting unit 70 is given as a model in which the spring constant existing as rigidity in the robot body 1 and the spring constant Ki given as a control parameter are coupled in series. It will be done. In this model, since the spring constant +t is a unique constant value, it is necessary to adjust Ki in order to obtain the desired spring constant. Therefore, in the spring constant calculating section 63, based on the values of 5 stored in the storage section 62 and 5 set in the spring constant setting section 70, the following calculation is performed so that the desired compliance can be obtained for the power tool 7. A virtual spring constant Ki, which is a control parameter, is determined by the formula.

By substituting the thus obtained virtual spring constant Ki into the virtual spring constant matrix K of the spring constant multiplier 31, the power tool 7 can be controlled with desired compliance.

5 and 6 show other embodiments of the invention.

In this embodiment, the stiffness is measured in consideration of the fact that the stiffness of the robot main body 1 differs depending on the posture taken by the robot main body 1, and the virtual spring constant is corrected. Figure 5 is a diagram that considers how to determine the rigidity according to the posture of the robot body 1, and in particular, the difference in rigidity depending on the posture is divided into cases where the working range is small (a) and cases where the working range is large (b). It shows.

As shown in Fig. 5(a), if the working range is between 11 points and 22 points and this working range is small, the position data can be corrected using any fixed stiffness value measured. good. In other words, if the difference between the stiffness of the robot body 1 at the 11th point and the stiffness at the 22nd point can be ignored, for example, the stiffness at the 11th point Kl, the stiffness at the 22nd point, etc. 1. .

Either the stiffness Kll□ at an arbitrary position between the 11th point and the 22nd point, or the average value KR12' of each stiffness at the 11th point and the 22nd point is measured by the stiffness measurement unit 61 as a representative value of stiffness,
It can be used to calculate virtual spring constants.

On the other hand, as shown in FIG. 5(b), if the work range is a series from 11 points to 43 points and the work range is wide and the posture of the robot body 1 changes greatly, the robot in the work range The rigidity of the main body in one posture cannot be representative of the rigidity of the entire working range. When performing such work, a series of work ranges is prepared in advance into a plurality of partial work ranges, for example P, ~P2. P2-P3. P3-P4. P4~P,
□ (j=
1, 2, 3...) and store them. Then, when the robot main body 1 performs an operation to start work in the work ranges P, -P, a virtual spring constant is calculated while switching the stiffness measured for each divided work range.

FIG. 6 shows an example of the configuration of the stiffness measurement correction processing section considering the case of FIG. 5(b). In the stiffness measurement correction processing section 60' shown in FIG. 6, in the configuration shown in FIG. Switching section 66 and position calculation section 23
The rigidity switching section 66
A stiffness selection unit 65 is connected to output a selection command, and the other configurations are basically the same as the configuration shown in FIG. 3. However, as described above, the stiffness measurement section measures the robot body 1 for each divided work range using each output signal input from the force acting nose section 22 and the position calculation section 23.
Since it has the function of measuring the stiffness of
j represents the divided partial work range), so
The reference numeral 62' is attached thereto.

In the above configuration, the rigidity selection section 65
The divided working range in which the working tool 7 exists is determined based on the position data from 3, and a signal specifying the measured stiffness value KR, required in each working range is output. Further, the stiffness switching section 66 inputs the designation signal from the stiffness selection section 65, and inputs the corresponding value from □ into the stiffness value stored in the storage section 62' based on this designation signal, and changes the rigidity of the robot body 1. Output as stiffness value. The operations of the other components are the same as those described in the embodiment of FIG. 3 above.

According to the above configuration and operation, even when the posture of the robot body 1 described in FIG. 5(b) changes significantly and the rigidity changes greatly, appropriate control can be performed in response to the change in rigidity. can.

〔Effect of the invention〕

As is clear from the above description, according to the present invention, the deflection characteristics of the arm of a working machine, etc. are taken into consideration as a spring constant element, this is measured under predetermined conditions, and the position and force control device is used to perform work. Since the virtual spring constant is configured to include the rigidity of the machine, the desired compliance can be accurately given to the work tool, and appropriate pressing force against the work can be ensured. In addition, the rigidity may differ when the posture of the working machine differs, so in this case, a series of work ranges is divided into parts and the stiffness is determined for each partial work range, allowing more precise operation control. I can do it.

[Brief explanation of drawings]

FIG. 1 is a system configuration diagram of a multi-degree-of-freedom working machine to which the position and force control device according to the present invention is applied, and FIG. 2 is a block diagram showing the overall configuration of the position and force control device according to the present invention.
Figure 3 is a block diagram showing the configuration of the main parts of the control device, Figure 4 is a schematic diagram showing the relationship between the spring constant indicating rigidity and the set spring constant, and Figure 5 is a block diagram showing the relationship between the spring constant indicating rigidity and the set spring constant. Diagram 6 to explain that stiffness varies depending on changes.
The figure is a block diagram showing another embodiment of the stiffness measurement correction processing section. [Explanation of symbols] 1...Robot body 2...Arm 5...Force sensor 6...Work 7...Work tool 22. ... Force acting nose section 23 ... Position calculation section 24 ... Position target value setting section 25 ... 30 ... 31 ... 50 ... 60, 60'61.61' 63 ・ ・ ・ ・ 65 ・ ・ ・ ・ 66 ・ ・ ・ ・ 70 ・ ・ ・ ・

Claims (2)

[Claims] (1) force detection means for detecting a force of at least one axis applied to a work tool provided at the end of an arm of a multi-degree-of-freedom work machine; and a position detection means for detecting the position of the work tool;
a spring constant multiplier that virtually generates a spring element; In the position and force control device that controls the position and force of the work tool while giving spring characteristics, the multi-degree-of-freedom work is performed using force data detected by the force detection means and position data detected by the position detection means. a stiffness measuring means for measuring stiffness determined by a portion of the machine that causes deflection; a spring constant setting means for setting a spring constant that produces a desired compliance; and the multi-degree-of-freedom working machine obtained by the stiffness measuring means. and a spring constant calculation means for calculating a spring constant for achieving the compliance using the stiffness of the spring constant and the spring constant set in the spring constant setting means, and setting this spring constant value in the spring constant multiplication means. A position and force control device for a multi-degree-of-freedom working machine, characterized by comprising: (2) The position and force control device for a multi-degree-of-freedom working machine according to claim 1, further comprising a storage means for storing a plurality of stiffness values measured by the stiffness measuring means, and a plurality of stiffness values stored in the storage means. Rigidity switching means for switching any one of the stiffness values given to the spring constant calculation means;
stiffness selection means for controlling switching operation of the stiffness switching means based on position data from the position detection means;
The stiffness measuring means measures the stiffness value of each predetermined partial work range obtained by dividing the series of work ranges, and the storage means stores the stiffness values of each of the plurality of partial work ranges, and When performing work on the range, the stiffness selection means determines each of the partial work ranges, and causes the stiffness switching means to switch to a stiffness value corresponding to the determined partial work range and provide the stiffness value to the spring constant calculation means. A position and force control device for a multi-degree-of-freedom working machine, which is characterized by commanding the following: