JPH0475114A - Compliance control system - Google Patents

Abstract

PURPOSE:To obtain a robust system by which compliance can be kept at a preset value even when inertia, etc., is fluctuated by performing compliance control to which sliding mode control is applied on a controlled system with high inertia fluctuation. CONSTITUTION:A force sensor 27 is provided at the tip of the arm 26 of a robot, and amount of displacement on rectangular coordinate by a virtual spring equivalent to difference between command torque and feedback torque from the force sensor 27 is found, and a targeted position is transformed to the indirect coordinate of each axis setting a position in which the amount of displacement is added on a position command as the targeted position. The sliding mode control is applied to the servo control system of each axis, and the switching phase of the sliding mode control is switched by deviation among a position, speed, and a force, and a servo control system settable at arbitrary compliance can be formed. Thereby, it is possible to realize the compliance control by the sliding mode control resistant for parameter fluctuation and disturbance, etc.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a robot compliance control method,
In particular, it relates to compliance control methods suitable for assembly robots, etc.

[Conventional technology]

Welding intended only for position control or trajectory control,
The higher the rigidity of a painting robot, the more desirable it is. On the other hand, for assembly robots and the like, simply having high rigidity is not practical. For example, when considering screw tightening, there is no problem if the position of the hole and the position of the robot are completely within the permissible range, but this is generally not possible. Therefore, a certain degree of flexibility is required for assembly robots and the like.

For this purpose, there is compliance control that takes forces into account in the servo system as a method for arbitrarily changing the stiffness.

[Problem to be solved by the invention]

However, if you try to implement this servo system using general linear control (PI sub-control, etc.), it will be difficult to achieve the set compliance due to changes in the characteristics of the controlled object.In other words, with general linear control, Since the servo system is constructed assuming that the parameters are completely known and there are no parameter variations, it is difficult to apply it to controlled objects with large parameter variations.

In addition, conventional control systems that provide compliance on orthogonal coordinate axes have a servo system on orthogonal coordinates, but in this servo control, each output variable (X, Y,
Since there is interference between the input variables (input torque to the motor) and the input variables (input torque to the motor), performing compliance control with such a servo control system requires non-interference (one-to-one input and output variables). This requires complex processing such as digitization).

Therefore, even if the servo system is built digitally, a high-grade, high-speed processor is required, resulting in increased costs.

Furthermore, since such complicated processing is performed, it is very difficult to incorporate complicated control (such as sliding mode control here).

Furthermore, in robots that do not perform conventional force control, the servo control system is generally controlled by variables for each axis, so when building a servo control system in a Cartesian coordinate system, the servo control system is It requires fundamental changes and is extremely difficult to implement.

The present invention has been made in view of these points, and an object of the present invention is to provide a compliance control method that realizes compliance control by sliding mode control that is resistant to parameter fluctuations, disturbances, and the like.

[Means to solve the problem]

In order to solve the above problems, the present invention provides a force sensor at the tip of the robot's arm in a compliance control method that controls the servo control system of the robot, and measures the difference between the command torque and the feedback torque from the force sensor. Find the amount of displacement on the orthogonal coordinates due to the virtual spring,
The position obtained by adding the displacement amount to the position command is set as the target position,
By converting the target position into the joint coordinates of each axis, controlling the servo control system of each axis in a sliding mode, and switching the switching surface of the sliding mode control according to position, speed, and force deviations, arbitrary compliance can be achieved. A compliance control scheme is provided that is characterized by forming a configurable servo control system.

[Effect]

The amount of displacement corresponding to the difference between the command torque and the feedback torque is determined using the virtual spring. This displacement amount is added to the position command to find the target value. Convert this target value to the coordinates of each axis. Each axis is rotationally controlled by a servo motor. The servo system for each axis is controlled in a sliding mode.

〔Example〕

Hereinafter, one embodiment of the present invention will be described based on the drawings.

FIG. 2 is a hardware configuration diagram of a robot system for implementing the present invention. The host processor 9 is a processor that controls the entire robot. A robot position command is written from the host processor 9 to the shared RAM 10. Note that the RO coupled to the host processor 9
M, RAM, etc. are omitted.

DS that controls the servo motor 22 built into the robot
P (digital signal processor) 11 is RO
According to the system program of M12, shared RAM10
The position command, force command, spring constant, etc. are read at regular intervals.

The DSP II calculates a position deviation ε based on the difference between this position command and the position feedback θ from the pulse coder 23 built into the servo motor 22.

In addition, this position deviation ε is differentiated to calculate the speed deviation ε (eye).Furthermore, a torque command value Fd arbitrarily given by the instructor and a feedback torque Fi from the force sensor 27, which will be described later, are determined, and the torque deviation (Fdt Ft)
Calculate. Sliding mode control is executed from these data as described later.

In addition, the human power torque τ of the current compensation loop obtained by sliding mode control is calculated, a PWM waveform for driving the servo motor 22 is generated from this, and D
It is sent to the servo amplifier 21 via SL14.

The servo amplifier 21 receives the PWM command and drives the servo motor 22. The servo motor 22 is operated via a reduction gear.
The arm 26 is driven. A force sensor 27 is provided at the tip of the arm 26 to detect feedback torque.

Next, consider the following as a virtual spring model in the orthogonal coordinate space.

Kl*ΔX s = F d t −F t −−−−
(1) Here, each character has the following meaning.

△x1: Displacement due to the virtual spring in the x1 direction in the orthogonal coordinate system Fdt: Target force in the Xt force direction (given) F, : Force in the Xt force direction measured by the force sensor (cafe feedback) K1: Spring constant i is the number of axes For a 6-axis robot, i=1 to 6.

Consider obtaining compliance with a spring constant of Ki around the target force by attempting to move the robot tip by Δxi given above (giving an extra position command to the servo system).

From equation (1), ΔXt ``Kt −' books (Fdt Ft ) can be obtained.

Here, in order to give compliance by setting the target position as xdi, the new target position is defined as X d t+ΔX.

Change to This is a value in the orthogonal coordinate system, and when converted to joint coordinates, that is, target values for each axis, T ((9dt
)=Xdt+ΔX.

Therefore, the target position of each axis is obtained by θd 1=T−′(X d t+ΔxI) (2). However, T is an arm matrix.

If you set up a servo system with this θdi as the target position for each axis,
The following plant equation is obtained.

■i*θ1 ” +At *θi”' +c, r 1
=τmu tenτfb i ”””””””
”-(3) Here, each character is as follows.

Iz: Inertia seen from the i-joint At: Coefficient of dynamic friction Gr of the i-joint: Gravitational term τi seen from the i-joint = Torque human force τrb1:) Feedback of torque The above F1 is converted into joint torque Here, τfb”J ” ・F ・−・−(4) holds true.

However, τ11, F is a vector, and τfb”(τf
b l+τf b 2 + + τfbn)
TF=(F,,F2,F,)"J is a Jacobian matrix.

In the above explanation, all axes were explained, but in the following explanation, one axis of characteristics will be explained. Therefore, the subscript i denoting each axis is omitted.

The switching surface S of sliding mode control is defined as S=ε(
11+C*ε ・・・・・・・・・ −(5) Torque human power τ is τ−■. *B*S+τc (6) is assumed. Here, the first term on the right side represents linear human power, and the second term on the right side represents switching human power. However, C: position loop gain ε: θd-θ (including correction amount for compliance) ε(1): θd(1)-θf11 ■. = Expected minimum inertia Imax' is the expected maximum inertia. Here, as a Lyapunov function candidate, ■=(1
/2)*S2 Consider (7). (
5) Differentiating both sides of the equation, 5(1) = εf21 +
C*ε(1)(8) is obtained. Rewriting equation (3) using ε and θd, we get ε(2)=θd ” + (A/I) C31)+ (
Gr/I) −(τ/I) −(τ1./I) (9) Substituting equation (5) into equation (6) and organizing it into equation (9) yields 1, +21=θa”+ (A/I)C31]-+-(Gr/
I)-(Io/I) ・B-e(1)-(IO/I)
・B−C,ε(τ./I) −(τ,,/I) Substituting equation (10) into equation (8) and rearranging, we get 5(1)
−〇a”+(A/I)θ(1)+(Gr/I) −[C−(Io/I)・B] ・ε(1)−(1,/I
) ・B−C・ε (τ./I)−(τ,,/1) When formula (5) is solved for ε(1), e”′=S−C0ε −(12) Formula (12) becomes Substituting into equation (11) and rearranging, S['l = [C
-(1,/I)-B'] ・5-02 ・ε+θd(
2) +(A/I)θ[11+ (Gr/I)−(τ./I)
-(τ,,/I) ■(1)=s, 3(1) -[C-(Io/I) ・B:] S2+[-C'
・ε+θd32] + (A/I) θ”' + (G r/I) (τ.

/I) - (τ, , /I)] ・SHere, ■ τ so that (1) is always negative. Determine. That is, ■(1
If 3 is always chosen to be negative, ■ becomes monotonically decreasing and converges to the minimum value O. Therefore, V -0 becomes Si2, and sliding mode control is established.

First, if [(,-(I./I)・B'lS'<0, then C-(1,/I)・B<O C* (I/1.)<B Therefore, B=c* (1, -X/Io). Next, [-C2・ε+θd'] + (A/I) θ(1
]+(Gr/I)-(τC/I) (τrb/I) ・τ so that S<0. Determine the switching human torque τ.

is τ. = 1 (ε) + 2 (θd”) + 3 (θ”')
)+ to 4 (Gr)+ to 5 (rrb).

Therefore, this switching input torque τ. By switching each term in the following manner according to the value of the switching surface S, sliding mode control can be performed.

At the time of S2O, (the switching input in this case is the switching human power A) Regarding the term Kl (ε), when ε≧0, Kl (ε) = -C2*IO*ε When the switch is 0, Kl (ε) )=-C'*I□i*ε 2 (Regarding the term θd[21), θd(2) ≧
2 when 0 (θd(21)=J□8*θd(2)θd+2
]<Q, 2 (θd(21)=Io*θd(2) and 3(θ(
Regarding the term 1)), when θ(1)≧0, 3(θN+) 4. A*θ(1) θ(When 11<Q, 3(θ”')=A,,.*θ(4 (Gr)
= Gr, aX K5 (τ1.) = -τfb When S<0 (Switching force at this time is assumed to be switching force B.) Regarding the term Kl (ε), when ε≧0, Kl (ε)= -C'' * 1., Kl (E) = -C'*Io *ε when 0 (θd
Regarding the term (21), when θd(2) ≧0, 2(θd”) = Io*θd[2] θd12)<Q, 2(θd(2) ) = ko@ax *θd(2)K
Regarding the term 3(θ(I)), when θ(th≧0), 3 (θ”' ) −Am+n *θ31′θ(++
When <Q, 3(θ(1))=A□, *θ(1) K 4 (G r )=G r m1nK5 (τ7.
)=-τ,b FIGS. 1(a) and 1(b) are flowcharts of the process. In the figure, the numerical value following SP indicates the step number.

[SP1] Read the position feedback value QI and force feedback value F from the DSL 14.

[SP2] Target position Xd from shared RAM l0.

, force command Fdi, and spring constant. Here, the target position xdi is obtained by reading the position on orthogonal coordinates and the position on each axis.

[SP3] From the spring constant K, ΔX is calculated from the above-mentioned formula, ΔXt = Kt −' (Fdt −Ft).

C3P4) Calculate a new target position (X d , +ΔX,) and determine the target position θd of each axis from this target position.

This is determined from equation (5) mentioned above, θat =T-' (Xa, +ΔX,). Here, T is the arm matrix of the robot, and T-' is its inverse matrix.

[5P5E Feedback torque of each axis τ2. seek. This is obtained from the equation shown above, τ, b=J"-F. Here, JT is the transposed matrix of the Jacobian matrix.

Moving to FIG. 1(b), find the [SP6''1 positional deviation amount ε and its differential ε[1].

[SP7:] The switching surface S is expressed by the above equation (5), S=
Calculate from ε(+) 1C*ε.

[SP8: l If S2O, go to SP9, otherwise 5
Proceed to PIO.

[SP9] Select switching input A and switch each Kl(
ε), K2(θd')), K3(θ[1)), K4
(Gr), K5 (r, ,) and change the switching force τ. seek.

[5P10] Select switching manual power B and switch manual power τ in the same way as SP6. seek.

[5P11] Find the torque input to the current compensation loop from the following equation.

τ=Io *B*S+τ0 [5P12:] Torque input to the current compensation loop is received and passed.

Note that the above-mentioned SPI to SP5 are processes common to each axis, and SP6 to SP5P12 are processes for each axis.

By performing compliance control using sliding mode control in this manner, stable compliance control is possible even if inertia etc. change depending on the robot's posture.

In the above explanation, the robot compliance control method has been described, but it can also be applied to control of other servo systems in which inertia etc. vary.

〔Effect of the invention〕

As explained above, in the present invention, by performing compliance control applying sliding mode control to a controlled object with large inertia fluctuations, a robust system that can maintain compliance at a set value even if there are fluctuations in inertia etc. can be obtained. .

Furthermore, since compliance is given in orthogonal coordinates and converted to coordinates of each axis, compliance control can be performed with a simple configuration.

shared RAM digital signal processor (DSP) servo amplifier Servomotor force sensor

Claims (4)

[Claims] (1) In the compliance control method for controlling the robot's servo control system, a force sensor is installed at the tip of the robot's arm, and a virtual spring corresponding to the difference between the command torque and the feedback torque from the force sensor is used on orthogonal coordinates. determine the amount of displacement, set the position obtained by adding the amount of displacement to the position command as a target position, convert the target position to joint coordinates of each axis, control the servo control system of each axis in a sliding mode, and control the servo control system of each axis in the sliding mode. A compliance control method that is characterized by forming a servo control system that can be set to any compliance by switching the control switching surface according to position, speed, and force deviations. (2) The compliance control method according to claim 1, wherein the compliance is given as a spring constant. (3) The switching surface is expressed by the following formula, where S is the value of the switching surface, ε^(^1^) is the speed deviation, and ε is the position deviation, and the formula for the switching surface S is S=ε^(^1^) )+C*ε. The compliance control method according to claim 1, wherein sliding mode control is performed as: )+C*ε. (4) The compliance control method according to claim 1, wherein the sliding mode control is controlled by a digital signal processor that controls a servo control system of the robot.