JPH10151590A - Manipulator force control device - Google Patents

Abstract

(57) [Summary] [PROBLEMS] To enable stable contact even in a highly rigid environment. A first compliance control means has an impedance model added to a position higher than a position or velocity control system, and a second compliance control means has an impedance model added to a force control system which directly controls a force applied to a hand of a manipulator.
A compliance control means, and an index representing the stability of the first compliance control means as a function of an impedance parameter consisting of a mass, a damper, and a spring, and a means for obtaining the index; 1
And a means for smoothly switching between the second compliance control means and the second compliance control means.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator force control device for an operation involving contact between a manipulator and an environment.

[0002]

2. Description of the Related Art Conventionally, a compliance control method has been developed as a force control method for a manipulator accompanied by contact with the environment. The compliance control system realizes control by software in which the dynamic characteristics of the manipulator have desired mechanical impedance (mass, damper, spring). As one of the compliance control systems, there is a system in which a position control system or a speed control system is configured in a manipulator, and an impedance model is added to a higher level (hereinafter, referred to as a position-based or speed-based compliance control system). For example, the method of Furuta and Kosuge et al. (Transactions of the Society of Instrument and Control Engineers, Vo
l.24, No. 1, 1988) is a position-based compliance control method. The method of Hirabayashi, Owada, Sugimoto et al. (Japanese Patent Publication No. 5-43122) is a speed-based compliance control method. The impedance model of the position-based compliance control method is based on a force vector F detected by a force (including torque) detecting means.

[0003]

(Equation 1)

Accordingly, a correction amount vector δP of the hand target position (including posture) vector is calculated. A vector obtained by adding δP to the hand target position vector is given as a command value of the position control system. Here, Fd is a target value of the force vector applied to the hand. M, D, and K are positive definite symmetric matrices representing a mass, a damper, and a spring, respectively.

[0005]

In the conventional position-based or speed-based compliance control system, when a hand comes into contact with a highly rigid environment, the impedance model is stabilized due to a phase delay of the position control system or the speed control system. If the margin is set small, there is a problem that the contact becomes unstable. Accordingly, an object of the present invention is to solve the above-described problem of the position-based or speed-based compliance control system and to realize a manipulator force control device capable of performing stable contact even in a highly rigid environment.

[0006]

SUMMARY OF THE INVENTION In order to solve the above-mentioned problems, the present invention relates to a force control device for a manipulator, wherein first compliance control means in which an impedance model is added to an upper position of a position or speed control system; Second compliance control means in which an impedance model is added to a higher order of a force control system for directly controlling the force applied to the force, and an index representing the stability of the first compliance control means as an impedance parameter comprising a mass, a damper, and a spring. A function for determining the index, and a means for smoothly switching between the first compliance control means and the second compliance control means in accordance with the value of the index. is there.

[0007]

DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to FIGS. FIG. 1 shows a control block diagram of a force control system according to the present invention. In FIG. 1, reference numeral 1 denotes the dynamics of a manipulator to be controlled. The input is a joint driving torque vector τ, and the output is a hand position (including posture) vector P. Reference numeral 2 denotes a force (including torque) detecting means for detecting an external force vector F applied to the hand. 3 represents forward kinematics transformation,
The position vector P of the hand is calculated using the displacement vector θ of the joint. Reference numeral 4 denotes an impedance model of a position-based compliance control method, and calculates a correction amount vector δP of the hand target position vector Pd by equation (1). Note that the differential in equation (1) is calculated by the backward difference. Reference numeral 5 denotes a position controller, which performs a proportional / differential / integral operation. Reference numeral 6 denotes an impedance model of the force-based compliance control method (only the spring K). Reference numeral 7 denotes a force controller, which performs a proportional operation or a proportional / integral operation. 8 is the Jacobi matrix J (θ) of the manipulator
Is used to convert a force command vector UF applied to the manipulator into a torque command vector τ. Reference numerals 9 and 10 select switching between a position-based compliance control method and a force-based compliance control method. In 9 and 10, I is a unit matrix, and S is a selection matrix expressed as S = diag (s ₁ , s ₂ ,..., S _i ,..., S _n ), and 1 ≦ n ≦ 6 (2). Also, 0 ≦ s _i ≦ 1,1 ≦
Let i ≦ 6. When s _{i is set} to 1, the movement direction is subjected to position-based compliance control. s _i is 0
In this case, force-based compliance control is performed for the movement direction. If 0 <s _i <1, an intermediate control between the two is performed. However, F, when selecting a location-based compliance control and power-based compliance control for different coordinate system with P, 9, 10 of FIG. 1 using the coordinate transformation matrix A, ASA ^-1, A
(1-S) A- ¹ .

Here, a manipulator with a speed reducer is considered, and the position control system of FIG. 1 near the contact position is linear and its transfer function matrix can be regarded as a diagonal matrix due to the effect of the speed reducer. The transfer function matrix of the position control system shown in FIG. 1 is expressed as Gp = diag ( _GP1 , _GP2 ,..., _GPi ,..., _GPn ), 1 ≦ n ≦ 6 (3). The impedance parameters M, D, K also diagonal matrix _{M = diag (M 1, M} 2, ..., M i, ..., M n) D = diag (D 1, D 2, ..., D i, ..., D _n ) K = diag (K ₁ , K ₂ ,..., K _i ,..., K _n ), 1 ≦ n ≦ 6 (4) Transfer function matrix Gm = diag the impedance model 4 Figure _{1 (G m1, G m2,} ..., G mi, ..., G mn), 1 ≦ n ≦ 6 (5) G mi = 1 / (M i s 2 + D _i s + K _i) represents the (6). An environmental model is required to determine the stability margin (index indicating the stability) of the compliance control. However, the environment model is generally unknown, and it is difficult to accurately determine the environment model. Further, the unstable contact becomes remarkable when the hand of the manipulator comes into contact with a highly rigid environment. Therefore, the environmental model stiffness _{Ko = diag (K 01, K} 02, ..., K 0i, ..., K 0n), represented by 1 ≦ n ≦ 6 (7) . At this time, the contact system of the position-based compliance control of FIG. 1 can be represented as shown in FIG. In FIG. 3, ΔP and ΔF are the hand position of the manipulator,
It is a small variation of the force applied to the hand. In FIG. 3, reference numerals 11, 12, and 13 denote a transfer function matrix of the impedance model 4, a transfer function matrix of the position control system, and a rigidity matrix of the environment, respectively.

[0009]

First Embodiment Gain margin is considered as an index representing the stability of position-based compliance control. The gain margin g _{Mi in} the i-direction of the contact system in FIG. 3 can be expressed as follows. g _Mi = −20 log ₁₀ | G _pi (ω _o ) G _mi (ω _o ) K _0i | [dB] (8) for ω _o s, t arg ｛G _pi (ω _o ) G _mi (ω _o ) K _0i ｝ ＝
When −π g _Mi becomes 0 [dB] or less, contact in the i direction becomes unstable. Therefore, the threshold value h ₂ > h ₁ > 0 is set for g _Mi , and s
_i is switched according to the value of g _Mi as shown in FIG. here,
The position control system G _pi (s) is approximated by a first-order lag system as G _pi = ω _pi / (s + ω _pi ) (9). ω _pi is the cut-off frequency of G _pi (rad / s). From the equations (6), (8), and (9), the gain margin g _Mi of the equation (8) can be expressed as follows.

[0010]

(Equation 2)

The cutoff frequency of the position control system may be determined by performing some kind of identification test. Alternatively, a theoretical value (ideal value) may be obtained from the inertia of the arm, the gain of the position controller, and the like. The stiffness K _0i of the environment is represented by ΔP and Δ in FIG.
It is possible to extrapolate from F, but generally it is difficult. Here, the exact value of K _0i without using, to estimate the K _0i at larger value. Then, a contact experiment or the like is performed to adjust the thresholds h ₁ and h ₂ of g _Mi. Equation (1
0) The impedance model damper D _i and spring K _i
It can be seen that as the value of is set smaller, the value of the gain margin g _Mi becomes smaller, and the switching from the position-based compliance control to the force-based compliance control is performed.

[0012]

Embodiment 2 Consider a phase margin as an index indicating the stability of position-based compliance control. The phase surplus φ _{Mi in} the i direction of the contact system in FIG. 3 can be expressed as follows. φ _Mi = [π + arg ｛G _pi (ω _o ) G _mi (ω _o ) K _0i ｝] × 180 / π [deg] (11) for ω _os , t | G _pi (ω _o ) G _mi ( ω _o ) K _0i | = 1 When φ _Mi becomes 0 [deg] or less, contact in the i direction becomes unstable. Therefore, in the same manner as in Example 1, the threshold h ₂ to phi _Mi>
When h ₁ > 0 is established, s _i is switched as shown in FIG. 2 according to the value of φ _Mi.

[0013]

Embodiment 3 When g _Mi and φ _Mi > 0, that is, when the contact system in FIG. 3 is stable

[0014]

(Equation 3)

Is the stability of the contact system of FIG. γ_i
Is larger, the margin of stability is larger, and γ_i= 0 is the contact of FIG.
This is the stability limit of the tactile system. Therefore, the same as in the first and second embodiments.
And γ_iTo the threshold h_Two> H₁> 0, s _iTo γ_i
Are switched as shown in FIG.

[0016]

As described above, according to the present invention, an index representing the stability of the compliance control system in which an impedance model is added to a position or speed control system is used as an index of an impedance parameter comprising a mass, a damper, and a spring. Expressed as a function, according to the value of the index, according to the above-mentioned compliance control method, the compliance control method in which an impedance model is added to a higher order of the force control system that directly controls the force applied to the hand of the manipulator, is smoothly switched. There is an effect that a stable contact operation can always be realized. It is also clear that the mapping from the impedance parameter to the index indicating the stability may be used even if the index indicating the stability cannot be explicitly expressed by an expression including the impedance parameter.

[Brief description of the drawings]

FIG. 1 is a control block diagram according to an embodiment of the present invention.

FIG. 2 is a switching function of s _i in the embodiment of the present invention.

FIG. 3 illustrates a contact system for position-based compliance control according to an embodiment of the present invention.

[Explanation of symbols]

1: Dynamics of manipulator 2: Force detection means 3: Forward kinematics conversion 4: Impedance model of position-based compliance control 5: Position controller 6: Impedance model of force-based compliance control (spring only) 7: Force controller 8: Transpose matrix of Jacobian matrix of manipulator 9: Selection matrix of position-based and force-based compliance control 10: Selection matrix of position-based and force-based compliance control 11: Transfer function matrix of impedance model of position-based compliance control 12 ： Transfer function matrix of position control system 13 ： Stiffness matrix of environment

Claims (4)

[Claims] 1. A force control device for a manipulator, comprising: first compliance control means having an impedance model added to a position or speed control system; and impedance to a force control system for directly controlling a force applied to a hand of the manipulator. A second compliance control unit to which a model is added; an index representing the stability of the first compliance control unit as a function of an impedance parameter including a mass, a damper, and a spring; And a means for smoothly switching between the first compliance control means and the second compliance control means in accordance with the following. 2. The manipulator force control device according to claim 1, wherein the index is a gain margin. 3. The force control device for a manipulator according to claim 1, wherein the index is a phase margin. 4. The force control device for a manipulator according to claim 1, wherein the index is a margin of stability.