# Patent

## Title
Control method of robot apparatus and robot apparatus

## Abstract

A control method of a robot apparatus, the robot apparatus including a link and a pair of actuators, obtaining each driving force command value of each of the actuators, and controlling each of the actuators, the control method including: a torque command value computation step; a change computation step of computing a difference between the joint stiffness command value and a value and performing a computation of subtracting a value from the joint stiffness command value; an iterative step of iterating the computations of the torque command value computation step and the change computation step until the difference converges to a value equal to or smaller than a predetermined value; and a driving force command value computation step to compute each of the driving force command values when the difference is converged to a value equal to or smaller than the predetermined value.

## Background
This application is a continuation of application Ser. No. 13/722,514, filed Dec. 20, 2012 (allowed), the contents of which are incorporated by reference herein.


BACKGROUND OF THE INVENTION
Field of the Invention
The present invention relates to a control method of a robot apparatus including a hand that can flexibly touch an object and to the robot apparatus. The present invention can be applied to a working robot and a legged mobile robot.
Description of the Related Art
In a control method of a manipulator, it is becoming more important that a hand be able to flexibly touch an object. If the method is applied to an industrial robot, cooperative work of a robot apparatus and a human can be realized, and fitting work of components is facilitated by controlling the direction of the flexibility of the hand. If the method is applied to a legged mobile robot, impact on the body can be alleviated by soft landing, and absorption of a difference in level allows stable walking on an unleveled land.
To realize the control of the flexibility of the hand, impedance control for installing a force sensor on the hand or control using artificial muscle actuators is performed. Muscles of a human are known to serve as actuators, and at the same time, serve as a viscoelasticity-variable control mechanism. Among the artificial muscles, pneumatic rubber artificial muscles represented by McKibben artificial muscles particularly have similar viscoelasticity characteristics as those of the human muscles. Therefore, the control of the softness of artificial muscle actuators arranged on a manipulator allows touching an object at arbitrary flexibility of hand. However, the artificial muscle actuators are known to have difficulties in controllability for reasons that the viscoelasticity characteristics indicate non-linearity and that the actuators need to be antagonistically arranged and controlled because the force is generated only in a contraction direction.
Regarding this, disclosed is a technique for generating feedforward input for simultaneous control of a joint angle and flexibility of hand using a model of a manipulator with viscoelasticity characteristics of muscles and using a corrected value calculation unit (see Japanese Patent No. 3436320). In Japanese Patent No. 3436320, the model is used to output a joint angle and viscoelasticity coefficients of artificial muscle actuators at the time of control input, and the joint angle and a target value are compared. An error between the joint angle and the target value is back-propagated to the corrected value calculation unit to correct the feedforward input. Operation of providing the corrected feedforward input again to the model is repeated to gradually obtain the feedforward input.
In Japanese Patent No. 3436320, the joint angle and the flexibility of hand are simultaneously controlled by the feedforward control. However, there is no study example for antagonistically driving the artificial muscle actuators by minimum control input. To minimize the control input, hand flexibility corresponding to a target trajectory of the joint angle needs to be taken into account to simultaneously optimize the hand flexibility and the control input. However, the target stiffness of the hand flexibility is provided in advance in the feedforward control of Japanese Patent No. 3436320, and an algorithm for optimizing the hand flexibility is not included.
An object of the present invention is to simultaneously optimize a torque command value and a joint stiffness command value in trajectory control of a joint angle to antagonistically drive links based on minimum driving force command values.
SUMMARY OF THE INVENTION
The present invention provides a control method of a robot apparatus, the robot apparatus including: a link pivotally connected to a base body through a joint; and a pair of actuators that generate driving force for pulling the link in opposite directions relative to the base body to provide torque to the joint based on a difference in the driving force and to provide stiffness to the joint based on a sum of the driving force, the robot apparatus obtaining each driving force command value of each of the actuators necessary for a joint angle of the joint to follow a target trajectory, the robot apparatus controlling each of the actuators so that the driving force generated in each of the actuators coincides with each of the driving force command values, the control method including: a torque command value computation step of using the target trajectory, angular velocity of the target trajectory, angular acceleration of the target trajectory, and a joint stiffness command value indicating a total value of the driving force command values to calculate a torque command value indicating the torque necessary for the joint based on inverse dynamics of the link; a change computation step of computing a difference between the joint stiffness command value and a value, which is obtained by dividing an absolute value of the torque command value by a moment arm radius of the link, and performing a computation of subtracting a value, which is obtained by multiplying the difference by a coefficient greater than 0 and equal to or smaller than 1, from the joint stiffness command value to change the joint stiffness command value; an iterative step of iterating the computations of the torque command value computation step and the change computation step until the difference converges to a value equal to or smaller than a predetermined value; and a driving force command value computation step of using the joint stiffness command value and the torque command value to compute each of the driving force command values when the difference is converged to a value equal to or smaller than the predetermined value.
According to the present invention, the iterative computation algorithm for simultaneously optimizing the stiffness of the joint and the trajectory of the joint can generate feedforward input for antagonistically driving the actuators based on minimum driving force command values relative to the target trajectory. The minimization of the driving force command values can minimize, for example, an amount of consumed air in the McKibben artificial muscles. This can extend the operating time in a robot apparatus that uses an on-board air supply such as a tank.
Further features of the present invention will become apparent from the following description of exemplary embodiments with reference to the attached drawings.



BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is a diagram illustrating a viscoelasticity model of a muscle.
FIG. 2 is an explanatory view illustrating a schematic configuration of a robot apparatus according to a first embodiment of the present invention.
FIG. 3 is a block diagram of a control unit according to the first embodiment of the present invention.
FIG. 4 is a diagram illustrating a target trajectory according to the first embodiment of the present invention.
FIG. 5A is a diagram illustrating a simulation result according to the first embodiment of the present invention.
FIG. 5B is a diagram illustrating a simulation result according to the first embodiment of the present invention.
FIG. 5C is a diagram illustrating a simulation result according to the first embodiment of the present invention.
FIG. 6 is a block diagram of a control unit according to a second embodiment of the present invention.
FIG. 7A is a diagram illustrating a simulation result according to the second embodiment of the present invention.
FIG. 7B is a diagram illustrating a simulation result according to the second embodiment of the present invention.
FIG. 7C is a diagram illustrating a simulation result according to the second embodiment of the present invention.
FIG. 7D is diagram illustrating a simulation result according to the second embodiment of the present invention.
FIG. 8 is an explanatory view illustrating a schematic configuration of a robot apparatus according to a third embodiment of the present invention.
FIG. 9 is a block diagram of a control unit according to the third embodiment of the present invention.
FIG. 10 is a block diagram of feedforward control of the control unit according to the third embodiment of the present invention.
FIG. 11 is a diagram for describing a stiffness ellipse according to the third embodiment of the present invention.
FIG. 12 is a diagram illustrating a trajectory design according to the third embodiment of the present invention.
FIG. 13A is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 13B is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 14A is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 14B is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 14C is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 15A is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 15B is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 15C is a diagram illustrating a simulation result according to the third embodiment of the present invention.
FIG. 16 is a diagram illustrating a simulation result according to the third embodiment of the present invention.



DESCRIPTION OF THE EMBODIMENTS
Preferred embodiments of the present invention will now be described in detail in accordance with the accompanying drawings.
First Embodiment
FIG. 1 is a diagram illustrating a viscoelasticity model of an actuator applied to a robot apparatus according to a first embodiment of the present invention. The present first embodiment describes an example of simultaneous control of a joint angle and stiffness of a joint of a manipulator using pneumatic artificial muscle actuators in a robot apparatus.
(1) Modeling
The artificial muscle actuator is an actuator with characteristics similar to characteristics called viscoelasticity of muscle. As illustrated in FIG. 1, the artificial muscle actuator is modeled using a force generation element, an elasticity element and a viscosity element. Contractile force of the force generation element is defined as u, and an amount of contraction of muscle is defined as x wherein a contraction direction is positive. The following formula denotes a contractile rate.

{dot over (x)}

An elastic force constant is defined as k, a viscous force constant is defined as b, and muscle contractile force is defined as F. In this case, viscoelasticity characteristics of muscle are modeled as follows.

F=u−kux−bu{dot over (x)}  (1)

This provides a non-linear element in which elastic force and viscous force of muscle contractile force are proportional to the contractile force u of the force generation element.

FIG. 2 illustrates a robot apparatus of the present first embodiment. A robot apparatus 100 illustrated in FIG. 2 is a so-called one-link manipulator and includes a link 101, a drive unit 120 with a pair of actuators e1 and f1, and a base body including a base member 103 and a rod-like arm member 104 extending from the base member 103. The link 101 is pivotally connected to the arm member 104 through a joint 105.
Although the base body includes the base member 103 and the arm member 104 as illustrated in FIG. 2 in the description of the present first embodiment, the arrangement is not limited to this. For example, the base body may be another link connected to a body of a legged mobile robot, connected to a pedestal of a working robot, or connected to the link 101.
The actuators e1 and f1 are mono-articular muscle actuators. One end is connected to the base member 103 as the base body, and the other end is connected to a base end 101a of the link 101. The actuators e1 and f1 are antagonistically arranged to swing the link 101 based on a difference in driving force (contractile force). Therefore, the actuators e1 and f1 are symmetrically arranged on both sides of the link 101 across the link 101.
The robot apparatus 100 includes a control unit 150 that sets the driving force of the actuators e1 and f1 based on driving force command values to control operation of the link 101.
The actuators e1 and f1 are pneumatic artificial muscle actuators illustrated in FIG. 1. The artificial muscle actuators generate force only in the contraction direction. Therefore, the actuators are antagonistically arranged as illustrated in FIG. 2 to position the joint 105 at an arbitrary angle. Thus, the actuators e1 and f1 generate driving force that pulls the link 101 in opposite directions relative to the base member. Since the actuators e1 and f1 are mono-articular muscle actuators made of artificial muscles, the driving force is generated by contraction. Therefore, the contractive force of the actuators e1 and f1 coincides with the driving force.
The driving force (contractile force) of force generation elements of the antagonistically arranged actuators e1 and f1 are defined as ue1 and uf1, respectively. An angle of the link 101 relative to the arm member 104, i.e. an angle of the joint 105 is defined as θ, and inertia moment of the link 101 is defined as I. A moment arm diameter of the link 101, i.e. a length between a pivot center point of the link 101 and a connection point of the actuators e1 and f1 at the link 101, is defined as r. The following Formula (2) denotes an equation of motion.

I{umlaut over (θ)}=(uf1−ue1)r−(uf1+ue1)kr2θ−(uf1+ue1)br2{dot over (θ)}  (2)

It can be recognized that a difference between the contractile force ue1 and uf1 in the first term on the right side of Formula (2) provides rotation torque to the joint 105, and a sum of the contractile force ue1 and uf1 in the second and third terms on the right side changes the stiffness and the viscosity relative to the joint 105. Therefore, the actuators e1 and f1 provide torque to the joint 105 based on the difference in the driving force and provides stiffness to the joint 105 based on the sum of the driving force.
(2) Control System Design
The control unit 150 executes a torque command value computation step 151, a change computation step 152, an iterative process 153 and a driving force command value computation step 155. An object of the present first embodiment is to provide feedforward control input for the execution of the driving force command value computation step 155 of calculating the driving force command values ue1 and uf1 to thereby cause the angle of the joint 105 to follow a target trajectory. In this case, feedforward input that minimizes the contractile force (driving force) is calculated. The target trajectory of the joint 105 is defined as ra. That is, the control unit 150 calculates the driving force command values ue1 and uf1 of the actuators e1 and f1 necessary to cause the angle θ of the joint 105 to follow the target trajectory ra. The control unit 150 controls the actuators e1 and f1 to cause the driving force generated in the actuators e1 and f1 to be equal to the driving force command values ue1 and uf1.
(2.1) Feedforward Control System Design
In this section, a derivation method of the feedforward input provided to the actuators e1 and f1 will be described. As described below, the difference between the driving force command values ue1 and uf1 multiplied by the moment arm diameter r is defined as T1, and the joint stiffness command value indicating the sum (total value) of the driving force command values ue1 and uf1 is defined as U1.

(Uf1−ue1)r=T1  (3)

uf1+ue1=U1  (4)

Since the artificial muscle actuators e1 and f1 generate force only in the contraction direction, the following conditions need to be satisfied at the same time.

uf1>0,ue1>0  (5)

When Formulas (3) and (4) are assigned to Formula (2), the following can be expressed.

I{umlaut over (θ)}=T1−U1kr2θ−U1br2{dot over (θ)}  (6)

It can be recognized that T1 provides torque to the joint 105 and that U1 increases or decreases the stiffness and the viscosity relative to the joint 105.
Therefore, to calculate driving force command values that simultaneously satisfy the torque command value T1 and the joint stiffness command value U1, Formulas (3) and (4) are solved for ue1 and uf1 to determine as follows.




  
    
      
        
          
            u
            
              f
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                1
              
              +
              
                
                  T
                  1
                
                /
                r
              
            
            2
          
        
        ,
        
          
            u
            
              e
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                1
              
              -
              
                
                  T
                  1
                
                /
                r
              
            
            2
          
        
      
    
    
      
        (
        7
        )
      
    
  




It can be recognized from Formula (7) that the joint stiffness command value U1 can be minimized to minimize the driving force command values uf1 and ue1 to provide an arbitrary torque command value T1. However, the following conditions can be obtained from Formulas (5) and (7).

T1/r<U1,T1/r>−U1  (8)

It can be recognized that the joint stiffness command value U1 needs to satisfy the following condition using the torque command value T1 to provide the artificial muscle actuators e1 and f1 with the driving force command values uf1 and Ue1 only in the contraction direction.

|T1|/r<U1  (9)

As for the torque command value T1 for causing the angle of the joint 105 to follow an arbitrary target trajectory ra, angular velocity and angular acceleration of the target trajectory ra can be expressed by

{dot over (r)}a,{umlaut over (r)}a 

or ra′ and ra′′, respectively. The torque command value T1 can be expressed by the following Formula (10) based on inverse dynamics of the link 101.

T1=I{umlaut over (r)}a+U1kr2ra+U1br2{dot over (r)}a  (10)

Formula (10) includes the joint stiffness command value U1, and as can be recognized from Formula (9), the joint stiffness command value U1 is a command value restricted by the torque command value T. In the present first embodiment, an iterative computation algorithm is used to obtain the torque command value T1 and the joint stiffness command value U1 that simultaneously satisfy Formulas (9) and (10).
FIG. 3 illustrates a block diagram of a control unit 150 according to the present first embodiment. A block indicated by “Iteration Algorithm” executes the change computation step 152.
In the present first embodiment, the number of iterations is defined as i, and the torque command value T1 and the joint stiffness command value U1 based on iterative computations of i times are written as follows.

T1[i],U1[i]  (11)

In the torque command value computation step 151, the control unit 150 uses the target trajectory ra, the angular velocity ra′ of the target trajectory, the angular acceleration ra′′ of the target trajectory, and the joint stiffness command value U1[i] to compute the torque command value T1[i] indicating the torque necessary for the joint, based on inverse dynamics of the link 101. More specifically, the control unit 150 uses formula (12) to compute the torque command value T1[i] of an i-th iteration (torque command value computation step).

T1[i]=I {umlaut over (r)}a+U1[i]kr2ra+U1[i]br2{dot over (r)}a  (12)

In the change computation step 152, the control unit 150 computes a difference between the joint stiffness command value U1[i] and a value |T1[i]|/r obtained by dividing an absolute value of the torque command value T1[i] by the moment arm radius r of the link 101. The difference is defined as E1[i], and the control unit 150 computes the difference E1[i] by the following Formula (13).

E1[i]=U1[i]−|T1[i]|/r  (13)

In the change computation step 152, the control unit 150 performs a computation of subtracting a value, which is obtained by multiplying the difference E1[i] by a convergence factor γ greater than 0 and equal to or smaller than 1, from the joint stiffness command value U1[i] to change the joint stiffness command value U1[i] (change computation step). More specifically, the control unit 150 sets the convergence factor γ of the iterative computation algorithm to 0<γ≦1 to calculate an i+1-th joint stiffness command value U1[i|1] based on the following Formula (14).

U1[i+1]=U1[i]−γE1[i]  (14)

In the iterative step 153, the control unit 150 iterates the computations of Formulas (12) to (14) for a predetermined number of times m until the difference E1[i] converges to equal to or smaller than a predetermined value (for example, 0) (iterative step).
Since the difference E1[i] is converged to 0, the joint stiffness command value U1[m] is a minimum value satisfying Formula (9), and the torque command value T1[m] using this is a minimum value. Based on this, the feedforward input with the minimum driving force command values ue1 and uf1 is delivered from Formula (7) as follows.




  
    
      
        
          
            u
            
              f
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                
                  1
                  ⁡
                  
                    [
                    m
                    ]
                  
                
              
              +
              
                
                  T
                  
                    1
                    ⁡
                    
                      [
                      m
                      ]
                    
                  
                
                /
                r
              
            
            2
          
        
        ,
        
          
            u
            
              e
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                
                  1
                  ⁡
                  
                    [
                    m
                    ]
                  
                
              
              -
              
                
                  T
                  
                    1
                    ⁡
                    
                      [
                      m
                      ]
                    
                  
                
                /
                r
              
            
            2
          
        
      
    
    
      
        (
        15
        )
      
    
  




Therefore, in the driving force command value computation step 155, after the iterative process 153, the control unit 150 uses the joint stiffness command value U1[m] and the torque command value T1[m] to compute the driving force command values uf1 and ue1 (driving force command value computation step).
To set a lower limit for the joint stiffness command value, the following formula can be used in place of Formula (13), wherein a stiffness lower limit is defined as V1.

E1[i]=U1[i]−|T1[i]|/r−V1  (16)

Based on Formula (16), the joint stiffness of the manipulator at an arrival angle can be controlled by, for example, adjusting V1 of the target trajectory at arrival time.
When the arrival angle of the target trajectory ra is defined as θd, a torque command value T1d necessary to set the link is as follows.

U1kr2θd≦T1d  (17)

However, Formula (9) also needs to be satisfied at the same time. Therefore, the elastic force constant k and the pulley radius r need to satisfy the following condition.

krθd<1  (18)

In the derivation described above, it is assumed that the elastic force of muscle is generated in proportion to the angle from θ=0 deg. To generate the feedforward control input with an arbitrary angle as a reference angle (hereinafter, “neutral angle”) of the generation of the elastic force of muscle, the following formula (19) can be used in place of Formula (12), wherein θc denotes the neutral angle.

T1[i]=I {umlaut over (r)}a+U1[i]kr2(ra−0c)+U1[i]br2{dot over (r)}a  (19)

(2.2) Trajectory Design
A design method of the target trajectory ra of the joint 105 will be illustrated. In the present embodiment, a trajectory including an acceleration section, a constant velocity section, and a deceleration section as illustrated in FIG. 4 is used for the target trajectory ra. Here, raS and raF denote an initial angle and a target angle of the trajectory, respectively. Furthermore, ta, tb and tfin denote a start time of the constant velocity section, an end time of the constant velocity section, and a positioning end time, respectively. Although the trajectory including the acceleration section, the constant velocity section and the deceleration section is used in the present first embodiment, a trajectory without the constant velocity section, such as a minimum jerk trajectory, may also be used.
(3) Simulation
A simulation using the control system of the previous section is performed. The inertia moment of the link is defined by I=8.3×10−2 kgm2, and the moment arm diameter is defined by r=0.1 m. The elastic force and viscous force constants are defined by k=25 and b=3. As for the target trajectory ra, the initial angle is defined by raS=−20 deg, the target angle is defined by raF=20 deg, the start time of the constant velocity section is defined by ta=0.4 sec, the end time of the constant velocity section is defined by tb=0.6 sec, and the positioning end time is defined by tfin=1 sec. The joint stiffness command value U1 is delivered to satisfy the restriction illustrated in Formula (9) based on the iterative computation algorithm illustrated in 2.1. The lower limit of the joint stiffness command is set to V1=0.02, and γ=1 is set to perform the iterative computations.
FIG. 5A illustrates the angle θ and the target trajectory ra of the joint by a solid line and a dashed line, respectively. FIG. 5B illustrates responses of the driving force command values ue1 and uf1 by a solid line and a dashed line, respectively. FIG. 5C illustrates the difference between the driving force command values ue1 and uf1 by a solid line, the sum by a dashed line, and a response of T1/r by an alternate long and short dash line.
It can be recognized from FIG. 5A that the angle θ of the joint 105 indicated by the solid line overlaps with the trajectory ra illustrated by the dashed line and that the joint 105 follows the target trajectory ra. It can be recognized from FIG. 5B that the characteristic that the contractile force of the artificial muscle can only be a positive value is satisfied, because the driving force command values ue1 and uf1 are obtained by distributing the control input (torque command value) T1 of the feedforward control system based on the conditions illustrated in Formulas (7) and (9). In FIG. 5C, uf1−ue1 indicated by the solid line overlaps with T1/r indicated by the alternate long and short dash line, and the condition of Formula (3) is satisfied. It can be recognized that uf1+ue1=U1 indicated by the dashed line is in the proximity of the absolute value of T1/r indicated by the solid line at intervals of V1 and that uf1+ue1=U1 is always a minimum value. It can be recognized that the proposed method of the present first embodiment allows antagonistic drive with the minimum driving force command values ue1 and uf1 while realizing the following of the target trajectory ra. The minimization of the driving force command values ue1 and uf1 allows minimizing the amount of consumed air in, for example, the McKibben artificial muscles. As a result, the operating time can be extended in a robot apparatus that uses an on-board air supply such as a tank.
Second Embodiment
A robot apparatus according to a second embodiment of the present invention will be described. In the present second embodiment, control operation by a control unit is different from the control operation by the control unit of the first embodiment. In the configuration of the main body of the robot apparatus, components with similar configurations as those of the main body of the robot apparatus of FIG. 1 described in the first embodiment are designated with the same reference numerals, and the detailed description is omitted.
The present second embodiment delivers a two-degree-of-freedom control system merging the feedforward control system illustrated in the first embodiment and a feedback control system. The feedback control system is delivered to allow simultaneously following the target trajectory and the joint stiffness command values obtained by the iterative computation algorithm. With only the feedforward control system, the control performance deteriorates if there is an identification error in a parameter such as the inertia moment of the link 101. However, it will be illustrated that the configuration of the two-degree-of-freedom control system with the feedback control system allows simultaneous control of the target trajectory and the joint stiffness command values, even if there is a model error.
(1) Modeling
A model used in the present second embodiment is the same as that of the first embodiment.
(2) Control System Design
The delivery of the feedforward control system is similar to that of the first embodiment. The joint torque command TFF[m] delivered by the iterative computation algorithm of the first embodiment is described as TFF in the present second embodiment.
FIG. 6 illustrates a block diagram of a control unit 150A according to the present second embodiment. The section surrounded by the dashed lines in the feedforward control system illustrated in FIG. 3 is indicated by KFFW in FIG. 6.
Other than the feedforward control system KFFW, the control unit 150A has a function of executing a PID control step KPID1 as a corrected torque command value computation step 154.
In the PID control step KPID1, the control unit 150A computes control input torque TFB as a corrected torque command value for compensating the difference between the angle θ and the target trajectory ra of the joint 105 (corrected torque command value computation step).
Based on Formula (2), if ue1 and uf1 are determined to satisfy

(uf1−ue1)r=TFF+TFB=T′1  (20),

T′1 as a sum of the feedforward control torque TFF and the feedback control torque TFB can be provided to the joint 105. The torque command value T′1 denotes a computation result obtained by adding the control input torque TFB calculated in the PID control step KPID1 to the torque command value TFF calculated in the torque command value computation step 151 (see FIG. 3) of the feedforward control system KFFW.

Therefore, in the present second embodiment, the torque command value T′1 is used as a torque command value used to calculate the driving force command values in the execution of the driving force command value calculation step 155.
As in the first embodiment, the driving force command values ue1 and uf1 need to be determined to simultaneously satisfy the following conditions related to the stiffness of the joint 105.

U1=uf1+ue1=U1[m]  (21)

To simultaneously satisfy the torque command value T′1 and the joint stiffness command value U1, Formulas (20) and (4) can be solved for the driving force command values ue1 and uf1 to determine as follows.




  
    
      
        
          
            u
            
              f
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                1
              
              +
              
                
                  T
                  1
                  ′
                
                /
                r
              
            
            2
          
        
        ,
        
          
            u
            
              e
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                U
                1
              
              -
              
                
                  T
                  1
                  ′
                
                /
                r
              
            
            2
          
        
      
    
    
      
        (
        22
        )
      
    
  




However, the artificial muscle actuators e1 and f1 generate force only in the contraction direction. Therefore, as in the first embodiment, T′1 needs to satisfy the following condition.

−U1r<T′1<U1r  (23)

As a result, the stiffness of the joint 105 coincides with the joint stiffness command value U1, and at the same time, the joint 105 is followed and controlled at the target angle ra based on the torque command value T′1. As illustrated in the block diagram of FIG. 6, the control unit 150A can execute a restriction process 156 of restricting the control input as follows to realize Formula (23).

|T′1|<U1r  (24)

Alternatively, there is a method of changing the gain of the PID control step KPID1 to set the size of the control input within the range of Formula (23).

The first embodiment has illustrated that the stiffness lower limit V1 can be set to control the joint stiffness. However, if the gain of the feedback control system is high, the stiffness of the joint 105 becomes dominant.
Therefore, as illustrated in FIG. 6, the control unit 150A introduces a touch gain Gt. If there is a touch, Gt=0 is set to cut off the feedback system. As a result, the stiffness of the link 101 can be quickly switched to the joint stiffness command value U1 at arbitrary time, such as at the detection of contact to a human or an object by the hand.
In the present second embodiment, a transfer function used in the PID control step KPID1 is as follows.




  
    
      
        
          
            K
            
              PID
              ⁢
              
                
              
              ⁢
              1
            
          
          ⁡
          
            (
            s
            )
          
        
        =
        
          (
          
            25
            +
            
              7
              s
            
            +
            
              
                5
                ⁢
                s
              
              
                
                  1
                  /
                  
                    (
                    
                      100
                      ×
                      2
                      ×
                      π
                    
                    )
                  
                
                ⁢
                s
                ⁢
                
                  
                
                ⁢
                1
              
            
          
          )
        
      
    
    
      
        (
        25
        )
      
    
  




(3) Simulation
The control system delivered in the previous section is used to perform a simulation. It is assumed that the parameters of the link 101 and the target trajectory ra are similar to those of the first embodiment. In the present second embodiment, to verify the control performance for the identification error of the model, the inertia moment of the link 101 is set to I′=1.05I, and a model with an error relative to the model at the generation of the feedforward control input is used for the simulation. In the present second embodiment, if the lower limit of the joint stiffness command is small as in the first embodiment, the range of the restriction of the feedback control system is reduced. As a result, a wind-up phenomenon easily occurs. Therefore, V1=0.1 is set to deliver the feedforward input.
FIG. 7A illustrates the angle θ and the target trajectory ra of the joint 105 by a solid line and a dashed line, respectively. FIG. 7B illustrates responses of the driving force command values ue1 and uf1 by a solid line and a dashed line, respectively. FIG. 7C illustrates the difference between the driving force command values ue1 and uf1 by a solid line, the sum by a dashed line, and the response of T′1/r by an alternate long and short dash line. FIG. 7D illustrates TFF and TFB by a solid line and a dashed line, respectively.
It can be recognized from FIG. 7A that the angle of the joint 105 indicated by the solid line overlaps with the target trajectory ra indicated by the dashed line and that the joint 105 follows the target trajectory ra. The trajectory cannot be followed only by the feedforward control system due to the model error. However, the feedback control input compensates the model error as illustrated by the dashed line of FIG. 7D, and the trajectory can be followed. It can be recognized from FIG. 7B that driving force command values ue1 and uf1 satisfy the characteristic that the contractile force of the artificial muscle can only be a positive value, because the control input T′1 of the two-degree-of-freedom control system is distributed based on the conditions illustrated in Formulas (22) and (24). In FIG. 7C, uf1−ue1 indicated by the solid line overlaps with T′1/r indicated by the alternate long and short dash line, and the condition of Formula (20) is satisfied. As in the first embodiment, it can be recognized that uf1+ue1=U1 indicated by the dashed line is in the proximity of the absolute value of T′1/r indicated by the solid line and is always a minimum value in the range satisfying the restriction of Formula (24). According to the two-degree-of-freedom control system illustrated in the present second embodiment, it can be recognized that antagonistic driving with the minimum contractile force can be realized while realizing the following of the target trajectory ra, even if there is an identification error in the model.
Third Embodiment
A robot apparatus according to a third embodiment of the present invention will be described in detail. FIG. 8 is an explanatory view illustrating a schematic configuration of the robot apparatus according to the third embodiment of the present invention. The present third embodiment describes an example of control of hand stiffness of a robot apparatus 200 as a two-link manipulator with three pairs (six muscles) of artificial muscle actuators.
(1) Modeling
The robot apparatus 200 illustrated in FIG. 8 includes a pulley 203, a first link 201 pivotally connected to the pulley 203 through a first joint 211, and a second link 202 pivotally connected to the first link 201 through a second joint 212.
The first link 201 is made of a longitudinal member, and a base end 201a of the first link 201 is pivotally supported by the pulley 203 in a plane of an x-y rectangular coordinate system (hereinafter, called “work plane”). The second link 202 is made of a longitudinal member, and a base end 202a of the second link 202 is pivotally supported by a tip 201b of the first link 201 in the work plane.
A tip (hereinafter, called “link tip”) 202b of the second link 202 includes an end effector (for example, hand) not illustrated. More specifically, the first link 201 is arranged between the first joint 211 and the second joint 212 and is pivotally supported by the first joint 211. The second link 202 is pivotally supported by the second joint 212.
In the present second embodiment, the pulley 203 serves as the base body of the first link 201, and the first link 201 serves as the base body of the second link 202. The pulley 203 is arranged on, for example, the robot body.
The robot apparatus 200 includes a pair of first actuators e1 and f1, a pair of second actuators e2 and f2, and a pair of third actuators e3 and f3. One end of each of the first actuators e1 and f1 is connected to the pulley 203, and the other end is connected to a center section in the longitudinal direction of the first link 201. The first actuators e1 and f1 are antagonistically arranged so that the first link 201 pivots based on the difference in the driving force.
One end of each of the second actuators e2 and f2 is connected to the center section in the longitudinal direction of the first link 201, and the other end is connected to the base end 202a of the second link 202. The second actuators e2 and f2 are antagonistically arranged so that the second link 202 pivots based on the difference in the driving force. One end of each of the third actuators e3 and f3 is connected to the pulley 203, and the other end is connected to the base end 202a of the second link 202. The third actuators e3 and f3 are antagonistically arranged so that the first link 201 and the second link 202 pivot based on the difference in the driving force. More specifically, the first actuators e1 and f1 are symmetrically arranged on both sides of the first link 201 across the first link 201. The second actuators e2 and f2 are symmetrically arranged on both sides of the first link 201 across the first link 201. The third actuators e3 and f3 are symmetrically arranged on both sides of the first link 201 across the first link 201.
The robot apparatus 200 further includes a control unit 250 that sets the driving force of the actuators e1, f1, e2, f2, e3 and f3 based on driving force command values to control the operations of the links 201 and 202.
The first actuators e1 and f1 are first mono-articular drive actuators that drive the first link 201. The second actuators e2 and f2 are second mono-articular drive actuators that drive the second link 202. The third actuators e3 and f3 are bi-articular simultaneous drive actuators that simultaneously drive the first link 201 and the second link 202. Upper arms and thighs of humans are known to include bi-articular simultaneous drive actuators called bi-articular muscles. The muscle arrangement of the extremities of humans is complicated. An effective muscle concept is introduced, and a two-link model with six muscles (three pairs) is presented.
The actuators e1, f1, e2, f2, e3 and f3 are artificial muscle actuators with viscoelasticity characteristics of muscle illustrated in FIG. 1. The artificial muscle actuators are actuators with characteristics similar to the characteristics called viscoelasticity of muscle. As illustrated in FIG. 1, the muscle is modeled using a force generation element, an elasticity element and a viscosity element.
For the actuators e1, f1, e2, f2, e3 and f3 of FIG. 8, uen and ufn (n=1, 2, 3) are driving force command values for generating the driving force of the force generation elements. Furthermore, ken, kfn, ben and bfn (n=1, 2, 3) are elastic force constants and viscous force constants of the artificial muscle actuators. Angles of the first and second links 201 and 202, i.e. angles of the joints, are defined as θ1 and θ2. Inertia moments of the first and second links 201 and 202 are defined as I1 and I2. Lengths of the first and second links 201 and 202 are defined as 2×11 and 2×12. Mass of the first and second links 201 and 202 is defined as m1 and m2.
A length of the moment arm diameter, i.e. a length between the pivot center point of the first link 201 and the connection point of the actuators e1 and f1 at the pulley 203 and a length between the pivot center point of the second link 202 and the connection point of the actuators e2 and f2 at the second link 202, is defined as r.
When the elastic force constants and the viscous force constants of the muscles are defined as k and b in the present third embodiment, equations of motion are as in the following Formulas (26) and (27).

(m1l12+1+4m2l12+4m2l1l2 cos θ2+m2l22+I2){umlaut over (θ)}1+(m2l22+I2+2m2l1l2 cos θ2){umlaut over (θ)}2−2m2l2l2(2{dot over (θ)}1+{dot over (θ)}2)sin θ2{dot over (θ)}2=(uf1−ue1)r−(uf1+ue1)kr2θ1−(uf1+ue1)br2θ1+(uf3−ue3)r−(uf3+ue3)kr2(θ1+θ2)−(uf3+ue3)br2({dot over (θ)}1+{dot over (θ)}2)  (26)

(m2l22+I2+2m2l1l2 cos θ2){umlaut over (θ)}2+2m2l1l2 sin 02{dot over (θ)}12=(uf2−ue2)r−(uf2+ue2)kr2θ2−(uf2+ue2)br2θ2+(uf3−ue3)r−(uf3+ue3)kr2(θ1+θ2)−(uf3+ue3)br2({dot over (θ)}1−{dot over (θ)}2)  (27)

(2) Control System Design
The trajectory control of the joint that minimizes the contractile force (driving force) are performed in the first and second embodiments, and a two-degree-of-freedom control system is designed in the present third embodiment.
FIG. 9 illustrates a block diagram of a control unit 250 of the present third embodiment. FIG. 10 shows a block diagram of the feedforward control. A block indicated by KFFW is the feedforward control system. The control unit 250 executes a torque command value computation step 251, a change computation step 252, an iterative step 253, a corrected torque command value computation step 254 and a driving force command value computation step 255. In the present third embodiment, the control unit 250 obtains driving force command values uen and ufn of the actuators en and fn necessary for the angles of the joints 211 and 212 to follow the target trajectory. The control unit 250 controls the actuators en and fn to cause the driving force generated in the actuators en and fn to coincide with the driving force command values uen and ufn.
(2.1) Feedforward Control System
When the difference in the driving force command values is defined as Tn and the sum is defined as Un as in the first embodiment, the following can be formed.

(ufn−uen)r=TFFn+TFBn=Tn,n=1,2,3  (28)

ufn+uen=Un,n=1,2,3  (29)

Here, TFFn and TFBn denote torque command values provided by the feedforward control system and the feedback control system, respectively. In this section, TFBn=0 is set to illustrate a delivery method of the feedforward control input.

When Formulas (26) and (27) are defined by θv=[θ1 θ2]T and Tv=[T1+T3 T2+T3]T to write matrices, the following is formed.

Mm(θv){umlaut over (θ)}v+Cm(θv,{dot over (θ)}v){dot over (θ)}v=−Kv(θv−θcv)−Cv{dot over (θ)}v+Tv  (30)

Here, Kv and Cv denote a stiffness matrix and a damping matrix based on the viscoelasticity of muscle, respectively, and are expressed as follows.




  
    
      
        
          
            K
            v
          
          =
          
            
              [
              
                
                  
                    
                      
                        U
                        1
                      
                      +
                      
                        U
                        3
                      
                    
                  
                  
                    
                      U
                      3
                    
                  
                
                
                  
                    
                      U
                      3
                    
                  
                  
                    
                      
                        U
                        2
                      
                      +
                      
                        U
                        3
                      
                    
                  
                
              
              ]
            
            ⁢
            
              kr
              2
            
          
        
        ,
        
          
            C
            v
          
          =
          
            
              [
              
                
                  
                    
                      
                        U
                        1
                      
                      +
                      
                        U
                        3
                      
                    
                  
                  
                    
                      U
                      3
                    
                  
                
                
                  
                    
                      U
                      3
                    
                  
                  
                    
                      
                        U
                        2
                      
                      +
                      
                        U
                        3
                      
                    
                  
                
              
              ]
            
            ⁢
            
              br
              2
            
          
        
      
    
    
      
        (
        31
        )
      
    
  




Furthermore, θcv denotes a vector including neutral posture angles θc1 and θc2 based on the elastic force of muscle when the joint torque T1 and T2 do not act and is expressed as follows.

θcv=[θc1θc2]T  (32)

The target trajectory relative to the joint angle θn (n=1, 2) is defined as ran (n=1, 2) to define as follows.

rav=[ra1ra2]T  (33)

The angular velocity and the angular acceleration of the target trajectory are expressed as follows, respectively.

A feedforward torque command value necessary for the joint angle to follow the target trajectory is expressed by TFFv=[TFF1+TFF3 TFF2+TFF3]T. The torque command value TFFv can be expressed as follows based on inverse dynamics of the link.

TFFv=Mm(rav){umlaut over (r)}av+Cm(rav,{dot over (r)}av){dot over (r)}av+Kv(rav−θcv)+Cv{dot over (r)}av  (34)

However, the number of control inputs of the manipulator with the bi-articular simultaneous drive actuators is redundant relative to the controlled degree of freedom. Therefore, although torque command values relative to each joint 211 and 212 (hereinafter, abbreviated “each joint torque”) can be obtained based on inverse dynamics, commands TFFn (n=1, 2, 3) including the torque by the bi-articular drive actuators cannot be uniquely determined. Thus, the joint torque command obtained from the right side of Formula (34) is defined as TMF=[TMF1 TMF2]T, and each joint torque command TMF is distributed to minimize the following maximum values of the absolute values of TFFn (n=1, 2, 3).

max(|TFFn|,n=1,2,3)  (35)

The relationship between TMF and TFFn (n=1, 2, 3) is as in the following Formulas (36) and (37).

TFF1+TFF3=TMF1  (36)

TFF2+TFF3=TMF2  (37)

If TMF1 and TMF2 have the same signs, TFF3 can be increased or decreased to increase or decrease TFF1 and TFF2 by the amount of change in TFF3 based on Formulas (36) and (37). If TMF1≦TMF2, TFF3 can be gradually increased or decreased from 0 to set TFF1=TFF3. In this case TFFn (n=1, 2, 3) can be calculated as follows.




  
    
      
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              T
              
                MF
                ⁢
                
                  
                
                ⁢
                1
              
            
            2
          
        
        ,
        
          
            F
            
              FF
              ⁢
              
                
              
              ⁢
              2
            
          
          =
          
            
              
                2
                ⁢
                
                  
                
                ⁢
                
                  T
                  
                    MF
                    ⁢
                    
                      
                    
                    ⁢
                    2
                  
                
              
              -
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  1
                
              
            
            2
          
        
        ,
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              3
            
          
          =
          
            
              T
              
                MF
                ⁢
                
                  
                
                ⁢
                1
              
            
            2
          
        
      
    
    
      
        (
        38
        )
      
    
  




On the other hand, if TMF2>TMF1, TFF2=TFF3 can be set, and TFFn (n=1, 2, 3) can be calculated as follows.





  
    
      
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                2
                ⁢
                
                  
                
                ⁢
                
                  T
                  
                    MF
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              -
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
              
            
            2
          
        
        ,
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              2
            
          
          =
          
            
              T
              
                FF
                ⁢
                
                  
                
                ⁢
                3
              
            
            =
            
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
              
              2
            
          
        
      
    
    
      
        (
        39
        )
      
    
  




If TMF1 and TMF2 have different signs, for example, although an increase in TFF3 reduces the absolute value of TFF1, the absolute value of TFF2 increases. Therefore, TFF1 and TFF2 can be set as follows.

|TFF1|=|TFF2|  (40)

If TMF1 and TMF2 have different signs, and Formula (40) is satisfied, TFF1 and TFF2 have different signals. Therefore, the following can be obtained based on Formulas (36), (37) and (40).





  
    
      
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              1
            
          
          =
          
            
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  1
                
              
              -
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
              
            
            2
          
        
        ,
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              2
            
          
          =
          
            
              
                -
                
                  T
                  
                    MF
                    ⁢
                    
                      
                    
                    ⁢
                    1
                  
                
              
              +
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
              
            
            2
          
        
        ,
        
          

        
        ⁢
        
          
            T
            
              FF
              ⁢
              
                
              
              ⁢
              3
            
          
          =
          
            
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  1
                
              
              +
              
                T
                
                  MF
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
              
            
            2
          
        
      
    
    
      
        (
        41
        )
      
    
  




This can be used to obtain, based on the iterative computation algorithm, the torque command value TFFn and the joint stiffness command value Un that satisfy the following condition of generating the force only in the contraction direction, as in the first embodiment.

|TFFn|<Unr,n=1,2,3  (42)

As in the first embodiment, the number of iterations is defined as i in the present third embodiment, and the torque command values TFFn and the joint stiffness command values Un based on iterative computations of i times are written as follows.

TFFn[i],Un[i],n=1,2,3  (43)

A stiffness matrix and a damping matrix based on the viscoelasticity of muscle including Un[i] as elements will be written as Kv[i] and Cv[i]
In the torque command value computation step 251, the control unit 250 uses the target trajectory, the angular velocity of the target trajectory, the angular acceleration of the target trajectory, and the joint stiffness command value to compute the torque command values TFFn[F] indicating the torque necessary for the joints 211 and 212, based on inverse dynamics of the links 201 and 202. More specifically, the control unit 250 computes the torque command value TFFn[i] of an i-th iteration based on Formula (44) (torque command value computation step).

TMFn[i]−Mm(rav){umlaut over (r)}av+Cm(rav,{dot over (r)}av){dot over (r)}av+Kv[i](rav−θcv)+Cv[i]{dot over (r)}av  (44)

The control unit 250 distributes Formula (44) based on the method illustrated in Formulas (38) to (41) to calculate TFFn[i] (n=1, 2, 3)
In the change computation step 252, the control unit 250 computes a difference between the joint stiffness command value Un[i] and a value |TFFn[i]/r obtained by dividing the absolute value of the torque command value TFFn[i] by the moment arm radius r of the link. The difference is defined as En[i], and the control unit 250 computes the difference E1[i] based on the following Formula (45).
In the present third embodiment, the stiffness lower limit relative to the joint stiffness command value Un is defined as Vn, and the difference En[i] relative to the condition of Formula (42) is defined as follows.

En[i]=Un[i]−|TFFn[i]|/r−Vn,n=1,2,3  (45).

In the change computation step 252, the control unit 250 performs a computation of subtracting a value, which is obtained by multiplying the difference En[i] by a convergence factor γ greater than 0 and equal to or smaller than 1, from the joint stiffness command value Un[i] to change the joint stiffness command value Un[i] (change computation step). More specifically, the control unit 250 sets the convergence factor γ of the iterative computation algorithm to 0<γ≦1 to calculate an i+1-th joint stiffness command value Un[i|1] based on the following Formula (46)

Un[i+1]=Un[i]−γEn[i],n=1,2,3  (46)

In the iterative step 253, the control unit 250 iterates the computations of Formulas (44) to (46) for a predetermined number of times m until the difference En[i] converges to equal to or smaller than a predetermined value (for example, 0) (iterative step).
In a block of “Distribution Algorithm” illustrated in FIG. 10, the distribution algorithm illustrated in Formulas (38) to (41) is executed. In a block of “Iteration Algorithm”, the iterative computation algorithm of Formulas (44) to (46) is executed.
The driving force command value computation step 255 (FIG. 9) in the case of only the feedforward control (i.e. when TFBn=0) will be described. The torque command value is Tn=TFFWn.
In the driving force command value computation step 255 after the end of the iterative step 253, the control unit 250 uses the joint stiffness command value Un[m] and the torque command value Tn[m] to compute the driving force command values uen and ufn for computing the driving force command values (driving force command value computation step). As a result, the torque corresponding to the torque command values Tn is applied to the joints 211 and 212, and the angles θ1 and θ2 of the joints 211 and 212 follow the target trajectory.
(2.2) Two-Degree-of-Freedom Control System
When the feedback control is also performed along with the feedforward control, the control unit 250 executes PID control steps KPID1 to KPID3 illustrated in FIG. 9 in the corrected torque command value computation step 254 (corrected torque command value computation step).
The control unit 250 uses, as the torque command value Tn, a result obtained by adding the control input torque TFBn to the feedforward input TFFn. In this case, the control unit 250 applies a restriction based on Formula (30) in the restriction step 256 as in the second embodiment and calculates the contractile force ufn and uen.
Specifically, the control unit 250 computes the control input torque TFB1 and TFB2 for compensating the difference between the joint angles θ1, θ2, and the target trajectories ra1, ra2 in the PID control steps KPID1 and KPID2 as the feedback control system illustrated in FIG. 9. In the PID control step KPID3, the control unit 250 computes control input torque TFB3 for compensating the difference between the joint angles θ1+θ2 and the target trajectories ra1+ra2.
As in the second embodiment, the control input Tn (n=1, 2, 3) is restricted as follows.

|Tn|<Unr,n=1,2,3  (47)

The driving force command value ufn and uen (n=1, 2, 3) is expressed as follows.





  
    
      
        
          
            u
            fn
          
          =
          
            
              
                U
                n
              
              +
              
                
                  T
                  n
                
                /
                r
              
            
            2
          
        
        ,
        
          
            u
            en
          
          =
          
            
              
                U
                n
              
              -
              
                
                  T
                  n
                
                /
                r
              
            
            2
          
        
      
    
    
      
        (
        48
        )
      
    
  




(2.3) Hand Stiffness Control
Since the hand directly touches the outside world in the two-link manipulator, the control of the stiffness of the hand is important. The stiffness of the hand is expressed by a stiffness ellipse as illustrated in FIG. 11. The ellipse illustrates a distribution of the stiffness relative to each direction. The greater the distance between the hand and the ellipse, the higher the stiffness. In the one-link manipulator of the first embodiment, the stiffness of the joint and the stiffness of the hand are the same. In the two-link manipulator with six muscles (three pairs) of the present third embodiment, the stiffness of the hand is obtained by controlling the joint stiffness command values U1, U2 and U3. For example, if the joint stiffness command values U1, U2 and U3 are controlled by

U1=U2=U3  (49),

it is known that the major axis of the stiffness ellipse faces the direction connecting the first joint and the hand. Since the computation of the stiffness is iterated to minimize the contractile force in the present third embodiment, the stiffness during the drive cannot be arbitrarily designated. However, the hand stiffness can be controlled at the target position of the hand. In the present third embodiment, the lower limit of the joint stiffness command value is changed as illustrated in FIG. 12, and the stiffness is controlled to satisfy Formula (49) at the target position of the hand.

(2.4) Trajectory Design
In the present third embodiment, the target trajectory is set to drive the hand forward on the y axis of FIG. 8. As in the first embodiment, the acceleration section, the constant velocity section, and the deceleration section are included. The target trajectories ra1 and ra2 relative to the joint angle are obtained from the hand trajectory based on inverse kinematics. Although the trajectory including the acceleration section, the constant velocity section and the deceleration section is used in the present third embodiment, a trajectory, such as a minimum jerk trajectory, that does not include the constant velocity section may also be used.
(3) Simulation
A simulation using the control system of the previous section is performed. The physical parameters of the first link 201 and the second link 202 are the same. The link length is 0.2 m, the inertia moment of the link is I1=I2=1.3×10−3 kgm2, the moment arm diameter is 0.05 m, and the elastic force and viscous force constants are k=12 and b=0.003. For the target trajectory, the start time of the constant velocity section is ta=0.2747 sec, the end time of the constant velocity section is tb=0.4746 sec, and the positioning end time is tf=0.75 sec. The neutral posture angles θc1 and θc2 based on the elastic force when the joint torque does not act are joint angles at the halfway point of the hand trajectory, which are θc1=29.7 deg and θc22=120.7 deg. As in the second embodiment, verification for the identification error of the model in the two-degree-of-freedom control system is performed in the present first embodiment. Therefore, the inertia moments are defined as I1 and I2 in the generation of the feedforward input. In the simulation, the inertial moments of the links are I1′=1.1I1 and I2′=1.05I2.
FIGS. 13A and 13B illustrate the joint angles θ1 and θ2 of the first and second links by solid lines and illustrate the target trajectories ra1 and ra2 by dashed lines. FIGS. 14A to 14C illustrate responses of the driving force command value uf1, uf2 and uf3 by solid lines and illustrate responses of the driving force command value ue1, ue2 and ue3 by dashed lines. FIGS. 15A to 15C illustrate differences in the driving force command value uen and ufn (n=1, 2, 3) by solid lines, sums by dashed lines, and responses of Tn/r (n=1, 2, 3) by alternate long and short dash lines. FIG. 16 illustrates TFB1, TFB2 and TFB3 by solid lines, dashed lines and alternate long and short dash lines, respectively.
It can be recognized from FIGS. 13A and 13B that the joint angles indicated by the solid lines overlap with the trajectories indicated by the dashed lines and that the links follow the target trajectories. As in the second embodiment, this is because the feedback control input compensates the model errors as illustrated in FIG. 16. It can be recognized from FIG. 14A that the driving force command value ue1 and uf1 satisfy the characteristic that the contractile force of the artificial muscle is a positive value, because the control input T1 of the two-degree-of-freedom control system is distributed based on the conditions illustrated in Formulas (47) and (48). It can be recognized from FIGS. 14B and 14C that the driving force command value ue2, uf2, ue3 and uf3 similarly indicates positive values.
In FIG. 15A, uf1−ue1 indicated by the solid line overlaps with T1/r indicated by the alternate long and short dash line, and the condition of Formula (28) is satisfied. Similarly, U2 and U3 indicated by solid lines overlap with T2/r and T3/r indicated by alternate long and short dash line in FIGS. 15B and 15C. In FIGS. 15A to 15C, uf1+ue1=U1, uf2+ue2=U2, uf3+ue3=U3 indicated by dashed lines are in the proximity of the absolute values of T′1r indicated by solid lines in sections where Vn (n=1, 2, 3) at 0 to 0.625 seconds set in FIG. 12 is close to 0. It can be recognized from this that the minimum value is always set in the range satisfying the restriction of Formula (47) as in the first and second embodiments. Subsequent to 0.65 seconds at which Vn increases, the stiffness of each antagonistic pair is shifted to a larger value relative to the absolute value of T′1/r (n=1, 2, 3). Therefore, it can be recognized that the stiffness ellipse of the hand is controlled to satisfy the condition of Formula (49) in the target trajectory. According to the proposed method of the present third embodiment, it can be recognized that the stiffness of the hand can be controlled at the target position by the elasticity of six muscles (three pairs) while realizing the following of the target trajectory with the minimum contractile force, even if there is an identification error in the model.
The present invention is not limited to the embodiments described above, and a person with an ordinary skill in the field can make many modifications within the technical scope of the present invention.
While the present invention has been described with reference to exemplary embodiments, it is to be understood that the invention is not limited to the disclosed exemplary embodiments. The scope of the following claims is to be accorded the broadest interpretation so as to encompass all such modifications and equivalent structures and functions.
This application claims the benefit of Japanese Patent Application No. 2012-028614, filed Feb. 13, 2012, which is hereby incorporated by reference herein in its entirety.

## Claims

1. A robot apparatus comprising:
a link pivotally connected to a base body via a joint and having an arm;
a pair of actuators that generate driving forces for pulling the link in mutually opposite directions relative to the base body; and
a control unit configured to control the driving forces of the pair of actuators in accordance with a stiffness command value representing a target stiffness of the link and a torque command value representing a target torque value of the link,
wherein the control unit determines, through iterative calculations, a stiffness command value and a torque command value that satisfy a relationship where the stiffness command value is greater than the absolute value of the torque command value divided by a length of the arm.

2. The robot apparatus according to claim 1, wherein the iterative calculations include iterating computations of a torque command value computation process and a change computation process until the difference between the two converges to a value equal to or smaller than a predetermined value,
wherein the torque command value computation process includes using a target trajectory, an angular velocity of the target trajectory, an angular acceleration of the target trajectory, and a joint stiffness command value to calculate the torque command value, and
wherein the change computation process includes computing a difference between the joint stiffness command value and a first value, which is obtained by dividing an absolute value of the torque command value by a moment arm radius of the link, and performing a computation of subtracting a second value, which is obtained by multiplying the difference between the joint stiffness value and the first value by a coefficient greater than 0 and equal to or smaller than 1, from the joint stiffness command value to change the joint stiffness command value.

3. The robot apparatus according to claim 2, wherein the control unit computes a corrected torque command value for compensating a difference between the angle of the joint and a target trajectory, and in computing a driving force command value for controlling the target driving force, uses a result obtained by adding the corrected torque command value obtained in the corrected torque command value computation process to the torque command value obtained in the torque command value computation process as the torque command value used for the computation of each of the driving force command values.

4. The robot apparatus according to claim 1, wherein the driving forces determined by the control unit in accordance with the stiffness command value and the target stiffness command value are possible minimum values.

5. The robot apparatus according to claim 1, wherein the base body is a pivotable link.

6. The robot apparatus according to claim 5, further comprising two pairs of actuators configured to effect contraction forces on the link and the pivotable link such that they rotate in opposite directions.

7. A method for controlling a robot apparatus comprising a link pivotally connected to a base body via a joint and having an arm, and a pair of actuators that generate driving forces for pulling the link in mutually opposite directions relative to the base body, the method comprising
a determining step of determining, through iterative computations, a stiffness command value and a torque command value that satisfy a relationship where the stiffness command value is greater than the absolute value of the torque command value divided by a length of the arm; and
a controlling step of controlling the driving forces of the pair of actuators in accordance with the stiffness command value representing a target stiffness of the link and the torque command value representing a target torque value of the link.

8. The method according to claim 7 wherein the iterative calculations include iterating computations of a torque command value computation process and a change computation process until the difference between the two converges to a value equal to or smaller than a predetermined value,
wherein the torque command value computation process includes using a target trajectory, an angular velocity of the target trajectory, an angular acceleration of the target trajectory, and a joint stiffness command value to calculate the torque command value, and
wherein the change computation process includes computing a difference between the joint stiffness command value and a first value, which is obtained by dividing an absolute value of the torque command value by a moment arm radius of the link, and performing a computation of subtracting a second value, which is obtained by multiplying the difference between the joint stiffness command value and the first value by a coefficient greater than 0 and equal to or smaller than 1, from the joint stiffness command value to change the joint stiffness command value.

9. The method according to claim 8, wherein the controlling step further comprises computing a corrected torque command value for compensating a difference between the angle of the joint and a target trajectory, and in computing a driving force command value for controlling the target driving force, uses a result obtained by adding the corrected torque command value obtained in the corrected torque command value computation process to the torque command value obtained in the torque command value computation process as the torque command value used for the computation of each of the driving force command values.

10. The method according to claim 7, wherein the driving forces determined in accordance with the stiffness command value and the target stiffness command value are possible minimum values.

11. The method according to claim 7, wherein the base body is a pivotable link.

12. The method according to claim 11, wherein said robot apparatus further comprises two pairs of actuators configured to effect contraction forces on the link and the pivotable link such that they rotate in opposite directions.