JPH10309684A - Compliance control method of manipulator - Google Patents

Abstract

PROBLEM TO BE SOLVED: To realize stable and precise contact work by setting off disturbance by using an observer to estimate disturbance caused by non-linear friction, etc., in the inside of a force feedback loop of torque base compliance control. SOLUTION: Torque generated on a joint by a force vector Fext which a terminal of a manipulator gives to an environment is detected by a force torque detection means 218. Inertial force and viscous frictional force of the manipulator is determined by a computing part 215 (nominal values Mjn (q), Djn of an inertial matrix and a viscosity matrix of the manipulator). In addition to these values, non-linear friction of the manipulator is estimated in accordance with a joint speed vector of the manipulator and a low pass filter 216 (a low pass filter matrix Flt (s) to remove a high frequency component). Thereafter, force torque compliance control is constituted by regarding a difference of the non-linear friction estimated as control input uj as new control input.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator compliance control system in an operation involving contact between a manipulator and the 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 method realizes, by software, a control in which the dynamic characteristics of the manipulator with respect to an external force at the hand have a desired mechanical impedance (inertia, viscosity, rigidity). As one of the compliance control methods, a position (speed) control system is configured on the manipulator, an impedance model is added to the upper position, and the position (speed) is set to the lower position (speed) control system using the impedance model. There is a method to give instructions. For example, in the literature `` Transactions of the Society of Instrument and Control Engineers, Vol.
55/62, 1988 "is a position-based compliance control method (hereinafter referred to as position-based compliance control). In addition,
The system disclosed in Japanese Patent No. 43744 is a speed-based compliance control system (hereinafter, referred to as speed-based compliance control).

The hand position vector of the manipulator is represented by P =
[p1, p2, ..., pi, ..., pn] '(n <= 6) and the target value vector is Pref = [pr1, pr2, ..., pri, ..., prn]' , The force vector given to the environment by the manipulator hand is fe = [fe1, fe
2, ..., fei, ..., fen] '
Compliance control is based on fe obtained by force detecting means or estimating means attached to the tip of the manipulator, P detected by position detecting means of the manipulator, and Pref inside the computer,

[0004]

(Equation 1)

[0005] A position (speed) vector of the manipulator that satisfies the impedance characteristic equation is calculated, and the calculated value is given to a lower position (speed) control system as a position (speed) command value vector. As described above, there are various types of impedance characteristic expressions as shown in Expressions (1) to (3). Equation (1)-
In (3), Md, Dd, and Kd are positive definite symmetric matrices representing inertia, viscosity, and rigidity, respectively. On the other hand, the method found in the document “Robot Engineering Handbook p.254, Robotics Society of Japan, 1990, Corona” differs from the position or velocity-based compliance control method in that feedback control of force and torque is first performed on the manipulator. Force torque-based compliance control that adjusts the virtual inertia, viscosity, and stiffness (virtual inertia, virtual viscosity, virtual stiffness) of the manipulator by configuring a feedback loop for position, velocity, acceleration, etc. (Hereinafter referred to as force torque based compliance control).

The equation of motion of the manipulator expressed in the work space is expressed as follows.

[0007]

(Equation 2)

Q = [q1, q2, ..., qi, ..., qn] 'is a joint variable vector, and uf = [uf1, uf2, ..., ufi, ..., ufn]' is a control This is an input vector. M (q) and D (q) are a manipulator inertia matrix and a viscous matrix, respectively. S (q, dq / dt) is a term representing the gravity of the manipulator, the centrifugal force / Coriolis force between the joints, and the nonlinear friction force. The control input uf represents a force (moment) generated at the manipulator hand by the force (torque) generated by the actuator provided at each joint. That is, the joint driving force (torque) is expressed as uj = [uj1, uj
2, ..., uji, ..., ujn] ', then uj = J (q)' uf (5). Here, J (q) is the Jacobian matrix of the manipulator. With force torque based compliance control,
First, the following force torque feedback control is configured.

Uf = fcmd + Kf (fcmd-fe) (6) Here, fcmd and Kf are a force command value vector and a force feedback gain matrix, respectively. From equations (4) and (6), the response of the manipulator to fcmd is

[0010]

(Equation 3)

## EQU1 ## Here, I represents a unit matrix. In addition, the force command fcmd is given by feedback of position, speed and acceleration.

[0012]

(Equation 4)

From the equations (7) and (8), the response of the manipulator to the external force -fe applied to the hand of the manipulator is:

[0014]

(Equation 5)

## EQU1 ## By adjusting the force feedback gain matrix Kf, the velocity feedback gain matrix Dv, and the acceleration feedback gain matrix Ma from Expression (9), the virtual inertia and the virtual viscosity of the manipulator can be set to some extent, though not arbitrarily. The virtual stiffness can be arbitrarily set by the position feedback gain matrix Kd.

[0016]

The position and speed based compliance control is based on a position and speed control system, so that the position control accuracy is good. However, when the tip of the manipulator comes into contact with a highly rigid environment, there is a problem that if the virtual viscosity of the impedance model is set small due to the phase delay of the position / speed control system, the contact becomes unstable. On the other hand, in the force torque based compliance control, the contact becomes stable with respect to a smaller virtual viscosity than in the position and velocity based compliance control. However, there is a problem that the position control accuracy is poor due to the effects of nonlinear friction and gravity. In the case of a robot manipulator in which the actuator section and the link section are connected via a power transmission mechanism such as a speed reducer, the contact stability of the compliance control system deteriorates due to the mechanical resonance of the manipulator caused by the rigidity of the speed reducer. , Conventional force torque base
There is a problem that sufficient contact stability cannot be guaranteed even in compliance control. Therefore, the conventional compliance control method has a problem that it is not possible to realize a good compliance control system in both the contact stability and the position control accuracy.

[0017]

SUMMARY OF THE INVENTION In order to solve the above-mentioned problems of the prior art, the present invention provides a manipulator with feedback control of force and torque. In the force torque based compliance control method characterized by comprising control, inside the force torque feedback loop,
The present invention is characterized in that a compensator for canceling the above-mentioned disturbance is added by using an observer for estimating an equivalent disturbance due to nonlinear friction, the gravity of the manipulator, and the interference force between the joints. The present invention also has means for measuring or estimating the position, velocity, acceleration signal and external force applied to the tip of the manipulator, and applying the movement of the manipulator tip to the position, velocity, acceleration signal and the tip of the manipulator. External force signal, virtual inertia, virtual viscosity to set the value according to the work,
In a compliance control method that performs control so as to match a motion calculated from an impedance characteristic expression expressed based on virtual stiffness, by configuring a sliding mode control in which the impedance characteristic expression is a hyperplane (switching plane), It is characterized by realizing a virtual impedance at the tip of the manipulator.

Further, the present invention is a control system for a robot manipulator in which an actuator section and a link section are connected via a power transmission mechanism having a finite rigidity, and the position, speed, position of the link section, speed,
A means for detecting or estimating acceleration and an external force signal applied to the tip of the manipulator, and setting the value of the motion of the tip of the manipulator in accordance with the position, speed, acceleration and the external force signal applied to the tip, and the value according to the work; inertia,
In the vibrance control system that performs control to match the motion calculated from the impedance characteristic formula based on virtual viscosity and virtual stiffness, the deviation between the position of the actuator unit and the position of the link unit is applied to the robot manipulator. It is characterized in that control is performed to converge the new external force torque command to one-stiffness of the power transmission mechanism.

[0019]

Embodiments of the present invention will be described below with reference to the drawings. First, a robot manipulator having a speed reducer will be briefly described. FIG. 3A is a schematic diagram of a one-degree-of-freedom robot manipulator in which an actuator unit and a link unit are connected by a speed reducer. In the figure, 1 is an actuator, 2 is a link,
3 is an end effector, 4 is a speed reducer, 5 is a position and speed detector of an actuator unit, 6 is a position and speed of a link unit,
The acceleration detector 7 is a detector for detecting an external force applied to the end effector. Due to the rigidity of the speed reducer 4, the actuator rotation angle and the link rotation angle after deceleration are twisted. Here, as shown in FIG. 3B, the end effector 3 is
Consider the case of contact with. However, the stator of the actuator 1 and the environment 8 are assumed to be fixed in the working space. At this time, the motion of the one-degree-of-freedom robot manipulator near the contact point can be modeled as shown in FIG.

In FIG. 5, reference numeral 21 denotes an inertia Mm of the actuator section, 22 denotes a viscous friction coefficient Dm of the actuator section, 23
Is the compensation Ml of the link, and 24 is the viscous friction coefficient D of the link
l, 25 is the reduction gear rigidity Kg, 26 is the environmental rigidity Ko, 27
Is the actuator generating force uf, 28 is the end effector 3
Represents the force fe applied to the environment 8. Here, the position of the actuator unit, the minute displacement of the end effector calculated from the speed detector 5 is xm, and the position, the speed of the link unit, and the minute displacement of the end effector obtained from the acceleration detector 6 are xm.
If xl is set, the model in FIG. 5 is represented by the following equation.

[0021]

(Equation 6)

If the contact point between the end effector 3 and the environment 8 is the origin of the small displacement xm, xl, fe = Ko xl (12) holds. (Embodiment 1) Next, the configuration of a control system according to an embodiment of the present invention will be described. Let the deviation between the two small displacements xm and xl be ex = xm-xl (13). Here, the deviation ex corresponds to a new control input v.
Hyperplane W = 0

[0023]

(Equation 7)

The control input uf is designed so that the following sliding mode occurs. Where C is a positive scalar constant. In equation (14), a hyperplane is set such that the deviation ex between the position xm of the actuator unit and the position xl of the link unit converges to 1 / (1 / Kg) of the rigidity of the power transmission mechanism for the new external torque command v. doing. For the one-degree-of-freedom robot manipulator when the sliding mode occurs,
The responses of Eqs. (11) and (14) are

[0025]

(Equation 8)

## EQU1 ## Equation (16) is Laplace transformed

[0027]

(Equation 9)

## EQU1 ## In a frequency band sufficiently lower than C [rad / s], ex = v / Kg, that is, xm = xl + v / Kg (18) holds. The response of the one-degree-of-freedom robot manipulator to a new control input v is as follows from Expressions (15) and (17).

[0029]

(Equation 10)

Here, V is expressed as V = Kf (fcmd-fe) + fcmd (20)

[0031]

[Equation 11]

As described above, the position of the link portion outside the external force feedback applied to the tip of the manipulator, and the force torque command in the force torque base compliance control constituting the speed feedback. Where Kf, Dv, Kd
Are the force feedback gain, velocity feedback gain, and position feedback gain, respectively. Equation (19),
From (20) and (21), the response of the one-degree-of-freedom robot manipulator to the external force -fe (see Fig. 5) is

[0033]

(Equation 12)

This indicates that the mechanical resonance of the robot manipulator is suppressed. Equation (22) is C
In the frequency band sufficiently lower than [rad / s],

[0035]

(Equation 13)

## EQU1 ## Here, xref is the target trajectory of the minute displacement xl of the end effector. Equation (23) is the impedance characteristic equation itself. From Equations (12) and (22), xref = 0
Then, a block diagram of the contact system is represented as shown in FIG. In FIG. 4, reference numeral 11 denotes a block representing the response equation (22) of the one-degree-of-freedom manipulator, and reference numeral 12 denotes a block representing the rigidity Ko of the environment. The necessary and sufficient conditions for stabilizing the contact system in FIG.

[0037]

[Equation 14]

## EQU4 ## If the rigidity of the environment Ko is finite, C
It can be seen that increasing the value of satisfies the equation (24) for the contact stability condition. By the above means, the force torque-based compliance control that suppresses the mechanical resonance of the manipulator due to the rigidity of the power transmission mechanism such as the reduction gear is configured, and the contact stability is higher than the conventional compliance control system. Good and practical compliance control can be realized. Next, a control input uf for generating a sliding mode in which the expression (14) is a hyperplane is obtained. In addition,
FIG. 1 is a block diagram of the entire control system. Equations (10), (11),
(13), (14), (20), (21)

[0039]

(Equation 15)

In other words, W dW / dt is as follows.

[0041]

(Equation 16)

Therefore, he <Kg (1 + Mm / Ml) -C Mm (1 + Kf) Dv / Ml if W ex> 0 he> Kg (1 + Mm / Ml) -C Mm (1 + Kf) Dv / Ml if W ex <0

[0043]

[Equation 17]

By switching each gain of the equation (25) so as to satisfy the condition, W dW / dt <0 is satisfied, and a sliding mode is generated. FIG. 1 is a block diagram of the entire control system. In FIG. 1, 101 is the dynamics (10) and (11) of the robot manipulator, and 102 is the rigidity of the environment. 103 is a position feedback gain (virtual rigidity) Kd, 104 is a velocity feedback gain (gain for adjusting virtual viscosity) Dv, 105 is a force feedback gain Kf, and 106 is a differential calculator. 107 is a hyperplane
The calculation unit 108a in (14) is a feedback gain switching calculation unit based on equation (27). 109, 110, 11
2, 113 and 113a are ex, dxm / dt, dxl / dt,
This is the feedback gain of dfe / dt, fe. 111 is dx
This is the feed forward gain of ref / dt. 114, 1
Reference numerals 15, 116, 117, and 118 denote detectors for an external force applied to the tip of the robot manipulator, a detector for the position xm of the actuator unit, a detector for the speed dxm / dt of the actuator unit, a detector for the position xl of the link unit, and the link unit, respectively. Speed dx
l / dt detector.

The hyperplane calculation unit 107 calculates the value of W based on the value of V and equation (14). The switching operation unit 108a has W
Based on the value and equation (27), each feedback gain 109-
Change the values of 113 and 113a. If the acceleration of the link can be detected, the control input uf

[0046]

(Equation 18)

In other words, W dW / dt is

[0048]

[Equation 19]

He <Kg if W ex> 0 he> Kg if W ex <0

[0050]

(Equation 20)

It suffices to switch the respective gains in the equation (28) so as to satisfy the above condition. A block diagram of the entire control system at this time is as shown in FIG. From the equations (29) and (30), it is possible to configure a control system by using the acceleration signal of the link portion without knowing the values of the inertia Ml and the viscous friction coefficient Dl of the link portion. In FIG. 2, reference numerals 108b, 112b, and 119 denote a gain switching calculator, a feedback gain of the acceleration of the link unit, and a detector of the acceleration of the link unit based on the equation (30), respectively. The other elements are exactly the same as in FIG. In addition, although the embodiment has been described with respect to the one-degree-of-freedom robot manipulator, the control system can be configured in exactly the same manner for a multi-degree-of-freedom robot manipulator. (Embodiment 2) Embodiment 2 of the present invention will be described.

The equation of motion of the manipulator in the joint space is

[0053]

(Equation 21)

I will put it. Here, Mj (q) and Dj are an inertia matrix and a viscous matrix of the manipulator in the joint space, respectively. Tf
ric is the nonlinear friction vector in the joint space, Text is the torque generated at the joint by the force vector (including torque) Fext given to the environment by the manipulator's hand, and Tfric = J (q) 'Ffric, Text = J (q) 'Fext (32) holds. The nominal values (nominal values: obtained by parameter identification) of the inertia matrix Mj (q) and the viscosity matrix Dj are Mjn (q),
Djn. Also, let the joint velocity vector of the manipulator be y = dq / dt. From the equation (31), the non-linear friction of the manipulator is estimated as follows. ＾ Tfric (s) = Flt (s) [uj (s)-Text (s)-｛Mjn (q) s + Djn｝ Y (s)] (33) where ＾ represents an estimated value . Flt (s) is a low-pass filter matrix that removes high-frequency components of the estimated value.

The nonlinear friction ＾ Tfric estimated from equation (33)
Using (s), the control input uj is configured as follows. uj (s) = ￣uj (s) + ＾ Tfric (s) (34) From Equations (33) and (34), the nonlinear friction compensator using the disturbance observer has a configuration as shown in FIG. In FIG. 6, reference numeral 215 denotes a calculation unit for calculating the inertial force and viscous friction force of the manipulator, 216 denotes a low-pass filter, 217 denotes a transpose of a Jacobian matrix, 218 denotes a force torque applied to the manipulator hand, and 219 denotes a joint angular velocity detection means of the manipulator. , 220 are drivers for controlling the torque of the joint actuator, and 221 is the dynamics of the manipulator.

Here, ￣uj in equation (34) is regarded as a new control input, and the above-described force torque base compliance control is configured. From Equations (33) and (34), the control input uj is

[0057]

(Equation 22)

## EQU5 ## ￣uf = Fcmd + Kf (Fcmd-Fext) (36)

[0059]

(Equation 23)

The configuration is as follows. ￣uj is constructed as 式 uj = J (q) '￣uf (38) using the Jacobian matrix of the manipulator in the same manner as in equation (5). Low-pass filter matrix

[0061]

(Equation 24)

From the equations (35) to (39), a block diagram of a force torque based compliance control system having a nonlinear friction compensator with a disturbance observer therein is shown in FIG.
It is represented as FIG. 7 is a control block diagram according to the second embodiment of the present invention. In FIG. 7, 201, 202,
203 is a position feedback gain, a speed feedback gain, and a force feedback gain, respectively.
Reference numerals 204, 205, and 206 denote a transpose matrix of the Jacobian matrix of the manipulator, a Jacobian matrix, and a forward kinematics transformation matrix of the manipulator, respectively. 207, 208, 2
Reference numeral 09 denotes a proportional integrator for manipulator joint driving force command, an integrator for joint torque due to external force, and a calculator for calculating integrated values of inertia force and viscous friction force of the manipulator. 210, 211, 212, 213 and 214 are force torque detecting means applied to the manipulator hand, manipulator joint angular velocity detecting means, manipulator joint angle detecting means, manipulator joint actuator driver (torque controller), and manipulator dynamics, respectively. is there. Reference numerals 207 to 209 in FIG. 7 correspond to the disturbance torque compensator in FIG. However, in the control system of FIG. 7, the acceleration feedback gain is a zero matrix. Mj (q) changes depending on the position and attitude of the manipulator.In industrial manipulators, the diagonal element of the inertia matrix becomes dominant over the off-diagonal element due to the effect of the reduction gear. The amount of calculation can be reduced by approximating an angular matrix. Also,
Mjn (q) may be linearized and approximated at a certain position and orientation, and an equivalent disturbance compensator may be configured as a constant matrix. (Embodiment 3) In Embodiment 2, the compensator for the non-linear friction and the gravitational term by the disturbance observer is configured in the joint space, but the compensator may be configured in the work space in the same manner.

The inertia matrix in the workspace of the manipulator,
The nominal value of the viscosity matrix Mn (q), Dn (q) is calculated by the inertia matrix in the joint space of the manipulator, the nominal value of the viscosity matrix Mjn (q), Djn.

[0064]

(Equation 25)

Is expressed as follows.非 線形 Ffric (s) = Flt (s) [uf (s)-Fext (s)-｛Mn (q) s + Dn (q)｝ Z ( s)] (41) uf (s) = ￣uf (s) + ＾ Ffric (s) (42), and ￣uf may be used as a new control input to configure force torque based compliance control. However,
z is z = dX / dt (manipulator velocity vector in the workspace). As in the second embodiment, when the deceleration effect of the manipulator is large and the isotropy in the work space is good,
It is also possible to obtain Mn (q) and Dn (q) by linear approximation at a certain position and orientation, and configure an equivalent disturbance compensator as a constant matrix. (Embodiment 4) W = 0 in sliding mode control
To

[0066]

(Equation 26)

The virtual viscosity matrix Dd and the virtual stiffness matrix Kd are represented by the following equation. The diagonal matrix Dd = diag [dd1, dd2,..., Ddi,..., Ddn] (44) Kd = diag [kd1, kd2, ..., kdi, ..., kdn] (45). ￣fe filters fe by the low-pass filter determinant Flt = diag [1 / (t1 s + 1), 1 / (t2 s + 1), ..., 1 / (tn s + 1)] (46) It is the force vector that was obtained. However, the cut-off frequency of the low-pass filter is set sufficiently higher than kdi / ddi. When d (W'W) / dt <0 (47) always holds along the trajectory of the manipulator, a sliding mode occurs, and the trajectory of the manipulator converges to the hyperplane W = 0 and is constrained there. The control law is determined so as to satisfy Expression (47).

First, W is divided into n hyperplanes as follows. W = [w1, w2, ..., wi, ..., wn] '(48)

[0069]

[Equation 27]

At this time, d (W'W) / dt is

[0071]

[Equation 28]

Can be expressed as follows. Calculating dW / dt from equations (4) and (43) gives the following.

[0073]

(Equation 29)

If the control input uf is uf = M (q) ＾ uf + fe (52) ＾ uf = [＾ uf1, ＾ uf2, ..., ＾ ufi, ..., ＾ ufn] ', then dW / dt is diagonalized with respect to the new control input ＾ uf. At this time, dwi / dt is

[0075]

[Equation 30]

Is obtained. New control input ufi

[0077]

(Equation 31)

From the equations (53) and (54),

[0079]

(Equation 32)

Holds. Therefore,

[0081]

[Equation 33]

By switching each gain in the equation (54) so as to satisfy the condition, widwi / dt <0 holds. At this time, Expression (47) is established from Expression (50), and the motion of the manipulator is completely constrained by the hyperplane (impedance characteristic expression). When the motion of the manipulator is constrained to the hyperplane, contact stability is always guaranteed. In other words, the feedback of the differential value of the external force signal ￣fe guarantees the contact stability, that is, the convergence to the hyperplane when in contact with the environment. The inertia matrix M (q) of the manipulator in the work space is calculated using the inertia matrix Mj (q) in the joint space.

[0083]

(Equation 34)

Thus, the joint driving force uj in equation (5) may be configured as follows using the control input ＾ uf.

[0085]

(Equation 35)

From equation (54), ＾ uf

[0087]

[Equation 36]

In particular, one configuration example of the control system can be represented as a block diagram in FIG. 8, reference numeral 301 denotes an impedance parameter setting unit for virtual viscosity Dd and virtual rigidity Kd.
Reference numeral 302 denotes a hyperplane operation unit of Expression (43), and reference numeral 303 denotes a feedback gain switching operation unit expressed by Expression (56). When the impedance parameters Dd and Kd are changed, the calculation units 302 and 303 are also changed. Reference numeral 304 denotes a differential calculator. 305 is the feed forward gain of dPref / dt
Hr, 306 is a feedback gain Hf of d￣fe / dt, 3
07 is a compensation input Hs for nonlinear friction, gravity, and joint interference force,
308 is a feedback gain Hv of dP / dt. 30
Reference numeral 9 denotes a conversion matrix from the manipulator's hand acceleration command ＾ uf found in equation (58) to a joint actuator generated force (torque) command. Reference numeral 310 denotes a transposed matrix of the Jacobian matrix, which converts a force acting on the hand (moment) into a force acting on the joint (torque). Reference numeral 311 denotes a Jacobian matrix, which converts a joint speed dq / dt into a hand speed dP / dt. 312
Is a forward kinematics transformation matrix R (q). 313 is a force moment detecting means, 314 is a driver of a joint actuator, 315 is a joint speed detecting means, and 316 is a joint position detecting means. Numeral 317 denotes the dynamics of the manipulator represented by the equation (4), and numeral 318 denotes the dynamics of the environment where the manipulator contacts. 319 is a low-pass filter. The hyperplane calculation unit 302 calculates the value of W based on equation (43). The switching operation unit 303 changes the value of each of the feedback gains 305 to 308 based on the value of W based on Expression (56).

This method requires a differential value of the external force signal. Since the external force signal detected by the force sensor often includes relatively large noise, a signal ￣fe obtained by filtering the external force signal with a low-pass filter is used. When actually constructing the control system, the nominal values Mn of M (q), D (q), S (q, dq / dt) obtained by parameter identification
(q), Dn (q), Sn (q, dq / dt) are used.

[0090]

(37)

The maximum value and the minimum value can be estimated, and two types of constant gains satisfying the expression (56) can be determined in advance. In a manipulator with a large reduction ratio of the actuator, the diagonal terms of M (q) and D (q) are considerably dominant compared to the off-diagonal terms, so the off-diagonal terms are ignored and hvij, j! = I May be set to zero to further simplify the control law. At this time, it is preferable to increase the absolute value of hsi to increase the robust stability against the uncertainty of the ignored off-diagonal term. When the reduction ratio is large, Mjn (q) can be approximated by a diagonal matrix. Furthermore, it can be approximated as a constant matrix,
In order to guarantee the convergence to the hyperplane, it is necessary to set the absolute value of hsi to a considerably large value, which is conservative. (Example 5) Low-pass filter

[0092]

(38)

For the position ピ ュ P, target position ￣Pref, speed ￣dP / dt, acceleration ￣d2P / dt2, and external force -￣fe of the manipulator,

[0094]

[Equation 39]

Then, the control system is designed so that the sliding mode is generated. The virtual inertia matrix Md is a diagonal matrix Md = diag [md1, md2, ..., mdi, ..., mdn] (63). Whenever a sliding mode occurs and the manipulator motion is constrained to the hyperplane,

[0096]

(Equation 40)

Holds. From Equations (61) and (62), W is expressed as follows.

[0098]

[Equation 41]

When W is divided into n hyperplanes as in the fourth embodiment, wi becomes as follows.

[0100]

(Equation 42)

Here, similarly to the first embodiment, the control input uf is expressed by the following expression.
As shown in (52), when dW / dt is calculated from equations (4) and (65), the following is obtained.

[0102]

[Equation 43]

Therefore, dwi / dt is as follows.

[0104]

[Equation 44]

The control input

[0106]

[Equation 45]

In other words, widwi / dt is as follows.

[0108]

[Equation 46]

From equation (70),

[0110]

[Equation 47]

By switching each gain of the expression (69) so as to satisfy the expression (69), the expression (47) is satisfied, and the motion of the manipulator is restricted by a hyperplane (an impedance characteristic expression). As in the fourth embodiment, when the motion of the manipulator is constrained to the hyperplane, the contact stability is always guaranteed. In the fifth embodiment, unlike the fourth embodiment, an acceleration term due to virtual inertia is included in the impedance characteristic equation. By defining a hyperplane for the position, velocity, acceleration, and external force signals filtered by the low-pass filter, the virtual inertia of the manipulator can be controlled without using the actual acceleration signal. From equation (69)

[0112]

[Equation 48]

In other words, one configuration example of the control system can be represented as a block diagram in FIG. However, the definitions of Hv, Hr, Hf, Hs are the same as in the equation (60). In FIG. 9, reference numeral 301 denotes a virtual inertia.
Impedance parameter setting units for Md, virtual viscosity Dd, and virtual rigidity Kd, 302 is a hyperplane calculation unit of Expression (65), and 303 is a feedback gain switching calculation unit expressed by Expression (71). 305 is a feedforward gain Hr of d￣Pref / dt, and 320 is a feedback gain Lv of d￣P / dt. 319 is a low-pass filter of Expression (61). Other elements are exactly the same as in the fourth embodiment. When actually configuring the control system

[0114]

[Equation 49]

The same applies to the construction method and the approximation processing of the joint driving force uj in equation (5) as in the fourth embodiment.

[0116]

According to the present invention, the deviation between the position of the actuator and the position of the link is determined by the force torque base compliance that forms the position of the link and the speed feedback outside the external force feedback applied to the manipulator tip. By configuring the control so that the force torque command in the control is converged to one-stiffness of the power transmission mechanism of the signal filtered by the low-pass filter, the mechanical resonance of the manipulator caused by the rigidity of the power transmission mechanism such as the speed reducer is suppressed. Since the configured force torque-based compliance control is configured, there is an effect that contact stability is good and practical compliance control can be realized as compared with the conventional compliance control system. According to the present invention,
Inside the force feedback loop of torque-based compliance control, a compensator has been added to cancel the disturbance using an observer that estimates the disturbance due to nonlinear friction and the gravity of the manipulator, so that stable and accurate contact work is always possible. There is an effect that it can be realized.

Further, according to the present invention, the impedance characteristic expressed based on the position, speed, acceleration signal, and external force signal of the tip of the manipulator, and virtual inertia, virtual viscosity, and virtual rigidity whose values are set according to the work. By configuring the sliding mode control using the equation as a hyperplane (switching plane), a virtual impedance at the tip of the manipulator is realized, so that there is an effect that a stable and accurate contact operation can always be realized.

[Brief description of the drawings]

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

FIG. 2 is a block diagram of a control system according to the embodiment of the present invention.

FIG. 3A is a schematic view of a 1-Hakusan degree robot manipulator in which an actuator section and a link section are connected by a speed reducer.
(B) A diagram showing an environment in which the actuator unit and the link unit come into contact with a one-degree-of-freedom robot manipulator coupled with a speed reducer.

FIG. 4 is a block diagram of a contact system when the present invention is implemented.

FIG. 5 is a model diagram of a one-degree-of-freedom robot manipulator in which an actuator unit and a link unit are connected by a speed reducer;

FIG. 6 is a block diagram of an equivalent disturbance compensator according to the embodiment of the present invention.

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

FIG. 8 is a block diagram of a control system according to the embodiment of the present invention.

FIG. 9 is a block diagram of a control system according to the embodiment of the present invention.

[Explanation of symbols]

1: Actuator 2: Link 3: End effector 4: Reducer 5: Position and speed detector of actuator part 6: Position, speed and acceleration detector of link part 7: Detector of external force applied to end effector 8: End effector 11: Block representing the response (22) of the manipulator when the present invention is implemented 12: Block representing the rigidity of the environment 21: Inertia of the actuator 22: Viscous friction coefficient of the actuator 23: Inertia of the link 24: Viscous friction coefficient of link part 25: Rigidity of reduction gear 26: Rigidity of environment 27: Force generated by actuator 28: Force given to environment by end effector of manipulator 101: Actuator part and link part are connected via reduction gear Dynamics of robot manipulator 102: Rigidity of environment 103: Link Position feedback gain (virtual stiffness) 104: speed feedback gain (gain for adjusting virtual viscosity) of the link portion 105: force feedback gain 106: differential calculator 107: hyperplane W calculator 108a: equation ( Feedback gain switching calculation section based on 27) 108b: Feedback gain switching calculation section based on equation (30) 109: Feedback gain of deviation between actuator section position and link section position 110: Speed feedback gain of actuator section 111 : Feedback forward gain of target speed command 112: Speed feedback gain of link portion 112b: Acceleration feedback gain of link portion 113: Feedback gain of time derivative of external force 113a: Feedback gain of external force 114: Detector of external force 1 15: Detector of actuator section position 116: Detector of actuator section speed 117: Detector of link section position. 118: Detector of link speed 119: Detector of link acceleration 201: Position feedback gain matrix 202: Speed feedback gain matrix 203: Force torque feedback gain matrix 204: Transpose of Jacobian matrix of manipulator 205: Manipulator Jacobi matrix (transformation matrix from joint velocity to hand velocity) 206: forward kinematics transformation matrix of manipulator 207: proportional integrator of joint driving force command 208: integrator of joint torque by external force 209: proportional integrator of joint angular velocity 210 : Force torque detecting means 211: joint angular velocity detecting means 212: joint angle detecting means 213: driver of joint actuator 214: manipulator dynamics 215: manipulator dynamics calculator 216: low-pass filter matrix 217: mani Transpose of the Jacobian matrix of the manipulator 218: Force torque detecting means 219: Joint angular velocity detecting means 220: Driver of joint actuator 221: Manipulator dynamics 301: Impedance parameters Md, Dd, Kd setting unit 302: Hyperplane W computing unit 303 : Feedback gain switching calculator 304: Differential calculator 305: Feed forward gain Hr of dPref / dt or d￣Rref / dt 306: Feedback gain Hv of d￣fe / dt 307: Nonlinear friction, gravity, inter-joint interference Force compensation input Hs 308: Feedback gain of dP / dt Hv 309: Conversion matrix from manipulator hand acceleration command ＾ uf to joint actuator generation force (torque) command 310: Transpose matrix of Jacobian matrix, hand force (moment) ) To joint acting force (torque) 311: Jacobi Column, the hand from the joint velocity dq / dt rate dP /
Conversion to dt 312: Forward kinematics conversion matrix, P = R (q) 313: Force moment detecting means 314: Driver of joint actuator 315: Joint velocity detecting means 316: Joint position detecting means 317: Dynamics of manipulator 318: Manipulator Dynamics of the environment where the light comes in contact 319: Low-pass filter 320: Feedback gain LV of d￣P / dt

Claims (6)

[Claims] 1. A force torque-based compliance control system comprising: a manipulator having feedback control of force and torque; and a feedback control of position, velocity, and acceleration configured above the manipulator.
A compensator for canceling the disturbance using an observer for estimating an equivalent disturbance due to nonlinear friction, the gravity of the manipulator, and the interference force between the joints inside the force torque feedback loop; Torque-based compliance control method. And a means for measuring or estimating a position, a velocity, an acceleration signal and an external force applied to the tip of the manipulator. The movement of the tip of the manipulator is controlled by the position, speed, acceleration signal of the manipulator and an external force applied to the tip. Signals,
In a compliance control method in which control is performed so as to match a motion calculated from an impedance characteristic expression represented based on virtual inertia, virtual viscosity, and virtual rigidity that set values according to work, the impedance characteristic expression is expressed by a hyperplane A manipulator compliance control method characterized by realizing a virtual impedance at the tip of the manipulator by configuring a sliding mode control with (switching plane). 3. An impedance characteristic equation is set for a new position, velocity, acceleration signal or external force signal obtained by filtering a position, velocity, acceleration signal of the manipulator tip or an external force signal applied to the tip by a low-pass filter. The compliance control method for a manipulator according to claim 2. 4. A control method for a robot manipulator in which an actuator section and a link section are connected via a power transmission mechanism having a finite rigidity, wherein the position and speed of the actuator section, the position, speed, acceleration and Having means for detecting or estimating the external force signal applied to the tip of the manipulator, the motion of the tip of the manipulator, the position of the tip, the velocity, the external force signal applied to the tip and the tip, the virtual inertia to set the value according to the work, In a compliance control method that performs control so as to match the motion calculated from the impedance characteristic formula expressed based on virtual viscosity and virtual stiffness, the deviation between the position of the actuator and the position of the link is added to the robot manipulator. Construct control to converge to one-stiffness of the power transmission mechanism for external force torque command A force torque based compliance control method for a robot manipulator. 5. A signal obtained by filtering a force torque command by a low-pass filter in a force torque-based compliance control system that constitutes a position and a speed feedback of a link portion outside an external force feedback applied to a tip of a manipulator, to a new external signal. The force torque-based compliance control method for a robot manipulator according to claim 4, wherein the command is a force torque command. 6. A sliding mode control in which a deviation between the position of the actuator unit and the position of the link unit converges to one-stiffness of a power transmission mechanism for a new external force torque command. Item 6. A force torque-based compliance control method for a robot manipulator according to item 4 or 5.