FORCE CONTROL DEVICE, FORCE CONTROL METHOD, AND FORCE CONTROL PROGRAM

Abstract

Provided is a force control device and the like that can, even in the case of torque saturation, reduce positional deviation between a virtual object and a control target by using an algebraic loop to feedback a position of the virtual object, and improve stability while improving followability. The force control device includes: a position detector that detects a position of the control target; a force sensor 11 that detects a force by which the control target is in contact with an object; a moving mechanism 41 that movably holds the control target; a driving unit 42 that includes a motor that uses a torque command value as an input to operate the moving mechanism 41; and a computing unit 40 that determines the torque command value. The computing unit 40 stores a virtual object 12 having predetermined dynamic characteristics, and a position controller 13, and is configured to: calculate a position at a next time, the position being calculated by simulating a motion of the virtual object when a target force exerted on the control target and a measured value of the force sensor 11 are given to the virtual object; obtain a torque calculated when the calculated position is given to the position controller 13 as a target position at the next time; and, when the obtained torque is outside the predetermined range set previously, determine the position of the virtual object 12 at the next time based on a boundary value of the predetermined range and the obtained torque.

Claims

1. A force control device comprising: a position detector that detects a position of a control target; a force sensor that detects a force by which the control target is in contact with an object; a moving mechanism that movably holds the control target; a driving unit that includes a motor that uses a torque command value as an input to operate the moving mechanism; and a computing unit that determines the torque command value, wherein the computing unit stores a virtual object having predetermined dynamic characteristics, and a position control rule, and is configured to: calculate a position at a next time, the position being calculated by simulating a motion of the virtual object when a target force exerted on the control target and a measured value of the force sensor are given to the virtual object; obtain a torque calculated when the calculated position is given to the position control rule as a target position at the next time; when the obtained torque is within a predetermined range set previously, determine the obtained torque as the torque command value to determine the calculated position as a position of the virtual object at the next time; and when the obtained torque is outside the predetermined range set previously, determine the position of the virtual object at the next time based on a boundary value of the predetermined range and the obtained torque.

2. The force control device according to claim 1, wherein the computing unit is configured to, when the obtained torque is outside the predetermined range set previously, determine a torque on a boundary of the predetermined range and closest to the obtained torque, as the torque command value, calculate the target position at the next time when a torque command value calculated by the position control rule becomes the determined torque command value, and determine the calculated target position as the position of the virtual object at the next time.

3. The force control device according to claim 2, wherein the computing unit is configured to determine, in a continuous time, the torque T by updating the position q.sub.x of the virtual object so as to satisfy
.sub.c({umlaut over (q)}.sub.x,{dot over (q)}.sub.x,q.sub.x)ff.sub.d()
=.sub.c(q.sub.x,q.sub.s,{dot over (q)}.sub.x,{dot over (q)}.sub.s,{umlaut over (q)}.sub.x,(q.sub.xq.sub.s)dt) where F is the predetermined torque range, q.sub.s is an actual position of the control target, f.sub.d is the target force, f is the measured value of the force sensor, the predetermined dynamic characteristics of the virtual object is represented using a certain function .sub.c as
.sub.c({umlaut over (q)}.sub.x,{dot over (q)}.sub.x,q.sub.x)=f+f.sub.d and it is assumed that a function .sub.c is a position control rule defined such that q.sub.s follows a target value of q.sub.x.

4. The force control device according to claim 2, wherein the computing unit is configured to determine, at a time k in a discrete time, determine q.sub.x(k) and (k) so as to satisfy
q.sub.x(k)(q.sub.x(k1),q.sub.x(k2),f.sub.d(k))((k))
(k)=(q.sub.x(k),q.sub.s(k),(k1)) where is the torque, F is the predetermined torque range, q.sub.x is the position of the virtual object, q.sub.s is an actual position of the control target, f.sub.d is the target force, f is the measured value of the force sensor, the predetermined dynamic characteristics of the virtual object is represented using a certain function .sub.c as
q.sub.x(k)=(q.sub.x(k1),q.sub.x(k2),f(k)+f.sub.d(k)) and it is assumed that a function is a position control rule defined such that q.sub.s follows a target value of q.sub.x and that an output (k) of this function is determined as the torque command value, where S(k1) is a set of state variables, stored in a controller, such as an integral term, a measured position p.sub.s(k1) in a preceding step, and a target position p.sub.d(k1) in the preceding step.

5. The force control device according to claim 4, wherein the function is given by a function having an input-output relation as $\left(q_1,q_2,f\right).Math.\stackrel{}{=}.Math.\frac{\left(2.Math..Math.M_{}+B_{}\right).Math.q_1-M_{}.Math.q_2+T^2.Math.f}{M_{}+B_{}.Math.T}$ where T, M.sub.v, and B.sub.v are positive constants.

6. The force control device according to claim 4, wherein the function is a function having a same input-output relation as in an algorithm as $.Math..Math.\left(q_x,q_s,\right).Math.\stackrel{}{=}.Math.\mathrm{Function}.Math.\left[\left\{q_{x.Math..Math.1},q_{x.Math..Math.2},q_{s.Math..Math.1},a_1\right\}:=.Math..Math.q_s^{*}:=\frac{\hat{A}.Math.q_s+B\left(q_{x.Math..Math.1}-q_{s.Math..Math.1}\right).Math.\text{/}.Math.T-L.Math..Math.a_1+M\left(2.Math.q_{x.Math..Math.1}-q_{x.Math..Math.2}\right).Math.\text{/}.Math.T^2}{\hat{A}+M.Math.\text{/}.Math.T^2}.Math..Math.:=\left(\hat{A}+M.Math.\text{/}.Math.T^2\right).Math.\left(q_x-q_s^{*}\right).Math..Math.\mathrm{return}.Math..Math.\right]$ where , B, L, M, and T are positive constants.

7. The force control device according to claim 4, wherein the calculation for determining q.sub.x(k) and (k) has a same input-output relation as in an algorithm as follows.
q.sub.x.sup.*(k):=(q.sub.x(k1),q.sub.x(k2),f(k)+f.sub.d(k))
q.sub.s.sup.*(k):=.sub.p(q.sub.x.sup.*(k),q.sub.s(k),(k1))
*(k):={circumflex over (K)}(q.sub.x.sup.*(k)q.sub.s.sup.*(k))
(k):=(*(k))
q.sub.x(k):=q.sub.s.sup.*(k)+*(k)/{circumflex over (K)}
(k):=.sub.S(q.sub.x(k),q.sub.s(k),(k1))

8. A force control method performed by a computer, the method comprising: a position detecting step of detecting a position of a control target; a force detecting step of detecting a force by which the control target is in contact with an object; a computing step of computing a torque command value for a driving unit that includes a motor that uses the torque command value as an input to operate a moving mechanism that movably holds the control target; and a moving step of moving the control target based on a computation result of the computing step, wherein the computing step comprises: calculating a position at a next time, the position being calculated by simulating a motion of a virtual object when a target force exerted on the control target and a measured value of the force detecting step are given to the virtual object, using the virtual object and a position control rule that are stored in the computer, the virtual object having predetermined dynamic characteristics; obtaining a torque calculated when the calculated position is given to the position control rule as a target position at the next time; when the obtained torque is within a predetermined range set previously, determining the obtained torque as the torque command value to determine the calculated position as a position of the virtual object at the next time; and when the obtained torque is outside the predetermined range set previously, determining the position of the virtual object at the next time based on a boundary value of the predetermined range and the obtained torque.

9. A force control program that makes a computer to function as: position detecting means for detecting a position of a control target; force detecting means for detecting a force by which the control target is in contact with an object; computing means for computing a torque command value for a driving unit that includes a motor that uses the torque command value as an input to operate a moving mechanism that movably holds the control target; and moving control means for controlling a movement of the control target based on a computation result of the computing means, wherein the computing means: calculates a position at a next time, the position being calculated by simulating a motion of the virtual object when a target force exerted on the control target and a measured value detected by the force detecting means are given to the virtual object, using the virtual object and a position control rule that are stored in the computer, the virtual object having predetermined dynamic characteristics; obtains a torque calculated when the calculated position is given to the position control rule as a target position at the next time; when the obtained torque is within a predetermined range set previously, determines the obtained torque as the torque command value to determine the calculated position as a position of the virtual object at the next time; and when the obtained torque is outside the predetermined range set previously, determines the position of the virtual object at the next time based on a boundary value of the predetermined range and the obtained torque.

Description

BRIEF DESCRIPTION OF DRAWINGS

[0044] FIG. 1 is a functional block diagram showing an example of a processing concept of a conventional force control device.

[0045] FIG. 2 is a functional block diagram showing an example of a processing concept of a force control device according to a first embodiment.

[0046] FIG. 3 is a hardware configuration diagram of the force control device according to the first embodiment.

[0047] FIG. 4 is a functional block diagram of the force control device according to the first embodiment.

[0048] FIG. 5 is a flowchart illustrating an operation of the force control device according to the first embodiment.

[0049] FIG. 6 is a photograph of a robot used in the Example.

[0050] FIG. 7 is a diagram showing a result of an admittance control when a conventional method was used.

[0051] FIG. 8 is a diagram showing a result of an admittance control when the force control device according to the present invention was used.

DESCRIPTION OF EMBODIMENTS

[0052] Hereinafter, an embodiment of the present invention will be described. The same elements are denoted by the same reference numerals throughout the embodiment.

First Embodiment of the Present Invention

[0053] A force control device according to the present embodiment will be described with reference to FIGS. 1 to 5. FIG. 1 is a functional block diagram showing an example of a processing concept of a conventional force control device, and FIG. 2 is a functional block diagram showing an example of a processing concept of a force control device according to the present embodiment. As shown in FIG. 1, an admittance control in the prior art determines a position P.sub.d of a virtual object 12 having predetermined dynamic characteristics as a target position based on a measured value f of a force sensor 11 and a target force f.sub.d (a force applied to a robot 14 to be controlled). A position controller 13 calculates a torque command value of a motor of the robot 14 so as to satisfy the target position P.sub.d. The robot 14 moves to a position P based on a force h applied to the robot 14 from the outside, the measured value f of the force sensor 11, and the torque command value .

[0054] In the case of the prior art of FIG. 1, the target position P.sub.d of the virtual object 12 and the position P of the robot 14 will be deviated from each other when the torque command value is outside a torque range (for example, an upper limit value and a lower limit value of a torque set previously according to safety, drive characteristics of the robot 14, environment, and the like). In the prior art, it is necessary to make a transition from such a saturated state to an unsaturated state, which thus inevitably causes discomfort when the follow-up processing is performed. Therefore, in the present embodiment, the processing as shown in FIG. 2, for example, is performed.

[0055] In FIG. 2, the torque command value calculated by the position controller 13 is fed back in an algebraic loop, thereby making it possible to match the position q.sub.x of the virtual object 12 and the position q.sub.s of the robot 14, resulting in that it is possible to eliminate the discomfort in the follow-up processing as conventionally felt.

[0056] Hereinafter, a configuration and an algorithm of the force control device according to the present embodiment will be specifically described. FIG. 3 is a hardware configuration diagram of the force control device according to the present embodiment. A force control device 1 is provided with: the robot 14 to be controlled; the force sensor 11 that is attached to the robot 14 and measures a reaction force from an object in an environment around the robot 14; a ROM 23 that stores a program for performing admittance control, and the like; a CPU 21 that reads the program into a RAM 22 as needed, and executes a processing; and an input-output interface 24 that is an interface for accepting an input from an input device such as a touch panel, a keyboard, or a mouse, and outputting data to a printer, a display, or the like. These processing units are connected to each other via a bus and exchange information thereamong. The above hardware configuration is merely an example, and may be changed as needed.

[0057] FIG. 4 is a functional block diagram of the force control device according to the present embodiment. The force control device 1 is provided with: the robot 14 that applies a predetermined force to an environment; and a computing unit 40 that controls a movement of the robot 14. The robot 14 is provided with: the force sensor 11 that measures the reaction force from the object in the surrounding environment; a moving mechanism 41 that moves the robot 14 or a part (such as, for example, an arm) of the robot 14; and a driving unit 42 that includes a motor for driving the moving mechanism 41. The computing unit 40 calculates a torque command value of the motor of the driving unit 42 based on a measurement result from the force sensor 11 and position information of the moving mechanism 41 and outputs the torque command value to the driving unit 42. The processing of calculating the torque command value by the computing unit 40 will be described in detail below.

[0058] (1) Mathematical Preparation

[0059] Let F be a closed interval on a real number. A saturation function and a normal cone therefor are defined as follows.

[00001]$\begin{array}{cc}{\mathrm{sat}}_{}\left(x\right).Math.\stackrel{}{=}.Math.{\underset{}{\mathrm{argmin}}\left(-x\right)}^2& \left(1\right)\\ {}_{}\left(x\right).Math.\stackrel{}{=}.Math.\{\begin{array}{cc}\left\{\left(x^{*}-x\right)0.Math.x^{*}\right\}& \mathrm{if}.Math..Math.x\\ & \mathrm{if}.Math..Math.x.Math.\end{array}& \left(2\right)\end{array}$

[0060] The relationship between the saturation function and the normal cone is expressed as follows (Reference: Acary, V., Brogliato, B.: Numerical Methods Nonsmooth Dynamical Systems, Applications in Mechanics and Electronics, Lecture Notes in Applied and Computational Mechanics, vol. 35. Springer (2008)).

x+(x)yx=(y)(3)

[0061] A sign function is defined as follows.

[00002]$\begin{array}{cc}\mathrm{sgn}\left(x\right).Math.\stackrel{}{=}.Math.\{\begin{array}{cc}\left[-1,1\right]& \mathrm{if}.Math..Math.x=0\\ x.Math.\text{/}.Math..Math.x.Math.& \mathrm{if}.Math..Math.x0\end{array}& \left(4\right)\end{array}$

[0062] With respect to a closed interval [F, F] defined by a positive constant F, the following holds.

[00003]$\begin{array}{cc}{\mathrm{sat}}_{\left[-F,F\right]}\left(x\right)=\{\begin{array}{cc}x& \mathrm{if}.Math..Math..Math.x.Math.F\\ \mathrm{Fx}.Math.\text{/}.Math..Math.x.Math.& \mathrm{if}.Math..Math..Math.x.Math.>F\end{array}& \left(5\right)\\ {}_{\left[-F,F\right]}\left(x\right)=\{\begin{array}{cc}0& \mathrm{if}.Math..Math..Math.x.Math.<F\\ \left[0,\right)& \mathrm{if}.Math..Math..Math.x.Math.=F\\ \left(-,0\right]& \mathrm{if}.Math..Math..Math.x.Math.=-F\\ & \mathrm{if}.Math..Math..Math.x.Math.>F\end{array}& \left(6\right)\end{array}$

[0063] The sign function and the normal cone are inversely mapped to each other as described below.

xF sgn(y).sub.[F,F](x)y(7)

[0064] Further, the relationship between the sign function and the saturation function is expressed as follows.

xF sgn(yx)x=sat.sub.[F,F](y)(8)

[0065] (2) Next, a general expression of the proposed control rule will be described.

[0066] (2.1) Continuous Time Expression

[0067] Let the actual position of the control target (robot 14) be q.sub.s, the position of the virtual object 12 be q.sub.x, the generated force in the driving device 42 be , the measured value of the force sensor 11 be f, and the target force given to the admittance controller be f.sub.d (which may be fixed as f.sub.d=0).

[0068] Further, the position control rule of the robot 14 when the target position is let be q.sub.x is given by an appropriate function .sub.c, as follows.

=.sub.c(q.sub.x,q.sub.s,{dot over (q)}.sub.x,{dot over (q)}.sub.s,{umlaut over (q)}.sub.x,(q.sub.xq.sub.s)dt)(9)

[0069] Further, the dynamic characteristics to be realized by the virtual object are given by an appropriate function .sub.c, as follows.

.sub.c({umlaut over (q)}.sub.x,{dot over (q)}.sub.x,q.sub.x)=f+f.sub.d(10)

[0070] Specific examples of these functions will be described later in section (3).

[0071] Letting the allowable range of the generated force of the driving device 4 be a set F, the continuous time expression of the proposed controller is expressed as follows.

.sub.c({umlaut over (q)}.sub.x,{dot over (q)}.sub.x,q.sub.x)ff.sub.d()(11a)

=.sub.c(q.sub.x,q.sub.s,{dot over (q)}.sub.x,{dot over (q)}.sub.s,{umlaut over (q)}.sub.x,(q.sub.xq.sub.s)dt)(11b)

[0072] This can be regarded as a simultaneous differential equation in which and q.sub.x (second order differential) are unknown. Also, this can be represented as a block diagram including an algebraic loop as shown in FIG. 2. Although the normal cone N.sub.f() in the equation (11a) is an argument, is not allowed to exist outside F, so that must be within F to satisfy the above. It can be interpreted that q.sub.x is updated (i.e., q.sub.x (second order differential) is determined) such that should be within F. If the generated force calculated when q.sub.x (second order differential) is determined so as to satisfy the expression (10) and is then substituted into the expression (11b) is within F, a value of this is adopted as it is. On the other hand, if this is outside F, saturates at the value closest to on the boundary of F, and in this case, q.sub.x (second order derivative) is determined so as to satisfy the expression (11b).

[0073] (2.2) Discrete Time Expression (Expression Including Algebraic Loop)

[0074] A discrete time expression of the controller (11) will be derived.

[0075] When the discretization as

{dot over (q)}q(k)q(k1))/T

is made, the equation (11a) can be re-written using an appropriate function , as follows.

q.sub.x(k)(q.sub.x(k1),q.sub.x(k2),f.sub.d(k))((k))(12a)

[0076] Further, the equation (11b) can be re-written using an appropriate function , as

(k)=(q.sub.x(k),q.sub.s(k),(k1))(12b)

where S(k1) is a set including, for example, an integral term, a measured position q.sub.s(k1) of a preceding step, and a target position p.sub.d(k1) of the preceding step. The output value (k) of this function is set as a torque command value to a motor.

[0077] Using yet another function .sub.s, S(k) in the next step is determined as follows.

S(k)=.sub.S(q.sub.x(k),q.sub.s(k),(k1))(13)

This S (k) is used as one of input values S(k1) in the next step.

[0078] The equations (12a) and (12b) can be regarded as simultaneous equations for the unknowns (k) and q.sub.x(k), which can also be interpreted as the block diagram as shown in FIG. 2. Using these, the proposed admittance control algorithm is expressed as follows.

[00004]$\begin{array}{cc}.Math.q_x^{*}\left(k\right):=\left(q_x\left(k-1\right),q_x\left(k-2\right),f\left(k\right)+f_d\left(k\right)\right)& \left(14.Math.a\right)\\ \left\{\left(k\right),q_x\left(k\right)\right\}:=\mathrm{solution}.Math..Math.\mathrm{of}.Math..Math.\left\{\begin{array}{c}{}_{}\left(\left(k\right)\right).Math.q_x^{*}\left(k\right)-q_x\left(k\right)\\ \left(k\right)=.Math..Math.\left(q_x\left(k\right),q_s\left(k\right),\left(k-1\right)\right)\end{array}\right\}& \left(14.Math.b\right)\\ .Math.\left(k\right):={}_S\left(q_x\left(k\right),q_s\left(k\right),\left(k-1\right)\right)& \left(14.Math.c\right)\end{array}$

[0079] Since these expressions include a step of solving the algebraic inclusive expression (14b), it is difficult to implement them as they are.

[0080] (2.3) Discrete Time Expression (Expression as Algorithm)

[0081] A method of solving the algebraic inclusive expression (14b) will be considered. Here, the following assumption is made.

Assumption 1: The function (q.sub.x, q.sub.s, S) is strictly a monotonical increasing with respect to the argument q.sub.x.
Many typical controllers satisfy this assumption.

[0082] It is clear that the following expression holds under this assumption.

[00005]$\begin{array}{cc}\{\begin{array}{c}.Math..Math.\left(q_x^{*},q_s,\right)-.Math..Math.\left(q_x,q_s,\right)>0q_x^{*}-q_x>0\\ .Math..Math.\left(q_x^{*},q_s,\right)-.Math..Math.\left(q_x,q_s,\right)=0q_x^{*}-q_x=0\\ .Math..Math.\left(q_x^{*},q_s,\right)-.Math..Math.\left(q_x,q_s,\right)<0q_x^{*}-q_x<0\end{array}& \left(15\right)\end{array}$

[0083] Further, under this assumption, it is clear that there exists a single-valued function .sub.inv that satisfies the following.

=(q.sub.x,q.sub.s,)q.sub.x=.sub.inv(,q.sub.s,)(16)

[0084] This function .sub.inv can be called an inverse controller. An ordinary controller gives a torque command value to a target position. In contrast, the reverse controller calculates a target position required to output a designated torque command value. The algebraic inclusive expression given by the expression (14b) can be re-written using the expression (15), as

((k))(q.sub.x.sup.*(k),q.sub.s(k),(k1))(q.sub.x(k),q.sub.s(k),(k1))(17)

(k)=(q.sub.x(k),q.sub.s(k),(k1))(18)

[0085] Here, it is considered that there are only three possible return values of the normal cone NF((k)): (, 0], 0, [0, ). From the expression (8), the above expressions become as follows.

(k)=((q.sub.x.sup.*(k),q.sub.s(k),(k1)))(19)

(k)=(q.sub.x(k),q.sub.s(k),(k1))(20)

[0086] Here, the expression (20) can be re-written using the inverse controller .sub.inv defined by the expression (16), as follows.

q.sub.x(k)=.sub.inv((k),q.sub.s(k),S(k1))(21)

[0087] Therefore, the algorithm of the expression (14) can be re-written as follows.

q.sub.x.sup.*(k):=(q.sub.x(k1),q.sub.x(k2),f(k)+f.sub.d(k))(22a)

(k):=(q.sub.x.sup.*(k),q.sub.s(k),(k1))(22b)

(k):=(*(k))(22c)

q.sub.x(k):=.sub.inv((k),q.sub.s(k),(k1))(22d)

(k):=.sub.S(q.sub.x(k),q.sub.s(k),(k1))(22e)

[0088] The above expression is one form of the proposed admittance control algorithm. This algorithm can be further simplified when the following assumption 2 is satisfied in addition to the assumption 1.

Assumption 2: There exist a positive constant {circumflex over (K)} and a function .sub.p(q.sub.s, S) that satisfy (q.sub.x, q.sub.s, S)={circumflex over (K)} (q.sub.xp(q.sub.s, S) for all of q.sub.x, q.sub.s, and S.

[0089] This means that the controller (q.sub.x, q.sub.s, S) is linear with respect to the target value q.sub.x, and many typical controllers (q.sub.x, q.sub.s, S) satisfy this assumption. In this case, the inverse controller function becomes as follows.

.sub.inv(,q.sub.s,)=.sub.p(q.sub.s,)+/{circumflex over (K)}(23)

[0090] And, the algorithm (22) becomes as follows.

q.sub.x.sup.*(k):=(q.sub.x(k1),q.sub.x(k2),f(k)+f.sub.d(k))(24a)

q.sub.s.sup.*(k):=.sub.p(q.sub.x.sup.*(k),q.sub.s(k),(k1))(24b)

*(k):={circumflex over (K)}(q.sub.x.sup.*(k)q.sub.s.sup.*(k))(24c)

(k):=(*(k))(24d)

q.sub.x(k):=q.sub.s.sup.*(k)+*(k)/{circumflex over (K)}(24e)

(k):=.sub.S(q.sub.x(k),q.sub.s(k),(k1))(24e)

[0091] The operation of the expression (22d) in the algorithm (22) is simplified as that of the expression (24e) in the algorithm (24). Therefore, the amount of calculation can be reduced by implementing the type of the algorithm (24), depending on the structure of the function ( ).

[0092] (3) Next, actual examples of the functions .sub.c and .sub.c will be described.

[0093] (3.1) Example of Function .sub.c: In the Case of PID Control with Target Acceleration Feedforward

[0094] In the case of a PID control with a target acceleration feedforward, the controller function .sub.c in the continuous time domain is given as

.sub.c(q.sub.x,q.sub.s,{dot over (q)}.sub.x,{dot over (q)}.sub.s,{umlaut over (q)}.sub.x,(q.sub.xq.sub.s)dt)M{umlaut over (q)}.sub.x+K(q.sub.xq.sub.s)+B({dot over (q)}.sub.x{dot over (q)}.sub.s)+L(q.sub.xq.sub.s)dt(25)

where Mq.sub.x (q.sub.x is the second order differential) is the feedforward term of a target acceleration, and K, B, and L are proportional, differential, and integral gains, respectively.

[0095] In this case, the following expression

=.sub.c(q.sub.x,q.sub.s,{dot over (q)}.sub.x,{dot over (q)}.sub.s,{umlaut over (q)}.sub.x,(q.sub.xq.sub.s)dt)(26)

can be discretized to form into the following expression.

[00006]$\begin{array}{cc}\begin{array}{c}\left(k\right)=.Math.M.Math.\frac{q_x\left(k\right)-2.Math.q_x\left(k-1\right)-q_x\left(k-2\right)}{T^2}+K\left(q_x\left(k\right)-q_s\left(k\right)\right)+\\ .Math.B.Math.\frac{q_x\left(k\right)-q_x\left(k-1\right)-q_s\left(k\right)+q_s\left(k-1\right)}{T}+L(a\left(k-1\right)+\\ .Math.T\left(q_x\left(k\right)-q_s\left(k\right)\right))\\ =.Math.\left(\hat{A}+M.Math.\text{/}.Math.T^2\right).Math.\left(q_x\left(k\right)-q_s^{*}\left(k\right)\right)\end{array}& \left(27\right)\end{array}$

[0096] Here, a corresponds to a=(q.sub.xq.sub.s)dt, and is as follows.

a(k)=a(k1)+T(q.sub.x(k)q.sub.s(k))(28)

Further, =K+B/T+LT and the following expression holds.

[00007]$\begin{array}{cc}{q}_s^{*}\left(k\right)=\frac{\begin{array}{c}\hat{A}.Math.q_s\left(k\right)+B.Math..Math.(q_x.Math.\left(k-1\right)-\\ q_s\left(k-1\right)).Math.\text{/}.Math.T-L.Math..Math.a\left(k-1\right)\end{array}}{\hat{A}+M.Math.\text{/}.Math.T^2}+\frac{M\left(2.Math.q_x\left(k-1\right)-q_x\left(k-2\right)\right)}{\hat{A}.Math.T^2+M}& \left(29\right)\end{array}$

[0097] Considering these, the functions , .sub.S, and .sub.inv that correspond to the above function .sub.c become as follows.

[00008]$\begin{array}{cc}.Math..Math.\left(q_x,q_s,\right).Math.\stackrel{}{=}.Math.\mathrm{Function}.Math.\left[\left\{q_{x.Math..Math.1},q_{x.Math..Math.2},q_{s.Math..Math.1},a_1\right\}:=.Math..Math.q_s^{*}:=\frac{\hat{A}.Math.q_s+B\left(q_{x.Math..Math.1}-q_{s.Math..Math.1}\right).Math.\text{/}.Math.T-L.Math..Math.a_1}{\hat{A}+M.Math.\text{/}.Math.T^2}+\frac{M\left(2.Math.q_{x.Math..Math.1}-q_{x.Math..Math.2}\right)}{\hat{A}.Math.T^2+M}.Math..Math.\mathrm{Return}.Math..Math.\left(\hat{A}+M.Math.\text{/}.Math.T^2\right).Math.\left(q_x-q_s^{*}\right)\right]& \left(30\right)\\ {}_{}\left(q_x,q_s,\right).Math.\stackrel{}{=}.Math.\mathrm{Function}.Math.\left[\left\{q_{x.Math..Math.1},q_{x.Math..Math.2},q_{s.Math..Math.1},a_1\right\}:=.Math..Math.\mathrm{Return}.Math..Math..Math.\left\{q_x,q_{x.Math..Math.1},q_{s.Math.,}.Math.a_1+T\left(q_x-q_s\right)\right\}\right]& \left(31\right)\\ {}_{\mathrm{inv}}\left(,q_s,\right).Math.\stackrel{}{=}.Math.\mathrm{Function}.Math.\left[\left\{q_{x.Math..Math.1},q_{x.Math..Math.2},q_{s.Math..Math.1,}.Math.a_1\right\}:=.Math..Math.q_s^{*}:=\frac{\hat{A}.Math.q_s+B\left(q_{x.Math..Math.1}-q_{s.Math..Math.1}\right).Math.\text{/}.Math.T-L.Math..Math.a_1}{\hat{A}+M.Math.\text{/}.Math.T^2}+\frac{M\left(2.Math.q_{x.Math..Math.1}-q_{x.Math..Math.2}\right)}{\hat{A}.Math.T^2+M}.Math..Math.\mathrm{Return}.Math..Math.q_s^{*}+r.Math.\text{/}.Math.\left(\hat{A}+M.Math.\text{/}.Math.T^2\right)\right]& \left(32\right)\end{array}$

[0098] Further, the above function ( ) clearly satisfies the assumption 2, so that the function .sub.p can be determined as follows.

[00009]$\begin{array}{cc}{}_p\left(q_s,\right).Math.\stackrel{}{=}.Math.\mathrm{Function}.Math.\left[\left\{q_{x.Math..Math.1},q_{x.Math..Math.2},q_{s.Math..Math.1,}.Math.a_1\right\}:=.Math..Math.\mathrm{Return}.Math..Math.\frac{\hat{A}.Math.q_s+B\left(q_{x.Math..Math.1}-q_{s.Math..Math.1}\right).Math.\text{/}.Math.T-L.Math..Math.a_1}{\hat{A}+M.Math.\text{/}.Math.T^2}+\frac{M\left(2.Math.q_{x.Math..Math.1}-q_{x.Math..Math.2}\right)}{\hat{A}.Math.T^2+M}\right]& \left(33\right)\end{array}$

[0099] Therefore, this function can be implemented in a form of the algorithm (24). A result of this implementation is as follows.

[00010]$\begin{array}{cc}{q}_x^{*}\left(k\right):=\left(q_x\left(k-1\right),q_x\left(k-2\right),f\left(k\right)+f_d\left(k\right)\right).Math..Math.q_s^{*}:=\frac{\begin{array}{c}\hat{A}.Math.q_s\left(k\right)+B(q_x.Math.\left(k-1\right)-\\ q_s\left(k-1\right)).Math.\text{/}.Math.T-L.Math..Math.a\left(k-1\right)\end{array}}{\hat{A}+M.Math.\text{/}.Math.T^2}+\frac{\begin{array}{c}M(2.Math.q_{x.Math.}.Math.\left(k-1\right)-\\ q_{x.Math.}\left(k-2\right))\end{array}}{\hat{A}.Math.T^2+M}.Math..Math.{}^{*}\left(k\right):=\left(\hat{A}+M.Math.\text{/}.Math.T^2\right).Math.\left(q_x^{*}\left(k\right)-q_s^{*}\left(k\right)\right).Math..Math.\left(k\right):={\mathrm{sat}}_{}\left({}^{*}\left(k\right)\right).Math..Math.q_x\left(k\right):=q_s^{*}\left(k\right)+\left(k\right).Math.\text{/}.Math.\left(\hat{A}+M.Math.\text{/}.Math.T^2\right).Math..Math.a\left(k\right):=a\left(k-1\right)+T\left(q_x\left(k\right)-q_s\left(k\right)\right)& \left(34\right)\end{array}$

[0100] (3.2) Example of Function .sub.c: In the Case of Mass-Damper System

[0101] When dynamic characteristics of a virtual object are given by a simple inertial-viscous system, the dynamic characteristics becomes as follows.

M.sub.v{dot over (q)}.sub.x+B.sub.v{dot over (q)}.sub.x=f+f.sub.d(35)

[0102] Therefore, the function .sub.c becomes as follows.

.sub.c({umlaut over (q)}.sub.x,{dot over (q)}.sub.x,q.sub.x)M.sub.v{dot over (q)}.sub.x+B.sub.v{dot over (q)}.sub.x(36)

[0103] Here, when the expression (35) is discretized, it becomes as follows.

[00011]$\begin{array}{cc}\frac{M_{}}{T^2}.Math.\left(q_x\left(k\right)-2.Math.q_{x.Math.}\left(k-1\right)+q_x\left(k-2\right)\right)+\frac{B_{}}{T}.Math.\left(q_x\left(k\right)-q_{x.Math.}\left(k-1\right)\right)-f\left(k\right)-f_d\left(k\right)=0& \left(37\right)\end{array}$

[0104] Solving this expression for q.sub.x(k) gives the following.

[00012]$\begin{array}{cc}{q}_x\left(k\right)=\frac{\begin{array}{c}\left(2.Math..Math.M_{}+B_{}\right).Math.q\left(k-1\right)+\\ M_{}.Math.q_x\left(k-2\right)+T^2\left(f\left(k\right)+f_d\left(k\right)\right)\end{array}}{M_{}+B_{}.Math.T}& \left(38\right)\end{array}$

[0105] That is, the function becomes as follows.

[00013]$\begin{array}{cc}\left(q_1,q_2,f\right).Math.\stackrel{}{=}.Math.\frac{\left(2.Math..Math.M_{}+B_{}\right).Math.q_1-M_{}.Math.q_2+T^2.Math.f}{M_{}+B_{}.Math.T}& \left(39\right)\end{array}$

[0106] Next, the operation of the force control device according to the present embodiment will be described. FIG. 5 is a flowchart illustrating the operation of the force control device according to the present embodiment. First, the computing unit 40 performs arithmetic processing of the admittance control based on the force applied from the outside to the robot 14 and the measured value of the force sensor 11 (S1). This arithmetic processing is as described above. As a result of the calculation, a motor torque command value for driving the robot 14 is outputted and inputted to the robot 14 (S2). The robot 14 is driven according to the torque command value (S3). When the driving of the robot 14 finishes, the processing is terminated. When the driving (including a stopped state) continues, as a result of the driving, the position of the robot 14 and the measured value of the force sensor 11 are outputted, and the process returns to the arithmetic processing of the admittance control in step S1. Thereafter, the same processing is repeated.

[0107] As described above, in the force control device according to the present embodiment, when the calculated torque is outside the predetermined range set previously, the position of the virtual object at the next time is determined using the algebraic loop based on the boundary value of the predetermined range and the torque command value, so that positional deviation between the virtual object and the control target is reduced, resulting in that it is possible to improve stability while improving followability.

EXAMPLE

[0108] With respect to the force control device according to the present invention, the following experiment was performed. FIG. 6 is a photograph of the robot 14 used in the experiment. The above-mentioned admittance control rule was implemented in the third joint of the robot 14, and the other joints were locked. Then, the behavior when the operation was performed by contacting the force sensor 11 and the behavior when the operation was performed by contacting a link portion (a portion other than the force sensor 11) were observed.

[0109] FIG. 7 is a diagram showing a result of an admittance control when a conventional method was used, and FIG. 8 is a diagram showing a result of an admittance control when the force control device according to the present invention was used. In FIGS. 7 and 8, measurement results are shown for the respective states as follows:

(1) to time A: operated with the force sensor 11 held by hand;
(2) from time B to time C: a state in which a force was applied to a place outside the force sensor 11; and
(3) From time C: a state in which a force was applied to the force sensor 11 while the link portion was fixed by hand such that the robot 14 did not move.
The top of the graph shows the position of the virtual object 12 and the position of the robot 14, the middle shows the measured value of the force sensor 11, and the bottom shows the driving torque of the motor.

[0110] As can be seen from FIGS. 7 and 8, in the state (1), the position of the robot 14 follows the position of the virtual object 12 well in any of the methods. In the state (2), according to the conventional method of FIG. 7, the position of the robot 14 and the position of the virtual object 12 are deviated from each other. In this case, the position will tend to return to the original position like a spring when the applied force is released, which might be dangerous. On the other hand, according to the method of the present invention in FIG. 8, the position of the virtual object 12 follows the position of the robot 14, so that there would be no such a danger at all even when the applied force is released. In the state (3), according to the conventional method shown in FIG. 7, only the position of the virtual object 12 moves. In this case, the robot 14 will tend to follow the position of the far-away virtual object 12 when the user releases his/her hand, which might be dangerous. On the other hand, according to the method of the present application in FIG. 8, the position of the virtual object 12 does not move and the position of the robot 14 does not move either, so that there would be no such a danger at all.

[0111] As described above, it has been revealed that the method of the present invention has significantly improved safety as compared with the conventional method in such a situation where the torque saturation occurs.

REFERENCE SIGNS LIST

[0112] 1 Force Control Device [0113] 11 Force Sensor [0114] 12 Virtual Object [0115] 13 Position Controller [0116] 14 Robot [0117] 21 CPU [0118] 22 RAM [0119] 23 ROM [0120] 24 Input-Output Interface [0121] 40 Computing Unit [0122] 41 Moving Mechanism [0123] 42 Drive Unit