JP2014004654A - Robot and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To perform antagonistic driving with a minimal contractile force command value in trajectory control of a joint angle of two links in which an artificial muscle actuator is used as a driving source.SOLUTION: A contractile force command value arithmetic unit 152 hypothesizes in which of directions of bending and stretching of a link each of a first single-joint muscle, second single-joint muscle, and two-joint muscle exerts a torque. The contractile force command value arithmetic unit 152 deduces a command value candidate for a contractile force command value by performing discrimination based on a joint torque command value, target joint angle, and target joint angular velocity on a hypothetical basis. The contractile force command value arithmetic unit 152 repeats the processing for each of combinations of the directions in which each of the muscles exerts a torque, and selects one of command value candidates whose maximum value is minimal.

Description

  The present invention relates to a robot in which artificial muscle actuators are antagonistically arranged with respect to a link and a robot control method for controlling the robot.

  In the manipulator control method, it is important that the hand can flexibly contact the object. If this is applied to an industrial robot, collaborative work with humans can be realized, and parts fitting work and the like can be facilitated by controlling the direction of flexibility of the hand. Moreover, if this is applied to a legged mobile robot, the impact on the body can be reduced by softly touching the ground, and it is possible to stably walk on rough terrain by absorbing the steps.

  In order to control the flexibility of the hand, impedance control for attaching a force sensor to the hand, control using an artificial muscle actuator, and the like are performed. It is known that the human muscle is not only an actuator but also a viscoelastic variable control mechanism. Among artificial muscles, pneumatic rubber artificial muscles represented by McKibben type artificial muscles are similar in muscle to viscoelastic properties. Therefore, by controlling the softness of the artificial muscle actuator in the robot, the object can be contacted with the flexibility of any hand. However, artificial muscle actuators have difficulty in controllability because they have non-linear viscoelastic characteristics and need to be antagonistically arranged to generate force only in the contraction direction. It is known that there is.

  Conventionally, a manipulator model including viscoelastic properties of muscles and a feed-forward input for simultaneously controlling joint angle and hand flexibility using a correction value calculation unit have been proposed (patent) Reference 1). In Patent Document 1, a joint angle and a viscoelastic coefficient of an artificial muscle actuator when a control input is given using a model are output and compared with a target value. Then, the error from the target value is propagated back to the correction value calculation unit, and the feedforward input is corrected. The operation of giving the corrected feedforward input to the model again is repeated, and the feedforward input is gradually obtained.

Japanese Patent No. 3436320

  In Patent Document 1, joint angle and hand flexibility are simultaneously controlled by feedforward control. However, there have been no research examples for antagonistically driving an artificial muscle actuator with a minimum control input.

  In order to minimize the control input, it is necessary to consider the target trajectory of the hand flexibility corresponding to the target trajectory of the joint angle and optimize it simultaneously with the control input. However, the feedforward control of Patent Document 1 previously gives a target trajectory for hand flexibility, and does not include an algorithm for optimizing it.

  SUMMARY OF THE INVENTION Accordingly, it is an object of the present invention to perform antagonistic driving with a minimum contraction force command value in trajectory control of a joint angle of two links using an artificial muscle actuator as a driving source.

  The robot of the present invention includes a first link connected to a base by a first joint, a second link connected to the first link by a second joint, and the first link around the first joint by contraction. And a second artificial muscle actuator for turning the first link around the first joint in a second turning direction opposite to the first turning direction by contraction. And a third artificial muscle actuator for turning the second link around the second joint in the first turning direction by contraction, and turning the second link around the second joint in the second turning direction by contraction. A fourth artificial muscle actuator that causes the first and second links to simultaneously rotate about the first and second joints in the first turning direction by contraction. A sixth artificial muscle actuator that simultaneously turns the first and second links around the first and second joints in the second turning direction by contraction, and a first joint torque command value of the first joint; Based on the first joint torque command value and the second joint torque command value, a first contraction of the first artificial muscle actuator based on the torque command value generation unit that generates a second joint torque command value of the second joint Force command value, second contraction force command value of the second artificial muscle actuator, third contraction force command value of the third artificial muscle actuator, fourth contraction force command value of the fourth artificial muscle actuator, fifth artificial force A contraction force command value calculating unit for obtaining a fifth contraction force command value of the muscle actuator and a sixth contraction force command value of the sixth artificial muscle actuator; The first artificial muscle actuator is contracted, the second artificial muscle actuator is contracted according to the second contraction force command value, the third artificial muscle actuator is contracted according to the third contraction force command value, The fourth artificial muscle actuator is contracted in accordance with the fourth contractile force command value, the fifth artificial muscle actuator is contracted in accordance with the fifth contractive force command value, and the fifth artificial muscle actuator is contracted in accordance with the sixth contractive force command value. A contraction force generation unit that contracts the six artificial muscle actuators, and the contraction force command value calculation unit uses the first joint torque command value as a constant, and the first, second, fifth, and sixth contraction forces In the first function established using the command value as a variable and the second function established using the second joint torque command value as a constant and the third, fourth, fifth and sixth contractile force command values as variables, When one of the first and second contraction force command values, one of the third and fourth contraction force command values, and one of the fifth and sixth contraction force command values are set to 0, the first and second contraction force command values The contraction force command value that is the maximum value among the other of the 2 contraction force command values, the other of the third and fourth contraction force command values, and the other of the fifth and sixth contraction force command values is minimized. A command value calculation process for calculating the other of the first and second contraction force command values, the other of the third and fourth contraction force command values, and the other of the fifth and sixth contraction force command values; And a command value output process for outputting the first to sixth contraction force command values calculated in the value calculation process to the contraction force generator.

  According to the present invention, the target trajectory of hand flexibility is uniquely determined, and each artificial muscle actuator is antagonistically driven with the minimum contraction force command value, so that the contraction force generator generates contraction force on each artificial muscle actuator. For example, the amount of consumption of air or the like required for making it possible can be minimized. Therefore, it is possible to extend the driving time of the robot.

It is explanatory drawing which shows the viscoelastic model of the artificial muscle actuator which concerns on 1st Embodiment of this invention. It is explanatory drawing which shows schematic structure of the robot which concerns on 1st Embodiment of this invention. It is a block diagram which shows the control apparatus of the robot which concerns on 1st Embodiment of this invention. It is a figure which shows the combination of the code | symbol of the muscular torque which concerns on 1st Embodiment of this invention. It is a flowchart which shows the processing operation of the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a flowchart of the 1st minimization calculation process by the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a graph which shows an example of straight line u _f1 ¹ , u _f2 ¹ , u _f3 ¹ . ¹⁰ is a graph showing another example of straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ . ¹⁰ is a graph showing another example of straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ . It is a flowchart of the 1st calculation process by the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a flowchart of the 2nd calculation process by the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a flowchart of the 3rd calculation process by the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a flowchart of the 4th calculation process by the contraction force command value calculating part which concerns on 1st Embodiment of this invention. It is a flowchart of the 1st command selection processing by the contraction force command value calculating part concerning a 1st embodiment of the present invention. It is a flowchart which shows the processing operation of the contraction force command value calculating part of the robot which concerns on 3rd Embodiment of this invention based on 2nd Embodiment of this invention. It is a flowchart of the 2nd minimization calculation process by the contraction force command value calculating part which concerns on 2nd Embodiment of this invention. It is a flowchart of the 5th calculation process by the contraction force command value calculating part which concerns on 2nd Embodiment of this invention. It is a flowchart of the 6th calculation process by the contraction force command value calculating part which concerns on 2nd Embodiment of this invention. It is a flowchart of the 7th calculation process by the contraction force command value calculating part which concerns on 2nd Embodiment of this invention. It is a flowchart of the 8th arithmetic processing by the contraction force command value calculating part which concerns on 2nd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 2nd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 2nd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 2nd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 2nd Embodiment of this invention. It is a figure showing the stiffness ellipse which concerns on 3rd Embodiment of this invention. It is a block diagram which shows the control apparatus of the robot which concerns on 3rd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 3rd Embodiment of this invention. It is a figure which shows the simulation result which concerns on 3rd Embodiment of this invention.

  Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings.

[First Embodiment]
In the first embodiment of the present invention, a control input for following a target trajectory of a joint angle is minimized for a two-link manipulator using an artificial muscle actuator as a drive source. In the first embodiment, the control input derivation method is shown only when the torques around the first and second joints required to follow the target trajectory are both positive numbers.

(1 Model derivation)
In this section, a model of a two-link manipulator using an artificial muscle actuator as a drive source is derived. The artificial muscle actuator used in the first embodiment is an actuator having characteristics similar to the viscoelastic characteristics of muscle, and uses a force generating element 11, an elastic element 12, and a viscous element 13 as shown in FIG. Modeled. The contraction force of the force generating element 11 is u, and the contraction amount of the muscle with the contraction direction being positive is x. Contraction speed,

とする。また、弾性力定数をｋ、粘性力定数をｃとし、筋収縮力をＦとする。このとき、筋の粘弾性特性は、

And The elastic force constant is k, the viscous force constant is c, and the muscle contraction force is F. At this time, the viscoelastic properties of the muscle are

と表わされる。人工筋肉アクチュエータは、収縮方向にのみ力を発生するため、リンクの角度を任意に制御するためには、人工筋肉アクチュエータを拮抗配置する必要がある。

It is expressed as Since the artificial muscle actuator generates a force only in the contraction direction, it is necessary to antagonistically arrange the artificial muscle actuator in order to arbitrarily control the link angle.

  FIG. 2 is an explanatory diagram showing a schematic configuration of the robot according to the first embodiment of the present invention. This robot 100 is a two-link manipulator. The robot 100 includes a pulley 103 as a base, a link (first link) 101 connected to the pulley 103 by a joint (first joint) 111, and a link connected to the link 101 by a joint (second joint) 112. (Second link) 102.

In the first embodiment, the robot 100 includes six artificial muscle actuators (hereinafter simply referred to as “actuators”) f ₁ , e ₁ , f ₂ , e ₂ , f ₃ , e ₃ and the actuators f ₁ , e 3. ₁ , f ₂ , e ₂ , f ₃ , and e ₃ .

Actuator f ₁ is the first artificial muscle actuators, first _(in FIG. 2, the counterclockwise direction, the direction of bending) turning direction R ₁ in the first link 101 about first joint 111 by shrinkage is pivoted. Actuator _{e 1} is a second artificial muscle actuators, second pivot direction opposite to the first turning direction _{R 1} of the first link 101 about the first joint 111 by shrinkage _{R 2} (in FIG. 2, the clockwise direction , Turn in the extending direction). Third actuator _{f 2} is an artificial muscle actuator, a second link 102 is rotated in the first rotation direction _{R 1} about the second joint 112 by contraction. Actuator _{e 2} is the fourth artificial muscle actuator, a second link 102 is pivoted to a second turning direction _{R 2} about the second joint 112 by contraction. Actuator _{f 3} is a fifth artificial muscle actuators, the first and second links 101 and 102 is simultaneously rotated in the first rotation direction _{R 1} about the first and second joint 111, 112 by contraction. Sixth actuator _{e 3} is an artificial muscle actuator, the first and second links 101 and 102 to simultaneously pivot about the first and second joint 111, 112 in the second turning direction _{R 2} by the contraction.

That is, the actuators f ₁ and e ₁ drive the first joint 111, the actuators f ₂ and e ₂ drive the second joint 112, and the actuators f ₃ and e ₃ drive the two joints 111 and 112 simultaneously.

Controller 150, each actuator _f i, _{e i} contractile force command value _{u fi,} seeking by calculation _{u ei,} the contractile force command value _{u fi,} each actuator _f i according to _{u ei,} shrink _{e i} (I = 1, 2, 3). Here, the contractile force command value of the actuator f _1, and the first contractile force command value u _f1. The contractile force command value of the actuator _{e 1,} the second contractile force command value _{u e1.} The contractile force command value of the actuator _{f 2,} and the third contractile force command value _{u f2.} The contractile force command value of the actuator _{e 2,} and the fourth contractile force command value _{u e2.} The contractile force command value of the actuator _{f 3,} and the fifth contractile force command value _{u f3.} The contractile force command value of the actuator _{e 3,} the sixth contractile force command value _{u e3.}

The amount of contraction x _i of the actuator f _i , e _i (i = 1, 2, 3) is obtained by using the moment diameter r and the joint angle θ _i (i = 1, 2, 3).

と表わせる。

It can be expressed as

Here, it is assumed that the elastic force constants of the actuators f _i and e _i are k _i , the viscous force constant is c _i , the joint angular velocity is ω _i , and the joint angular acceleration is α _i (i = 1, 2, 3). Torque (hereinafter referred to as muscle torque) τ _i (i = 1, 2, 3) generated by the contraction force command values u _fi , u _ei (i = 1, 2, 3) of the actuators f _i , e _i is

と表すことができる。ただし、関節角度θ_３は、

It can be expressed as. However, the joint angle θ ₃ is

と定義する。また、トルクの正の方向は、図２において反時計回り方向である第１旋回方向Ｒ_１とする。

It is defined as Further, the positive direction of the torque, the first rotation direction R ₁ is counterclockwise direction in FIG.

From equation (3), it can be seen that the difference in contraction force in the first term on the right side gives rotational torque to the joint, and the sum of the contraction forces in the second and third terms varies the elasticity and viscosity of the muscle. As described above, the artificial muscle actuators f _i and e _i (i = 1, 2, 3) exhibit only contraction force. Therefore, the contraction force command values u _fi and u _ei (i = 1, 2, 3) are

を満たさなければならない。

Must be met.

Here, the moments of inertia around the joints 111 and 112 of the first and second links 101 and 102 are I ₁ and I ₂ , the lengths of the links 101 and 102 are L ₁ and L ₂ , and the mass of the links 101 and 102 is m _1. , M ₂ . Also, torque command values (first joint torque command value and second joint torque command value) acting around the joints 111 and 112 are defined as T ₁ and T ₂ . The equation of motion of the two-link manipulator of the first embodiment is

となる。ただし、式（６）において、慣性行列は、

It becomes. However, in equation (6), the inertia matrix is

  Coriolis force and centrifugal force are

である。

It is.

  The inertia matrix and the Coriolis force / centrifugal force are expressed by the following equations (7) and (8).

In addition, the first joint torque command value T ₁ and the first joint torque command value T ₂ are obtained by using muscle torques τ ₁ , τ ₂ , and τ ₃ ,

となる。

It becomes.

According to the equations (3) and (9), the joint torque command values T ₁ and T ₂ are obtained by using the contraction force command values u _fi and u _ei .

となる。ただし、係数Ｋ_ｆ１，Ｋ_ｅ１，Ｋ_ｆ２，Ｋ_ｅ２，Ｋ_ｆ３，Ｋ_ｅ３は、

It becomes. However, the coefficients K _f1 , K _e1 , K _f2 , K _e2 , K _f3 , K _e3 are

と定義する（ｉ＝１，２，３）。

(I = 1, 2, 3).

That is, Expression (10) is established with the first joint torque command value T ₁ as a constant and the first, second, fifth, and sixth contractile force command values u _f1 , u _e1 , u _f3 , and u _e3 as variables. This is the first function. Equation (11), the second joint torque command value _{T 2} is a constant, holds the third, fourth, fifth and sixth contractile force command value _{u f2, u e2, u f3} , u e3 as a variable Two functions.

(2 Control system design)
In the control system of the first embodiment, the maximum value of the contractile force command values u _fi [k] and u _ei [k] is minimized based on the joint torque command values T ₁ [k] and T ₂ [k]. . Note that time k is a discrete signal, using a control cycle T _s and a positive integer n,

と表すことができる。なお、前述のように本第１実施形態では、関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］がどちらも正数となるときの収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］の導出方法について述べる。本第１実施形態の制御系は、例えば、重力により生じる負の方向のトルクに対して、動作を行うマニピュレータに対して適用することが可能である。

It can be expressed as. As described above, in the first embodiment, the contraction force command values u _fi [k] and u _ei [when the joint torque command values T ₁ [k] and T ₂ [k] are both positive numbers. A method for deriving k] will be described. The control system of the first embodiment can be applied to a manipulator that operates with respect to, for example, a negative torque generated by gravity.

  FIG. 3 is a block diagram showing the robot control apparatus according to the first embodiment of the present invention. In FIG. 3, the control device 150 includes a feedforward compensation unit 151 as a torque command value generation unit, a contraction force command value calculation unit 152, and a contraction force generation unit 153.

The feedforward compensation unit 151 inputs the target joint angle θ _ri [k], the target angular velocity ω _ri [k], and the target angular acceleration α _ri [k] (i = 1, 2) of the joints 111 and 112 at time k. And And

とする逆動力学演算により、関節角度を目標値に追従させるための関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］を演算する。つまり、フィードフォワード補償部１５１は、各関節１１１，１１２の目標関節角度θ_ｒｉ［ｋ］、目標角速度ω_ｒｉ［ｋ］、目標角加速度α_ｒｉ［ｋ］（ｉ＝１，２）に基づき、各関節１１１，１１２の関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］を生成する。

The joint torque command values T ₁ [k] and T ₂ [k] for causing the joint angle to follow the target value are calculated by inverse dynamics calculation. That is, the feedforward compensation unit 151 is based on the target joint angle θ _ri [k], the target angular velocity ω _ri [k], and the target angular acceleration α _ri [k] (i = 1, 2) of each joint 111, 112. Joint torque command values T ₁ [k] and T ₂ [k] for the joints 111 and 112 are generated.

Further, the contraction force command value calculation unit 152 inputs a target joint angle θ _ri [k], a target angular velocity ω _ri [k], and joint torque command values T ₁ [k], T ₂ [k], and inputs them. Based on this, the contraction force command values u _fi [k] and u _ei [k] having the minimum maximum values are obtained.

Contraction force generation unit 153, the contractile force command value _u fi _[k], each actuator _f i according to _u ei [k], to contract the _{e i.}

In the control system design, first, a problem for minimizing the maximum value of contraction force command values u _fi [k] and u _ei [k] (hereinafter referred to as a contraction force minimization problem) is defined, and the problem is solved. The control side is derived. Next, case classification is performed based on the coefficients K _fi and K _ei and the joint torque command values T ₁ [k] and T ₂ [k], and the contractile force command values u _fi [k] and u _ei [k] are calculated. . In the control system design of the first embodiment, [k] representing time is omitted for the sake of brevity.

(2.1 Simultaneous control of joint stiffness and contraction force)
First, the evaluation function is J, and J is defined as the following equation (15).

That is, J = max {u _f1 , u _e1 , u _f2 , u _e2 , u _f3 , u _e3 } is defined. The contraction force minimization problem means that when the joint torque command values T ₁ and T ₂ , the target joint angles θ _r1 and θ _r2, and the target joint angular velocities ω _r1 and ω _r2 are given, the formulas (3) to (3) This is a problem of _obtaining contractile force command values u _fi and u _ei that satisfy (5) and equation (9) and minimize the evaluation function J.

In the contraction force minimization problem, three formulas (10), (11), and (15) are the constraint conditions, but the number of contraction force command values u _fi and u _ei , which are unknown numbers, is six. In general, when the number of unknowns is larger than the number of constraints, the unknowns cannot be uniquely determined. However, the contractile force command values u _fi and u _ei are set according to the sign of the muscle torque τ _i .

と切り替える制御側を適用すると、収縮力最小化問題を解くことが可能となる。なぜならば、式（１６）の制御側により、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉのどちらか一方が必ず０となるため、未知数の数を３つに減らすことが可能となるためである。さらに、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉの一方を０とすることで、最小の収縮力で駆動することが可能となる。しかし、筋トルクτ_ｉは、係数Ｋ_ｆｉ，Ｋ_ｅｉを用いて、

If the control side to switch is applied, it becomes possible to solve the contraction force minimization problem. This is because one of the contraction force command values u _fi and u _ei is always 0 by the control side of the equation (16), so that the number of unknowns can be reduced to three. Furthermore, by setting one of the contraction force command values u _fi and u _ei to 0, it is possible to drive with the minimum contraction force. However, the muscle torque τ _i is calculated using the coefficients K _fi and K _ei

と表されるため、係数Ｋ_ｆｉ，Ｋ_ｅｉが正数となることが条件である。そのため、本第１実施形態では、マニピュレータが目標関節角度θ_ｒ１，θ_ｒ２と目標角速度ω_ｒ１，ω_ｒ２に従って運動するとき、係数Ｋ_ｆｉ，Ｋ_ｅｉが常に正数となるように、弾性力定数ｋ、粘性力定数ｃ、モーメントアーム径ｒを設計している。

Therefore, the condition is that the coefficients K _fi and K _ei are positive _numbers . Therefore, in the first embodiment, when the manipulator moves according to the target joint angles θ _r1 , θ _r2 and the target angular velocities ω _r1 , ω _r2 , the elastic force constant is set so that the coefficients K _fi and K _ei are always positive _numbers. k, viscous force constant c, and moment arm diameter r are designed.

In order to apply the control side shown in Expression (16), the positive and negative signs of the muscle torques τ ₁ , τ ₂ , and τ ₃ need to be determined. However, as shown in the equation (9), the muscle torques τ ₁ , τ ₂ , τ ₃ are redundant with respect to the joint torque command values T ₁ , T ₂ , and therefore the sign of the muscle torque from the sign of the joint torque command value. Cannot be uniquely determined.

Therefore, in the first embodiment, one of the combinations of signs of the muscle torques τ ₁ , τ ₂ , τ ₃ is assumed, and the contraction force command value u _fi , which minimizes the evaluation function J under this assumption, is assumed. A candidate for u _ei is derived. This is performed for all combinations of signs of the muscle torques τ ₁ , τ ₂ , and τ ₃ , and the contraction force that minimizes the evaluation function J is selected from the contraction force candidates.

FIG. 4 is a diagram illustrating eight combinations that can be taken by the signs of the muscle torques τ ₁ , τ ₂ , and τ ₃ . However, when the joint torque command values T ₁ and T ₂ are both positive numbers as in the first embodiment, the conditions 5, 6, 7, and 8 that the muscle torque τ ₃ is a negative number are used. There is no need to assume a combination. This is because if the muscle torque τ ₃ is a negative number, it is necessary to increase the muscle torque τ ₁ and the muscle torque τ ₂ from the equation (9) as compared to the case where τ ₃ is a positive number. This is because the contraction force of the second single joint muscle is increased.

Therefore, in the first embodiment, the contractile force command values u _fi and u _ei are derived only for the four combinations of condition 1, condition 2, condition 3, and condition 4.

Next, a robot control method by the control device 150 will be described. FIG. 5 is a flowchart showing the processing operation of the contraction force command value calculation unit. The contraction force command value calculation unit 152 inputs the joint torque command values T ₁ and T ₂ from the feedforward compensation unit 151, and receives target joint angles θ ₁ and θ ₂ , target joint angular velocity ω ₁ , to enter the ω ₂ (S1). The elastic force constant k, the viscous force constant c, and the moment arm diameter r are values set in advance.

Next, the contraction force command value calculation unit 152 calculates the coefficients K _fi and K _ei from the target joint angles θ ₁ and θ ₂ and the target joint angular velocities ω ₁ and ω ₂ using Expression (12) (S2). .

Next, the contraction force command value calculation unit 152 _{adds the} calculated coefficients K _fi and K _ei and the input joint torque command value T to the first function shown in Equation (10) and the second function shown in Equation (11). _{1, T 2} is substituted as a constant (S3).

  Next, the contraction force command value calculation unit 152 executes a first minimization calculation process for calculating a command value candidate that minimizes the evaluation function J (S4).

Next, the contraction force command value calculation unit 152 selects a command value candidate having the smallest evaluation function for the evaluation functions J _min ¹ to ⁴ and the command value candidates u _fi ^{1 to 4} and u _ei ¹ to ⁴ (S5). The first command selection process is executed (S6).

That is, the contraction force command value calculation unit 152 executes command value calculation processing for calculating the first to sixth contraction force command values u _fi and u _ei in S2 to S6 (command value calculation processing step). Specifically, the contraction force command value calculation unit 152 uses the first and second contraction force command values, the third and second contraction force command values in the first function shown in the equation (10) and the second function shown in the equation (11). One of the fourth contractile force command values and one of the fifth and sixth contractile force command values are set to zero. Then, the contraction force command value calculation unit 152 is the maximum of the other of the first and second contraction force command values, the other of the third and fourth contraction force command values, and the other of the fifth and sixth contraction force command values. These are calculated so that the contractile force command value as a value is minimized.

Next, the contraction force command value calculation unit 152 executes a command value output process for outputting the calculated first to sixth contraction force command values u _fi and u _ei to the contraction force generation unit 153 (S7: command value output) Processing step).

(2.2 Design of the first minimization calculation process)
FIG. 6 is a flowchart of the first minimization calculation process by the contraction force command value calculation unit. The contraction force command value calculation unit 152 assumes the first condition shown in FIG. 4 (S11), executes the first calculation process (S12), and minimizes the evaluation function J _min ¹ through the first to sixth contraction forces. First command value candidates u _fi ¹ and u _ei ¹ composed of command values are output (S13).

Subsequently, the contraction force command value calculation unit 152 assumes the second condition shown in FIG. 4 (S14), executes the second calculation process (S15), and minimizes the evaluation function J _min ² . The second command value candidates u _fi ² and u _ei ² composed of 6 contractile force command values are output (S16).

Subsequently, the contraction force command value calculation unit 152 assumes the third condition shown in FIG. 4 (S17), executes the third calculation process (S18), and minimizes the evaluation function J _min ³ . The third command value candidates u _fi ³ and u _ei ³ composed of 6 contractile force command values are output (S19).

Subsequently, contractile force command value computing unit 152 assumes the fourth condition shown in FIG. 4 (S20), and executes the fourth calculation process (S21), first to minimize the evaluation function _{J min} ⁴ The fourth command value candidates u _fi ⁴ and u _ei ⁴ composed of 6 contractile force command values are output (S22).

In the first, second, third, and fourth calculation processes, the signs of the muscle torques τ ₁ and τ ₂ are assumed to be the conditions 1, 2, 3, and 4 in FIG. This is a subroutine for calculating the candidate J _min ^j and the command value candidates u _fi ^j and u _ei ^j of the contraction force command value (j = 1, 2, 3, 4). Note that the superscript ^j of the evaluation function candidate J _min ^j and the contraction force command value candidates u _fi ^j and u _ei ^j represent the number of the corresponding arithmetic processing. In addition, since the first to fourth calculation processes are the same in the method of calculating the evaluation function candidate J _min ^j and the contraction force command value command value candidates u _fi ^j and u _ei ^j , The calculation method in one calculation process will be described in detail, and the calculation methods in the second to fourth calculation processes will be described briefly.

(2.2.1 Design of first arithmetic processing)
Among the first command value candidates u _fi ¹ , u _ei ¹ of the contraction force command value in the first calculation process, the first command value candidates u _e1 ¹ , u _e2 ¹ , u _e3 ¹ are represented by the condition 1 and the equation of FIG. (16)

となる。つまり、収縮力指令値演算部１５２は、式（１０）に示す第１関数及び式（１１）に示す第２関数において、ｕ_ｅ１ ^１＝０，ｕ_ｅ２ ^１＝０，ｕ_ｅ３ ^１＝０とする。従って、評価関数Ｊは、第１指令値候補ｕ_ｆ１ ^１、ｕ_ｆ２ ^１、ｕ_ｆ３ ^１の最大値となる。

It becomes. In other words, the contraction force command value calculation unit 152 uses u _e1 ¹ = 0, u _e2 ¹ = 0, u _e3 ¹ = 0 in the first function shown in Expression (10) and the second function shown in Expression (11). To do. Therefore, the evaluation function J is the maximum value of the first command value candidates u _f1 ¹ , u _f2 ¹ , u _f3 ¹ .

Further, from the equations (10), (11), and (18), the joint torque command values T ₁ , T ₂ are the first command value candidates u _f1 ¹ , u _f2 ¹ , u _f3 ¹ of the contraction force command values. Using,

と表すことができる。ここで、変数ｕ_ｆ３ ^１’を、

It can be expressed as. Where the variable u _f3 ¹ ′ is

と定義する。式（１９）及び式（２０）は変数ｕ_ｆ３ ^１’を用いて、

It is defined as Equation (19) and Equation (20) use the variable u _f3 ¹ ′,

と表すことができる。ただし、変数ｕ_ｆ３ ^１’の変域は、第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１が全て正数となる範囲である必要がある。

It can be expressed as. However, the range of the variable u _f3 ¹ ′ needs to be within a range where the first command value candidates u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ are all positive numbers.

It can be seen from the equations (21) to (23) that the first command value candidates u _f1 ¹ , u _f2 ¹ and u _f3 ¹ are all linear functions having u _f3 ¹ ′ as a variable. Therefore, in the first embodiment, the straight line u _f1 ¹ , u _f2 ¹ , u _f is the variable u _f3 ¹ ′ on the X axis and the first command value candidates u _f1 ¹ , u _f2 ¹ , u _f3 ¹ on the Y axis. The graph of _f3 ¹ is drawn. Then, the variable u _{f3 1} ^'to increase or decrease the X coordinate variables u _{f3 ^1'} by moving the Y-axis and a straight line parallel to the positive side and negative side of the X-axis, voted locus of intersection function J Find the minimum value of.

FIG. 7 is a graph illustrating an example of the straight lines u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ . The straight line u _f1 ¹ is represented by a dotted line, the straight line u _f2 ¹ is represented by a broken line, and the straight line u _f3 ¹ is represented by a solid line. In addition, a straight line parallel to the Y axis is represented by a one-dot chain line. Further, the range of the variable u _f3 ¹ ′ is represented by using an arrow. A point where the Y coordinate is maximum is defined as a point Z among the intersections of the straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ and the straight line parallel to the Y axis. Here, the Y coordinate of the intersection of the straight line parallel to the Y axis and the straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ is the first command value candidate u _f1 ¹ , u _f2 ¹ , u _f3 ¹ to be obtained. From this, it can be seen that the Y coordinate of the point Z represents the value of the evaluation function J.

In the example shown in FIG. 7, the evaluation function J is minimum at the intersection of the straight line u _f1 ¹ and the straight line u _f3 ¹ . This is because 'moves Decreasing the point Z according to the straight line _{u f1} ^1, the variable _{u f3} ^1' variable _{u f3} ¹ to move by increasing the accordance linear _{u f3} ^1, the value of even the evaluation function J in the case of either This is because of the increase.

However, the evaluation function J is not always minimized at the intersection of the straight lines u _f1 ¹ and u _f3 ¹ . This is because the X-intercept, Y-intercept of each straight line, and the X and Y coordinates of the intersection change depending on the coefficients K _f1 , K _f2 , K _f3 and the joint torque command values T ₁ , T _2. is there.

Therefore, in the first embodiment, a condition for dividing the case based on the X-intercept, the Y-intercept of each straight line, and the size of the X coordinate and the Y coordinate of the intersection is derived, and the evaluation function J is minimized in each case. Ask for. However, if both the straight line u _f1 ¹ and the straight line u _f2 ¹ are moved, the condition for case division becomes complicated. Therefore, in the first embodiment, for easy explanation, the X-intercept and the Y-intercept of the straight line u _f1 ¹ are fixed and illustrated. Then, graphs of the straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ when the X intercept and the Y intercept of the straight line u _f2 ¹ are gradually moved from the origin in the positive direction of the X axis and the Y axis on the drawing. The point where the evaluation function J takes the minimum value is obtained.

8 and 9 are graphs showing another example of the straight lines u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ . First, in FIGS. 8 (a) and 8 (b) and FIGS. 9 (a) and 9 (b), straight lines u _f1 ¹ , u _f2 ¹ , u _f3 ¹ and auxiliary lines are represented by dotted lines, broken lines, solid lines, Represented using a one-dot chain line. 8A is a graph when the X and Y intercepts of the straight line u _f2 ¹ are sufficiently smaller than the X and Y intercepts of the straight line u _f1 ¹ , and FIG. 8B is a straight line. A graph when the X intercept of u _f2 ¹ is moved in the positive direction of the X axis is shown. 9A is a graph when the Y intercept of the straight line u _f2 ¹ is moved in the positive direction of the Y axis compared to FIG. 8B, and FIG. 9B is a straight line u _f2 ^1. A graph is shown when the X-intercept and the Y-intercept are moved sufficiently large in the positive direction of the X-axis and Y-axis.

Next, the intersection of the straight line u _f1 ¹ and the straight line u _f3 ¹ is the point A ₁ , the intersection of the straight line passing through the point A ₁ and parallel to the Y axis and the straight line u _f2 ¹ is the point B ₁ , the straight line u _f2 ¹ and the X axis Is defined as C ₁ , and an intersection between a straight line passing through the point C ₁ and parallel to the Y axis and the straight line u _f1 ¹ is defined as D ₁ . Further, the intersection of the straight line u _f2 ¹ and the straight line u _f3 ¹ is the point E ₁ , the intersection of the straight line passing through the point E ₁ and the straight line u _f1 ¹ and the straight line u _f1 ¹ is the point F ₁ , and the straight line u _f1 ¹ and the X axis are Let the intersection be a point G ₁ , and _let H ₁ be the intersection of a straight line passing through the point G ₁ and parallel to the Y axis and the straight line u _f2 ¹ . Note that the subscript of each point represents the number of the corresponding arithmetic processing.

Here, from the equations (18) to (20), the x coordinates x _a1 , x _b1 , x _c1 , x of the points A ₁ , B ₁ , C ₁ , D ₁ , E ₁ , F ₁ , G ₁ , H ₁ x _d1 , x _{e 1} , x _{f 1} , x _{g 1} , x _{h 1} and y coordinates y a ₁ , y _{b 1} , y _{c 1} , y _{d 1} , y _{e 1} , y _{f 1} , y _{g 1} , y _{h 1} are

となる。なお、各座標の下付きの添え字は対応する点を表す。

It becomes. In addition, the subscript of each coordinate represents a corresponding point.

As shown in FIGS. 8A and 8B, when the Y intercept of the straight line u _f2 ¹ is sufficiently smaller than the Y intercept of the straight line u _f1 ¹ , the point A ₁ is Y with respect to the point B ₁ . Since it exists on the positive side of the axis, the coordinates y _a1 and y _b1 are

を満たす。また、図８（ａ）に示すように、直線ｕ_ｆ２ ^１のＸ切片が直線ｕ_ｆ１ ^１のＸ切片と比べて十分に小さいとき、点Ａ_１は点Ｃ_１に対してＸ軸の正側に存在するため、座標ｘ_ａ１，ｘ_ｃ１は、

Meet. Further, as shown in FIG. 8A, when the X intercept of the straight line u _f2 ¹ is sufficiently smaller than the X intercept of the straight line u _f1 ¹ , the point A ₁ is on the positive side of the X axis with respect to the point C ₁ . The coordinates x _a1 and x _c1 are

を満たす。このとき、変数ｕ_ｆ３ ^１’の変域は、

Meet. At this time, the domain of the variable u _f3 ¹ ′ is

となるため、図８（ａ）より、評価関数Ｊは点Ｄ_１において最小値を取ることがわかる。従って、式（３２）及び式（３３）が成立するとき、評価関数Ｊの最小値Ｊ_ｍｉｎ ^１は、

Since the, from FIG. 8 (a), the evaluation function J is seen that the minimum value at point D _1. Therefore, when Expression (32) and Expression (33) hold, the minimum value J _min ¹ of the evaluation function J is

となり、第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１はそれぞれ、

The first command value candidates u _f1 ¹ , u _f2 ¹ and u _f3 ¹ are respectively

となる。また、図８（ｂ）に示すように、点Ｃ_１が点Ａ_１に対してＸ軸の正側に存在するように、直線ｕ_ｆ２ ^１のＸ切片を移動すると、座標ｘ_ａ１，ｘ_ｃ１は、

It becomes. Further, as shown in FIG. 8B, when the X intercept of the straight line u _f2 ¹ is moved so that the point C ₁ exists on the positive side of the X axis with respect to the point A ₁ , the coordinates x _a1 and x _c1 are moved. Is

を満たすこととなる。このとき、図８（ｂ）より、座標ｘ_ａ１は変数ｕ_ｆ３ ^１’の変域内に存在し、評価関数Ｊは点Ａ_１において最小値を取ることがわかる。従って、式（３２）及び式（３７）が成立するとき、評価関数Ｊの最小値Ｊ_ｍｉｎ ^１は、

Will be satisfied. At this time, it can be seen from FIG. 8B that the coordinate x _a1 exists in the domain of the variable u _f3 ¹ ′, and the evaluation function J takes the minimum value at the point A ₁ . Therefore, when Expression (32) and Expression (37) hold, the minimum value J _min ¹ of the evaluation function J is

となる。第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１はそれぞれ、

It becomes. The first command value candidates u _f1 ¹ , u _f2 ¹ and u _f3 ¹ are respectively

となる。

It becomes.

Further, as shown in FIGS. 9A and 9B, when the Y intercept of the straight line u _f2 ¹ is moved so that the point B ₁ exists on the positive side of the Y axis with respect to the point A ₁ , y _a1 and y _b1 are

を満たすこととなる。ここで、図９（ａ）に示すように、点Ｅ_１が点Ｇ_１に対して、Ｘ軸の負側に存在するとき、すなわち、座標ｘ_ｅ１，ｘ_ｇ１が、

Will be satisfied. Here, as shown in FIG. 9 (a), the point _{E 1} is with respect to the point _{G 1,} when present on the negative side of the X-axis, i.e., coordinates _{x e1, x g1} is,

を満たすとき、評価関数Ｊは点Ｅ_１において最小値を取る。従って、式（４０）及び式（４１）が同時に成り立つとき、評価関数Ｊの最小値Ｊ_ｍｉｎ ^１は、

When satisfying the evaluation function J takes a minimum value at point E _1. Therefore, when Equation (40) and Equation (41) hold simultaneously, the minimum value J _min ¹ of the evaluation function J is

となる。第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１はそれぞれ、

It becomes. The first command value candidates u _f1 ¹ , u _f2 ¹ and u _f3 ¹ are respectively

となる。また、図９（ｂ）に示すように、点Ｅ_１が点Ｇ_１に対してＸ軸の正側に存在するように、直線ｕ_ｆ２ ^１のＸ切片とＹ切片をさらに正側に移動させると、座標ｘ_ｅ１，ｘ_ｇ１は、

It becomes. Further, as shown in FIG. 9B, the X and Y intercepts of the straight line u _f2 ¹ are further moved to the positive side so that the point E ₁ exists on the positive side of the X axis with respect to the point G ₁ . And the coordinates x _e1 and x _g1 are

を満たすこととなる。このとき、図９（ｂ）より、座標ｘ_ｅ１は変数ｕ_ｆ３ ^１’の変域に含まれないため、評価関数Ｊは点Ｈ_１において最小値を取ることがわかる。従って、式（４０）及び式（４４）が同時に成り立つとき、評価関数Ｊの最小値Ｊ_ｍｉｎ ^１は、

Will be satisfied. At this time, it can be seen from FIG. 9B that since the coordinate x _e1 is not included in the domain of the variable u _f3 ¹ ′, the evaluation function J takes the minimum value at the point H ₁ . Therefore, when Equation (40) and Equation (44) hold simultaneously, the minimum value J _min ¹ of the evaluation function J is

となり、第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１はそれぞれ、

The first command value candidates u _f1 ¹ , u _f2 ¹ and u _f3 ¹ are respectively

となる。

It becomes.

In the first embodiment, cases where the minimum value J _min ¹ of the evaluation function J is calculated using Expression (35), Expression (38), Expression (42), and Expression (45) are Case ₁₁ , It is defined as Case ₁₂ , Case ₁₃ , and Case ₁₄ . However, the number following Case represents the number of the corresponding calculation process among the first to fourth calculation processes.

FIG. 10 is a flowchart of the first calculation process by the contraction force command value calculation unit. As shown in FIG. 10, the contractile force command value calculation unit 152 performs the first calculation process using the first function shown in Expression (10) and the second function shown in Expression (11) as shown in Expression (18). In addition, u _e1 = 0, u _e2 = 0, and u _e3 = 0 (S31).

Next, the contractile force command value calculation unit 152 calculates Expression (24) and Expression (25) (S32). Next, the contraction force command value calculation unit 152 determines whether or not y _a1 ≧ y _b1 (S33). When the contraction force command value calculation unit 152 satisfies y _a1 ≧ y _b1 (S33: Yes), the contraction force command value calculation unit 152 calculates Expression (26) (S34). Next, the contraction force command value calculation unit 152 determines whether or not x _a1 ≧ x _c1 (S35). When x _a1 ≧ x _c1 (S35: Yes), the contraction force command value calculation unit 152 calculates Expression (27) (S36), and derives Expression (35) and Expression (36) (S37: Case _11). ).

Further, when x _a1 ≧ x _c1 is not satisfied (S35: No), the contraction force command value calculation unit 152 derives Expression (38) and Expression (39) (S38: Case ₁₂ ).

In addition, when y _a1 ≧ y _b1 is not satisfied (S33: No), the contraction force command value calculation unit 152 calculates Expression (28), Expression (29), and Expression (30) (S39). Next, the contraction force command value calculation unit 152 determines whether or not x _e1 <x _g1 (S40). Next, when x _e1 <x _g1 is satisfied (S40: Yes), the contraction force command value calculation unit 152 derives Expression (42) and Expression (43) (S41: Case ₁₃ ).

In addition, when x _e1 <x _g1 is not satisfied (S40: No), the contraction force command value calculation unit 152 calculates Expression (30) and Expression (31) (S42), Expression (45), and Expression (46). Is derived (S43: Case ₁₄ ).

Thus, the contraction force command value calculation unit 152 performs the case classification shown in Expression (32), Expression (33), and Expression (41). In each case, the minimum value J _min ¹ of the evaluation function J and the first command value candidates u _f1 ¹ , u _f2 ¹ , u _f3 ¹ are calculated. That is, the contraction force command value calculation unit 152 executes a first calculation process for calculating a first command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

In this embodiment, command value candidates u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ that minimize the evaluation function J are derived using the variable u _f3 ¹ ′ defined by Expression (21). The minimum value of J and the command value candidates u _f1 ¹ , u _f2 ¹ , u _f3 ¹ do not depend on how the variables are selected. This is because the minimum value of the evaluation function J is determined by the command value candidates u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ , but the command value candidates u _f1 ¹ , u _f2 ¹ , and u _f3 ¹ do not depend on how the variables are selected. This is to increase or decrease to satisfy the expressions (19) and (20). This also holds true for the second to fourth arithmetic processing described below.

(2.2.2 Design of second arithmetic processing)
The second command value candidates u _e1 ² , u _f2 ² , and u _e3 ² of the contraction force command value in the second calculation process are obtained from Condition 2 and Expression (16) in FIG.

となるため、評価関数Ｊは第２指令値候補ｕ_ｆ１ ^２，ｕ_ｅ２ ^２，ｕ_ｆ３ ^２の最大値となる。そこで、第１演算処理と同様に変数ｕ_ｆ３ ^２’を、

Therefore, the evaluation function J becomes the maximum value of the second command value candidates u _f1 ² , u _e2 ² and u _f3 ² . Therefore, the variable u _f3 ² ′ is set as in the first calculation process.

と定義する。変数ｕ_ｆ３ ^２’を用いて第２指令値候補ｕ_ｆ１ ^２，ｕ_ｅ２ ^２を、

It is defined as Using the variable u _f3 ² ′, the second command value candidates u _f1 ² and u _e2 ² are

とする。そして、Ｘ軸を変数ｕ_ｆ３ ^２’と、Ｙ軸を第２指令値候補ｕ_ｆ１ ^２，ｕ_ｅ２ ^２，ｕ_ｆ３ ^２とする３つの直線ｕ_ｆ１ ^２，ｕ_ｅ２ ^２，ｕ_ｆ３ ^２のグラフを描画する。さらに、直線ｕ_ｆ１ ^２のＸ切片とＹ切片を固定し、直線ｕ_ｅ２ ^２のＸ切片とＹ切片を原点から移動させると、第２演算処理においてはＣａｓｅ_２１、Ｃａｓｅ_２２、Ｃａｓｅ_２３、Ｃａｓｅ_２４の４通りの場合分けが必要となる。

And Then, a graph of three straight lines u _f1 ² , u _e2 ² , u _f3 ² with the variable u _f3 ² ′ on the X axis and the second command value candidates u _f1 ² , u _e2 ² , u _f3 ² on the Y axis is obtained. draw. Furthermore, when the X and Y intercepts of the straight line u _f1 ² are fixed and the X and Y intercepts of the straight line u _e2 ² are moved from the origin, Case ₂₁ , Case ₂₂ , Case ₂₃ , and Case _{24 in} the second calculation process. The following four cases are required.

FIG. 11 is a flowchart of the second calculation process by the contraction force command value calculation unit. As shown in FIG. 11, as the second calculation process, the contractile force command value calculation unit 152 first converts the first function shown in Expression (10) and the second function shown in Expression (11) into Expression (47). As shown, u _e1 = 0, u _f2 = 0, and u _e3 = 0 (S51).

Next, the contraction force command value calculation unit 152 calculates y _a2 and y _b2 (S52), and determines whether y _b2 ≦ y _a2 is satisfied (S53). When y _b2 ≦ y _a2 is satisfied (S53: Yes), the contraction force command value calculation unit 152 calculates x _c2 and x _a2 (S54), and determines whether x _c2 <x _a2 is satisfied (S55). ). When _xc2 < _xa2 is satisfied (S55: Yes), the contraction force command value calculation unit 152 is _uf1 ² = y _a2 , u _e2 ² = y _b2 , u _f3 ² = y _a2 , J _min ² = y _a2 Is derived (S56: Case ₂₁ ).

In addition, when x _c2 <x _a2 is not satisfied (S55: No), the contraction force command value calculation unit 152 determines whether x _c2 ≦ x _d2 is satisfied (S57). Next, when x _c2 ≦ x _d2 is satisfied (S57: Yes), the contraction force command value calculation unit 152 calculates y _e2 and y _f2 (S58), and u _f1 ² = y _f2 and u _e2 ² = 0. , U _f3 ² = y _e2 and J _min ² = y _e2 are derived (S59: Case ₂₂ ).

When x _c2 ≦ x _d2 is not satisfied (S57: No), the contraction force command value calculation unit 152 derives u _f1 ² = u _e2 ² = u _f3 ² = 0 and J _min ² = 0 (S60: Case). ₂₃ ).

In addition, when y _b2 ≦ y _a2 is not satisfied (S53: No), the contraction force command value calculation unit 152 calculates y _g2 and y _h2 (S61), and u _f1 ² = 0 and u _e2 ² = y _h2 , u _f3 ² = y _g2 and J _min ² = y _g2 are derived (S62: Case ₂₄ ).

In Case ₂₃ , the second command value candidates u _f1 ² , u _e2 ² , and u _f3 ² cannot all be positive numbers. Therefore, the second command value candidates u _fi ² , u _ei ² and the minimum value J _min ² are set to 0 in order to exclude them from candidates in a muscle command selection unit described later.

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ² of the evaluation function J and the second command value candidates u _f1 ² , u _e2 ² , and u _f3 ² . That is, the contraction force command value calculation unit 152 executes a second calculation process for calculating a second command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.2.3 Design of third arithmetic processing)
The third command value candidates u _f1 ³ , u _e2 ³ , and u _e3 ³ of the contraction force command value in the third calculation process are expressed by Condition 3 and Expression (16) in FIG.

となるため、評価関数Ｊは第３指令値候補ｕ_ｅ１ ^３，ｕ_ｆ２ ^３，ｕ_ｆ３ ^３の最大値となる。そこで、第１演算処理と同様に変数ｕ_ｆ３ ^３’を、

_Therefore , the evaluation function J is the maximum value of the third command value candidates u _e1 ³ , u _f2 ³ and u _f3 ³ . Therefore, the variable u _f3 ³ ′ is set as in the first calculation process.

と定義する。第３指令値候補ｕ_ｅ１ ^３，ｕ_ｆ２ ^３は、

It is defined as The third command value candidates u _e1 ³ and u _f2 ³ are

と表すことができる。ここで、

It can be expressed as. here,

と定義する。変数ｕ_ｆ１ ^３’，ｕ_ｅ２ ^３’と定数Ｋ_ｆ１’，Ｋ_ｅ２’，Ｔ_１’，Ｔ_２’を用いて式（５３）及び式（５４）を表すと、

It is defined as Expressions (53) and (54) are expressed using the variables u _f1 ³ ′, u _e2 ³ ′ and constants K _f1 ′, K _e2 ′, T ₁ ′, T ₂ ′.

となる。式（４９）と式（５９）、式（５０）と式（５８）を比較すると、第３演算処理の場合分けは、第２演算処理と等価であることがわかる。従って、第３演算処理においてはＣａｓｅ_３１、Ｃａｓｅ_３２、Ｃａｓｅ_３３、Ｃａｓｅ_３４の４通りの場合分けが必要となる。

It becomes. Comparing equation (49) and equation (59), equation (50) and equation (58), it can be seen that the case division of the third operation processing is equivalent to the second operation processing. Accordingly, in the third calculation process, four cases of Case ₃₁ , Case ₃₂ , Case ₃₃ , and Case ₃₄ are required.

  FIG. 12 is a flowchart of the third calculation process by the contraction force command value calculation unit. As shown in FIG. 12, the third calculation process of the first embodiment is divided into cases using the variable conversion and the second calculation process shown in Expression (55) to Expression (57).

That is, as the third calculation process, the contraction force command value calculation unit 152 first uses the first function shown in the equation (10) and the second function shown in the equation (11) as shown in the equation (51). _{It is} assumed that _f1 = 0, u _e2 = 0, and u _e3 = 0 (S71). Next, the contraction force command value calculation unit 152 is defined as in Expression (56) and Expression (57) (S72), and performs the same calculation as in the second calculation process (S73). At that time, the contraction force command value calculation unit 152 defines the equation (55) (S74).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ³ of the evaluation function J and the third command value candidates u _e1 ³ , u _f2 ³ , and u _f3 ³ . That is, the contraction force command value calculation unit 152 executes a third calculation process for calculating a third command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.2.4 Design of fourth arithmetic processing)
The fourth command value candidates u _f1 ⁴ , u _f2 ⁴ , and u _e3 ⁴ of the contraction force command value in the fourth calculation process are obtained from Condition 4 and Expression (16) in FIG.

となるため、評価関数Ｊは第４指令値候補ｕ_ｅ１ ^４，ｕ_ｅ２ ^４，ｕ_ｆ３ ^４の最大値となる。そこで、第１演算処理と同様に変数ｕ_ｆ３ ^４’を、

_Therefore , the evaluation function J becomes the maximum value of the fourth command value candidates u _e1 ⁴ , u _e2 ⁴ , u _f3 ⁴ . Therefore, the variable u _f3 ⁴ ′ is set as in the first calculation process.

と定義する。変数ｕ_ｆ３ ^４’を用いて第４指令値候補ｕ_ｅ１ ^４，ｕ_ｅ２ ^４を、

It is defined as Using the variable u _f3 ⁴ ′, the fourth command value candidates u _e1 ⁴ and u _e2 ⁴ are

とする。そして、Ｘ軸を変数ｕ_ｆ３ ^４’と、Ｙ軸を第４指令値候補ｕ_ｅ１ ^４，ｕ_ｅ２ ^４，ｕ_ｆ３ ^４とする３つの直線ｕ_ｅ１ ^４，ｕ_ｅ２ ^４，ｕ_ｆ３ ^４のグラフを描画する。さらに、直線ｕ_ｅ１ ^４のＸ切片とＹ切片を固定して図示し、直線ｕ_ｅ２ ^４のＸ切片とＹ切片を図上で移動させると、第４演算処理においては、Ｃａｓｅ_４１、Ｃａｓｅ_４２、Ｃａｓｅ_４３、Ｃａｓｅ_４４の４通りの場合分けが必要となる。

And Then, a graph of three straight lines u _e1 ⁴ , u _e2 ⁴ , u _f3 ⁴ with the variable u _f3 ⁴ ′ on the X axis and the fourth command value candidates u _e1 ⁴ , u _e2 ⁴ , u _f3 ⁴ on the Y axis is obtained. draw. Furthermore, when the X and Y intercepts of the straight line u _e1 ⁴ are fixed and shown, and the X and Y intercepts of the straight line u _e2 ⁴ are moved on the drawing, in the fourth calculation process, Case ₄₁ , Case ₄₂ , Case classification is required for Case ₄₃ and Case ₄₄ .

FIG. 13 is a flowchart of the fourth calculation process by the contraction force command value calculation unit. Contractile force command value calculating section 152, the fourth arithmetic processing, first, in the second function shown in the first function and the equation shown in equation (10) (11), as shown in equation _{(60), u f1} = It is assumed that 0, u _f2 = 0 and u _e3 = 0 (S81).

Next, the contraction force command value calculation unit 152 calculates x _a4 and x _b4 (S82). Next, the contraction force command value calculation unit 152 determines whether or not x _b4 <x _a4 (S83). When it is determined that x _b4 <x _a4 is satisfied (S83: Yes), the contraction force command value calculation unit 152 calculates y _c4 and y _d4 (S84). Then, the contraction force command value calculation unit 152 derives u _e1 ⁴ = 0, u _e2 ⁴ = y _e4 , and u _f3 ⁴ = y _c4 (S85).

Next, the contraction force command value calculation unit 152 determines whether or not y _d4 <y _c4 (S86). When the contraction force command value calculation unit 152 determines that y _d4 <y _c4 (S86: Yes), J _min ⁴ = y _c4 (S87: Case ₄₁ ). When the contraction force command value calculation unit 152 determines that y _d4 <y _c4 is not satisfied (S86: No), J _min ⁴ = y _d4 is set (S88: Case ₄₂ ).

When the contraction force command value calculation unit 152 determines that x _b4 <x _a4 is not satisfied (S83: No), it calculates y _e4 and y _f4 (S89). Then, the contraction force command value calculation unit 152 derives u _e1 ⁴ = y _e4 , u _e2 ⁴ = 0, u _f3 ⁴ = y _f4 (S90).

Next, the contraction force command value calculation unit 152 determines whether or not y _e4 <y _f4 (S91). When the contraction force command value calculation unit 152 determines that y _e4 <y _f4 (S91: Yes), it sets J _min ⁴ = y _f4 (S92: Case ₄₃ ). When the contraction force command value calculation unit 152 determines that y _e4 <y _f4 is not satisfied (S91: No), it sets J _min ⁴ = y _e4 (S93: Case ₄₄ ).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ⁴ of the evaluation function J and the fourth command value candidates u _e1 ⁴ , u _e2 ⁴ , and u _f3 ⁴ of the contraction force command value. That is, the contraction force command value calculation unit 152 executes a fourth calculation process for calculating a fourth command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.3 First command selection process)
In the first command selection process, the contractile force command value calculation unit 152 selects a minimum ^one of the candidates J _min ¹ , J _min ² , J _min ³ , and J _min ⁴ of the minimum value J _min of the evaluation function. Then, the contraction force command value calculation unit 152 outputs the command value candidates at that time as contraction force command values u _fi and u _ei .

FIG. 14 is a flowchart of first command selection processing by the contraction force command value calculation unit. First, the contractile force command value calculation unit 152 calculates the minimum value J _{min 1} of the evaluation function J and the contractile force command values u _fi and u _ei , the minimum value candidate J _min ¹ calculated by the first calculation process, and the contractile force command value. First command value candidates u _fi ¹ and u _ei ¹ (S101). Next, contractile force command value calculating section 152 _determines whether or not ^{_{J min 2> 0 (S102)}} , if a ^{_{J min 2> 0 (S102:}} Yes), the following equation (64), That is, it is determined whether J _min > J _min ² (S103).

When the contraction force command value calculation unit 152 determines that J _min > J _min ² (S103: Yes), J _{min is set} to J _min ² and u _fi and u _ei are set to u _fi ² and u _ei ² . (S104).

Next, contractile force command value calculating section 152 _determines whether or not ^{_{J min 3> 0 (S105)}} , if a ^{_{J min 3> 0 (S105:}} Yes), in _{J min>} J min ³ It is determined whether or not there is (S106). When the contraction force command value calculation unit 152 determines that J _min > J _min ³ (S106: Yes), J _{min is set} to J _min ^3, and u _fi and u _ei are set to u _fi ³ and u _ei ³ . (S107).

Next, the contraction force command value calculation unit 152 determines whether or not J _min > J _min ⁴ (S108). When the contraction force command value calculation unit 152 determines that J _min > J _min ⁴ (S108: Yes), J _{min is set} to J _min ^4, and u _fi and u _ei are set to u _fi ⁴ and u _ei ⁴ . (S109).

Here, as described in Section 2.2, when the contraction force cannot be all positive numbers in the second and third calculation processes, the minimum value candidates J _min ² and J _min ³ are set to 0. did. Therefore, in the first muscle command selection process, the contraction force command value calculation unit 152 calculates the minimum value J _min and the contraction force command values u _fi and u _ei only when the candidates J _min ² and J _min ³ are greater than zero. Update.

  As described above, the contraction force command value calculation unit 152 sets the command value candidate having the smallest evaluation function among the first to fourth command value candidates as the first to sixth contraction force command values output in the command value output process. The command selection process to be selected is executed.

According to the first embodiment, the target trajectory for the flexibility of the hand is uniquely determined by the case division algorithm for optimizing the control input that is the contraction force command value. Therefore, the artificial muscle actuator f _i with the minimum control input. , E _i can be antagonistically driven. By minimizing the control input, for example, in the McKibben type artificial muscle, the amount of air consumed can be minimized. As a result, the operation time can be extended in the robot 100 using an on-board type air source such as a tank.

[Second Embodiment]
Next, a robot according to a second embodiment of the present invention will be described. In the second embodiment, by adding a branch condition to the control method of the first embodiment, even when each joint torque command value takes both a positive number and a negative number, Indicates that derivation is possible. In the second embodiment, the device configuration is the same as that in FIGS. 2 and 3 in the first embodiment, and the control operation of the control device is different. Therefore, detailed description of the device configuration is omitted. The control operation of the control device will be described in detail.

In the second embodiment, a contraction force control method is derived in consideration of the case where the joint torque command values T ₁ and T ₂ are negative numbers. This makes it possible to minimize the maximum value of the contractile force in the target trajectory with an arbitrary joint angle.

(1 model derivation)
In the second embodiment, the same model as in the first embodiment is used.

(2 Control system design)
In the second embodiment, the contraction force command value calculation unit 152 assumes a combination of signs of muscle torques τ ₁ , τ ₂ , τ ₃ based on the signs of joint torque command values T ₁ , T _2. Different from the first embodiment.

(2.1 Classification based on the sign of joint torque command value)
When the joint torque command value is a negative number, it is necessary to consider a case where the muscle torque τ ₃ is a negative number among the combinations of signs of the muscle torques τ ₁ , τ ₂ , and τ ₃ shown in FIG. Because when a joint torque command value T ₁ is negative, it is because it is possible to reduce the first contraction force of the single joint muscle if the muscle torque tau ₃ and negative.

Thus, when one of the joint torque command values T ₁ and T ₂ is a negative number and when both are positive numbers, the combination of signs of muscle torques τ ₁ , τ ₂ , and τ ₃ to be considered is Different. Therefore, based on the signs of the joint torque command values T ₁ and T ₂ , the combinations of signs of muscle torques τ ₁ , τ ₂ , and τ ₃ to be considered are classified.

First, consider the case where the joint torque command value T ₁ is T ₂ becomes positive becomes negative. In FIG. 4, assuming a combination of condition 7 and condition 8 in which both the muscle torques τ ₁ and τ ₃ are negative numbers, the joint torque command value T ₁ is a negative number according to equation (9). Further, assuming a combination of Condition 1 and Condition _{3 in} which both the muscle torques τ ₂ and τ ₃ are positive numbers, the joint torque command value T ₂ is a positive number. From these facts, when the joint torque command value T ₁ is a positive number and the joint torque command value T ₂ is a negative number, the evaluation function J is assumed to be a combination of Condition 2, Condition 4, Condition 5, and Condition 6. It can be seen that u _fi and u _ei to be minimized should be derived.

In the second embodiment, when the joint torque command value T ₁ is positive and the joint torque command value T ₂ is negative, the joint torque command value T ₁ is negative and the joint torque command value T ₂ is positive. The contraction force command value is calculated using the calculation method when

Further, when the joint torque command values T ₁ and T ₂ are both negative numbers, the contractile force is calculated by using the calculation method when both the joint torque command values T ₁ and T ₂ are positive numbers. Further, in the present invention, when the joint torque command values T ₁ and T ₂ are 0, the same processing as in the case of a positive number is performed.

The contraction force command value calculation unit 152 performs case classification based on the signs of the joint torque command values T ₁ and T ₂ and selects candidate codes for the muscle torques τ ₁ , τ ₂ , and τ ₃ . Then, the contraction force command value calculation unit 152, when one of the joint torque command values T ₁ and T ₂ is a positive number and the other is a negative number, that is, the joint torque command value T ₁ and the joint torque command value T _2. Are different signs, the second minimization calculation process and the second command selection process are executed. Thus, the contractile force command value is calculated.

Further, the contraction force command value calculation unit 152 determines that the joint torque command values T ₁ and T ₂ are both positive or negative, that is, the joint torque command value T ₁ and the joint torque command value T ₂ have the same sign. In this case, as in the first embodiment, the first minimization calculation process and the first command selection process are executed. Thus, the contractile force command value is calculated.

FIG. 15 is a flowchart showing the processing operation of the contraction force command value calculation unit of the robot according to the second embodiment of the present invention. First, the contraction force command value calculation unit 152 receives the joint torque command values T ₁ and T ₂ from the feedforward compensation unit 151, and receives target joint angles θ ₁ and θ ₂ and target joint angular velocity ω from a host controller (not shown). ₁ and ω ₂ are input (S110). The elastic force constant k, the viscous force constant c, and the moment arm diameter r are values set in advance.

Next, the contraction force command value calculation unit 152 calculates the coefficients K _fi and K _ei from the target joint angles θ ₁ and θ ₂ and the target joint angular velocities ω ₁ and ω ₂ using Expression (12) (S111). . Next, the contraction force command value calculation unit 152 determines whether or not T ₁ ≧ 0 (S112). When it is determined that T ₁ ≧ 0 (S112: Yes), the contraction force command value calculation unit 152 determines whether T ₂ ≧ 0 (S113).

When the contraction force command value calculation unit 152 determines that T ₂ ≧ 0 (S113: Yes), the coefficient K _{fi is added to} the first function shown in Expression (10) and the second function shown in Expression (11). , K _ei and joint torque command values T ₁ and T ₂ are substituted as constants (S114). That is, contractile force command value calculating section 152, if a joint torque command value T ₁ and the joint torque command value T ₂ is the same sign, and performs step S114.

Next, the contraction force command value calculation unit 152 executes the first minimization calculation process described in the first embodiment (S115). Next, the contraction force command value calculation unit 152 selects a command value candidate having the smallest evaluation function for the evaluation functions J _min ¹ to ⁴ and the command value candidates u _fi ^{1 to 4} and u _ei ¹ to ⁴ (S116). The first command selection process similar to that in the first embodiment is executed (S117). Next, the contraction force command value calculation unit 152 executes a command value output process for outputting the calculated first to sixth contraction force command values u _fi and u _ei to the contraction force generation unit 153 (S118: command value output) Processing step).

In addition, when the contraction force command value calculation unit 152 determines that T ₂ ≧ 0 is not satisfied (S113: No), the coefficient is added to the first function expressed by Equation (10) and the second function expressed by Equation (11). K _fi and K _ei and joint torque command values T ₁ and T ₂ are substituted as constants (S119). That is, contractile force command value calculating section 152, if a joint torque command value T ₁ and the joint torque command value T ₂ is the same sign, and performs step S119.

Next, the contraction force command value calculation unit 152 executes a second minimization calculation process for calculating a command value candidate that minimizes the evaluation function J, which will be described later (S120). Next, the contraction force command value calculation unit 152 selects a command value candidate having the smallest evaluation function for the evaluation function J _min ^{5 to 8} and the command value candidates u _fi ^{5 to 8} and u _ei ⁵ to ⁸ (S121). The second command selection process described later is executed (S122). Next, the contraction force command value calculation unit 152 executes a command value output process for outputting the calculated first to sixth contraction force command values u _fi and u _ei to the contraction force generation unit 153 (S118: command value output) Processing step).

In addition, when the contraction force command value calculation unit 152 determines that T ₁ ≧ 0 is not satisfied (S112: No), it determines whether T ₂ ≧ 0 is satisfied (S123). Next, when the contraction force command value calculation unit 152 determines that T ₂ ≧ 0 (S123: Yes), K _f1 ′ = K _e2 , K _e1 ′ = K _f2 , K _f2 ′ = K _e1 , K _{e2 '= K f1, T 1} ' = T 2, T 2 '= defines the _{T 1} (S124). Then, the contraction force command value calculation unit 152 substitutes K _fi ′, K _ei ′, T ₁ ′, and T ₂ ′ for the first function shown in Expression (10) and the second function shown in Expression (11). (S125).

Next, the contraction force command value calculation unit 152 executes a second minimization calculation process for calculating a command value candidate for minimizing the evaluation function J, which will be described later (S126). Next, the contraction force command value calculation unit 152 sets u _f1 = u _f2 ′, u _e1 = u _e2 ′, u _f2 = u _f1 ′, u _e2 ′ = u _f1 (S127). Next, the contraction force command value calculation unit 152 selects a command value candidate having the smallest evaluation function for the evaluation functions J _min ⁵ to ⁸ and the command value candidates u _fi ^{5 to 8} and u _ei ⁵ to ⁸ (S128). The second command selection process described later is executed (S129). Next, the contraction force command value calculation unit 152 executes a command value output process for outputting the calculated first to sixth contraction force command values u _fi and u _ei to the contraction force generation unit 153 (S118: command value output) Processing step).

When the contraction force command value calculation unit 152 determines that T ₂ ≧ 0 is not satisfied (S123: No), K _fi ′ = K _ei , K _ei ′ = K _fi , T ₁ ′ = −T ₁ , T ₂ '= to define the -T ₂ (S130). Then, the contraction force command value calculation unit 152 substitutes K _fi ′, K _ei ′, T ₁ ′, and T ₂ ′ for the first function shown in Expression (10) and the second function shown in Expression (11). (S131).

Next, the contraction force command value calculation unit 152 executes the first minimization calculation process described in the first embodiment (S132). Next, the contraction force command value calculation unit 152 sets u _fi ′ = u _ei and u _ei ′ = u _fi (S133). Next, the contraction force command value calculation unit 152 selects a command value candidate having the smallest evaluation function for the evaluation functions J _min ¹ to ⁴ and the command value candidates u _fi ^{1 to 4} and u _ei ¹ to ⁴ (S134). The first command selection process similar to that in the first embodiment is executed (S135).

That is, the contraction force command value calculation unit 152 executes command value calculation processing for calculating the first to sixth contraction force command values u _fi and u _ei in S111 to S135 (command value calculation processing step).

Next, the contraction force command value calculation unit 152 executes a command value output process for outputting the calculated first to sixth contraction force command values u _fi and u _ei to the contraction force generation unit 153 (S118: command value output) Processing step).

  Since the second minimization calculation process and the second command value selection process are the same as the first minimization calculation process and the first command value selection process of the first embodiment, detailed calculation methods are omitted. .

(2.2 Cases in the second minimization calculation process)
FIG. 16 is a flowchart of second minimization calculation processing by the contraction force command value calculation unit. The contraction force command value calculation unit 152 assumes the second condition shown in FIG. 4 (S141), executes a fifth calculation process (S142), and first to sixth contraction forces that minimize the evaluation function J _min ⁵ Fifth command value candidates u _fi ⁵ and u _ei ⁵ consisting of command values are output (S143).

Subsequently, the contraction force command value calculation unit 152 assumes the fourth condition shown in FIG. 4 (S144), executes a sixth calculation process (S145), and minimizes the evaluation function J _min ⁶ . Sixth command value candidates u _fi ⁶ and u _ei ⁶ composed of six contraction force command values are output (S146).

Subsequently, contractile force command value calculation unit 152 assumes a fifth condition shown in FIG. 4 (S147), executes a seventh arithmetic processing (S148), first to minimize the evaluation function _{J min} ⁷ Seventh command value candidates u _fi ⁷ and u _ei ⁷ composed of six contraction force command values are output (S149).

Subsequently, contractile force command value calculation unit 152 assumes a sixth condition shown in FIG. 4 (S150), executes the eighth arithmetic processing (S151), first to minimize the evaluation function _{J min} ⁸ Eighth command value candidates u _fi ⁸ and u _ei ⁸ composed of six contraction force command values are output (S152).

The fifth, sixth, seventh, and eighth computation processes assume that the signs of the muscle torques τ ₁ and τ ₂ are the conditions ₂ , 4, 5, and 6 in FIG. This is a subroutine for calculating the candidate J _min ^j and the command value candidates u _fi ^j and u _ei ^j of the contraction force command value (j = 5, 6, 7, 8).

(2.2.1 Fifth arithmetic processing design)
Among the fifth command value candidates u _fi ⁵ , u _ei ⁵ of the contraction force command value in the fifth arithmetic processing, the fifth command value candidates u _e1 ⁵ , u _f2 ⁵ , u _e3 ⁵ are expressed by the condition 2 and the equation of FIG. From (16)

となるため、評価関数Ｊは収縮力指令値の第５指令値候補ｕ_ｆ１ ^５，ｕ_ｅ２ ^５，ｕ_ｆ３ ^５の最大値となる。そこで、第１演算処理と同様に変数ｕ_ｆ３ ^５’を、

Therefore, the evaluation function J is the maximum value of the fifth command value candidates u _f1 ⁵ , u _e2 ⁵ , and u _f3 ⁵ of the contraction force command value. Therefore, the variable u _f3 ⁵ ′ is set as in the first calculation process.

と定義する。変数ｕ_ｆ３ ^５’を用いて第５指令値候補ｕ_ｆ１ ^５，ｕ_ｅ２ ^５を、

It is defined as Using the variable u _f3 ⁵ ′, the fifth command value candidates u _f1 ⁵ and u _e2 ⁵ are

とする。そして、Ｘ軸を変数ｕ_ｆ３ ^５’と、Ｙ軸を収縮力指令値の第５指令値候補ｕ_ｆ１ ^５，ｕ_ｅ２ ^５，ｕ_ｆ３ ^５とする３つの直線ｕ_ｆ１ ^５，ｕ_ｅ２ ^５，ｕ_ｆ３ ^５のグラフを描画する。さらに、直線ｕ_ｆ１ ^５のＸ切片とＹ切片を固定し、直線ｕ_ｅ２ ^５のＸ切片とＹ切片を図上で移動させると、第５演算処理においてはＣａｓｅ_５１、Ｃａｓｅ_５２、Ｃａｓｅ_５３の３通りの場合分けが必要となることがわかる。

And Then, the three straight lines u _f1 ⁵ , u _e2 ⁵ , u _f with the variable u _f3 ⁵ ′ on the X-axis and the fifth command value candidates u _f1 ⁵ , u _e2 ⁵ , u _f3 ⁵ on the contraction force command value. The graph of _f3 ⁵ is drawn. Furthermore, when the X and Y intercepts of the straight line u _f1 ⁵ are fixed and the X and Y intercepts of the straight line u _e2 ⁵ are moved on the drawing, Case ₅₁ , Case ₅₂ , and Case ₅₃ 3 are obtained in the fifth calculation process. It can be seen that it is necessary to separate the streets.

FIG. 17 is a flowchart of a fifth calculation process by the contraction force command value calculation unit. As shown in FIG. 17, the contraction force command value calculation unit 152 first calculates the first function shown in Expression (10) and the second function shown in Expression (11) as Expression (65) as the fifth calculation processing. As shown, u _e1 = 0, u _f2 = 0, and u _e3 = 0 (S161).

Next, the contraction force command value calculation unit 152 calculates y _a5 and y _b5 (S162), and calculates x _c5 and y _c5 (S163). Next, the contraction force command value calculation unit 152 determines whether or not y _b5 <y _a5 (S164).

When y _b5 <y _a5 (S164: Yes), the contraction force command value calculation unit 152 determines whether y _c5 <y _d5 (S165). When y _c5 <y _d5 (S165: Yes), the contraction force command value calculation unit 152 calculates y _e5 and y _f5 (166), and u _f1 ⁵ = y _e5 and u _e2 ⁵ = y _f5 , u _f3 ⁵ = y _e5 and J _min ⁵ = y _e5 are derived (S167: Case ₅₁ ).

When the contraction force command value calculation unit 152 does not satisfy y _c5 <y _d5 (S165: No), u _f1 ⁵ = y _c5 , u _e2 ⁵ = y _c5 , u _f3 ⁵ = y _d5 , J _min ⁵ = y _d5 is derived (S168: Case ₅₂ ).

When y _b5 <y _a5 is not satisfied (S164: No), the contraction force command value calculation unit 152 determines that u _f1 ⁵ = y _a5 , u _e2 ⁵ = y _b5 , u _f3 ⁵ = 0, J _min ⁵ = y _b5 is derived (S169: Case ₅₃ ).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ⁵ of the evaluation function J and the fifth command value candidates u _f1 ⁵ , u _e2 ⁵ , u _f3 ⁵ . That is, the contraction force command value calculation unit 152 executes a fifth calculation process for calculating a fifth command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.2.2 Design of sixth arithmetic processing)
Among the sixth command value candidates u _fi ⁶ and u _ei ⁶ of the contraction force command value in the sixth arithmetic processing unit, the sixth command value candidates u _f1 ⁶ , u _f2 ⁶ and u _e3 ⁶ are the same as the condition 4 in FIG. From equation (16)

となるため、評価関数Ｊは収縮力指令値の第６指令値候補ｕ_ｆ１ ^６，ｕ_ｆ２ ^６，ｕ_ｅ３ ^６の最大値となる。そこで、変数ｕ_ｆ３ ^６’を、

Therefore, the evaluation function J is the maximum value of the sixth command value candidates u _f1 ⁶ , u _f2 ⁶ , u _e3 ⁶ of the contraction force command value. So the variable u _f3 ⁶ '

と定義する。変数ｕ_ｆ３ ^６’を用いて収縮力指令値の第６指令値候補ｕ_ｅ１ ^６，ｕ_ｅ２ ^６を、

It is defined as Using the variable u _f3 ⁶ ′, the sixth command value candidates u _e1 ⁶ and u _e2 ⁶ of the contraction force command value are

とする。そして、Ｘ軸を変数ｕ_ｆ３ ^６’と、Ｙ軸を収縮力指令値の第６指令値候補ｕ_ｅ１ ^６，ｕ_ｅ２ ^６，ｕ_ｆ３ ^６とする３つの直線ｕ_ｅ１ ^６，ｕ_ｅ２ ^６，ｕ_ｆ３ ^６のグラフを描画する。さらに、直線ｕ_ｆ１ ^２のＸ切片とＹ切片を固定し、直線ｕ_ｆｅ２ ^２のＸ切片とＹ切片を図上で移動させると、第６演算処理においてはＣａｓｅ_６１、Ｃａｓｅ_６２の２通りの場合分けが必要となる。

And Then, the three straight lines u _e1 ⁶ , u _e2 ⁶ , u _e are set with the variable u _f3 ⁶ ′ on the X axis and the sixth command value candidates u _e1 ⁶ , u _e2 ⁶ , u _f3 ^{6 on} the Y axis. The graph of _f3 ⁶ is drawn. Furthermore, when the X and Y intercepts of the straight line u _f1 ² are fixed and the X and Y intercepts of the straight line u _fe2 ² are moved on the drawing, there are two cases of Case ₆₁ and Case ₆₂ in the sixth calculation process. Division is necessary.

FIG. 18 is a flowchart of sixth calculation processing by the contraction force command value calculation unit. As shown in FIG. 18, as the sixth calculation process, the contraction force command value calculation unit 152 first converts the first function shown in Expression (10) and the second function shown in Expression (11) into Expression (69). As shown, u _f1 = 0, u _f2 = 0, and u _e3 = 0 (S171).

Next, the contraction force command value calculation unit 152 calculates x _a6 and y _b6 (S172), and sets y _c6 = x _a6 (S173). Next, the contraction force command value calculation unit 152 sets u _e1 ⁶ = 0, u _e2 ⁶ = y _b6 , and u _f3 ⁶ = y _c6 (S174). Next, the contraction force command value calculation unit 152 determines whether or not y _b6 <y _c6 (S175).

When y _b6 <y _c6 (S175: Yes), the contraction force command value calculation unit 152 derives J _min ⁶ = y _c6 (S176: Case ₆₁ ), and when y _b6 <y _c6 is not satisfied (S175: No) ), J _min ⁶ = y _b6 is derived (S177: Case ₆₂ ).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ⁶ of the evaluation function J and the sixth command value candidates u _e1 ⁶ , u _e2 ⁶ , u _f3 ⁶ . That is, the contraction force command value calculation unit 152 executes a fifth calculation process for calculating a fifth command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.2.3 Design of seventh operation processing)
Among the seventh command value candidates u _fi ⁷ and u _ei ⁷ of the contraction force command value in the seventh calculation process, the seventh command value candidates u _e1 ⁷ , u _e2 ⁷ and u _f3 ⁷ are expressed by the condition 5 and the equation of FIG. From (16)

となるため、評価関数Ｊは収縮力指令値の第７指令値候補ｕ_ｆ１ ^７，ｕ_ｆ２ ^７，ｕ_ｅ３ ^７の最大値となる。そこで、変数ｕ_ｅ３ ^７’を、

Therefore, the evaluation function J becomes the maximum value of the seventh command value candidates u _f1 ⁷ , u _f2 ⁷ , u _e3 ⁷ of the contraction force command value. So the variable u _e3 ⁷ '

と定義する。収縮力指令値の第７指令値候補ｕ_ｆ１ ^７，ｕ_ｆ２ ^７は、

It is defined as The seventh command value candidates u _f1 ⁷ and u _f2 ⁷ of the contraction force command value are

と表すことができる。ここで、

It can be expressed as. here,

と定義する。変数ｕ_ｅ１ ^７’，ｕ_ｅ２ ^７’，ｕ_ｆ３ ^７’と定数Ｔ_１’，Ｔ_２’，Ｋ_ｅ１’，Ｋ_ｅ２’，Ｋ_ｆ３’を用いて式（７１）及び式（７２）を表すと、

It is defined as Expressions (71) and (72) are expressed using variables u _e1 ⁷ ′, u _e2 ⁷ ′, u _f3 ⁷ ′ and constants T ₁ ′, T ₂ ′, K _e1 ′, K _e2 ′, K _f3 ′. When,

となる。式（７１）と式（８１）、式（７２）と式（８０）を比較すると、第６演算処理と第７演算処理の場合分けは等価であることがわかる。従って、第７演算処理においてはＣａｓｅ_７１、Ｃａｓｅ_７２の２通りの場合分けが必要となる。

It becomes. Comparing equation (71) and equation (81), and equation (72) and equation (80), it can be seen that the sixth operation processing and the seventh operation processing are equivalent. Therefore, in the seventh calculation process, two cases of Case ₇₁ and Case ₇₂ are required.

  FIG. 19 is a flowchart of a seventh calculation process by the contraction force command value calculation unit. As shown in FIG. 19, the seventh calculation process of the second embodiment is divided into cases using the variable conversion and the seventh calculation process shown in Expression (77) to Expression (79).

That is, as the seventh calculation process, the contraction force command value calculation unit 152 first uses the first function shown in Expression (10) and the second function shown in Expression (11) as shown in Expression (73). _{It is} assumed that _e1 = 0, _ue2 = 0, and _uf3 = 0 (S181). Next, the contraction force command value calculation unit 152 defines as in Expression (78) and Expression (79) (S182), and performs the same calculation as in the sixth calculation process (S183). At that time, the contraction force command value calculation unit 152 defines as shown in Expression (77) (S184).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ⁷ of the evaluation function J and the seventh command value candidates u _f1 ⁷ , u _f2 ⁷ , u _e3 ⁷ . That is, the contraction force command value calculation unit 152 executes a seventh calculation process for calculating a seventh command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.2.4 Design of 8th arithmetic processing)
The eighth command value candidates u _e1 ⁸ , u _f2 ⁸ , u _f3 ⁸ of the contraction force command value in the eighth calculation process are expressed by the condition 6 and the equation (16) in FIG.

となるため、評価関数Ｊは収縮力指令値の第８指令値候補ｕ_ｆ１ ^８，ｕ_ｅ２ ^８，ｕ_ｅ３ ^８の最大値となる。そこで、変数ｕ_ｅ３ ^８’を、

Therefore, the evaluation function J becomes the maximum value of the eighth command value candidates u _f1 ⁸ , u _e2 ⁸ and u _e3 ⁸ of the contraction force command value. So the variable u _e3 ⁸ '

と定義する。収縮力指令値の第８指令値候補ｕ_ｆ１ ^８，ｕ_ｅ２ ^８は変数ｕ_ｅ３ ^８’を用いて、

It is defined as The eighth command value candidate u _f1 ⁸ , u _e2 ⁸ of the contraction force command value uses the variable u _e3 ⁸ ′,

と表すことができる。ここで、

It can be expressed as. here,

と定義する。変数ｕ_ｆ１ ^８’，ｕ_ｅ２ ^８’，ｕ_ｆ３ ^８’と定数Ｔ_１’，Ｔ_２’，Ｋ_ｆ１’，Ｋ_ｅ２’，Ｋ_ｆ３’を用いて式（８４）及び式（８５）を表すと、

It is defined as Expressions (84) and (85) are expressed using the variables u _f1 ⁸ ′, u _e2 ⁸ ′, u _f3 ⁸ ′, and constants T ₁ ′, T ₂ ′, K _f1 ′, K _e2 ′, K _f3 ′. When,

となる。式（６７）と式（９０）、式（６８）と式（８９）を比較すると、第５演算処理と第８演算処理の場合分けは等価であることがわかる。従って、第８演算処理においてはＣａｓｅ_８１、Ｃａｓｅ_８２、Ｃａｓｅ_８３の３通りの場合分けが必要となる。

It becomes. Comparing equation (67) and equation (90), equation (68) and equation (89), it can be seen that the case division of the fifth operation processing and the eighth operation processing is equivalent. Accordingly, in the eighth calculation process, three cases of Case ₈₁ , Case ₈₂ , and Case ₈₃ are required.

  FIG. 20 is a flowchart of the eighth calculation process by the contraction force command value calculation unit. As shown in FIG. 20, the eighth calculation process of the second embodiment is divided into cases using the variable conversion and the seventh calculation process shown in Expression (86) to Expression (88).

That is, as the eighth calculation process, the contraction force command value calculation unit 152 first uses the first function shown in Expression (10) and the second function shown in Expression (11) as shown in Expression (82). _{It is} assumed that _e1 = 0, u _f2 = 0, and u _f3 = 0 (S191). Next, the contraction force command value calculation unit 152 defines the equations (87) and (88) (S192), and performs the same calculation as the fifth calculation process (S193). At that time, the contraction force command value calculation unit 152 defines the equation (86) (S194).

As described above, the contraction force command value calculation unit 152 calculates the minimum value J _min ⁸ of the evaluation function J and the eighth command value candidates u _f1 ⁸ , u _e2 ⁸ , u _e3 ⁸ . That is, the contraction force command value calculation unit 152 executes an eighth calculation process for calculating an eighth command value candidate including the first to sixth contraction force command values that minimizes the evaluation function.

(2.3 Joint torque command values T ₁ and T ₂ are both negative numbers)
For the equations (10) and (11) of the first embodiment,

とする変数変換を行うと、

When variable conversion is performed,

と表すことができる。式（９２）より、Ｔ_１’とＴ_２’は正数となるため、式（９１）〜式（９３）に示す変数変換と第１最小化演算処理を実行することで、関節トルク指令値Ｔ_１，Ｔ_２がどちらも負数となる場合の場合分けを行うことが可能となることがわかる。そこで、図１５に示すように、まず、式（９１）〜式（９３）に示す変数変換を行う。次に、第１最小化演算処理を実行して評価関数Ｊを最小化する収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’を求める。さらに、式（９１）に示す変数変換を行い、収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’からｕ_ｆｉ，ｕ_ｅｉを得る。

It can be expressed as. Since T ₁ ′ and T ₂ ′ are positive numbers from the equation (92), the joint torque command value is obtained by executing the variable conversion and the first minimization calculation processing shown in the equations (91) to (93). It can be seen that case separation can be performed when both T ₁ and T ₂ are negative numbers. Therefore, as shown in FIG. 15, first, variable conversion shown in Expression (91) to Expression (93) is performed. Next, the contraction force command values u _fi 'and u _ei ' for minimizing the evaluation function J are obtained by executing the first minimization calculation process. Furthermore, variable conversion shown in Expression (91) is performed, and u _fi and u _ei are obtained from the contractile force command values u _fi ′ and u _ei ′.

(The cases when 2.4 joint torque command value T ₁ is joint torque command value T ₂ becomes negative is positive)
For equations (10) and (11),

とする変数変換を行うと、

When variable conversion is performed,

と表すことができる。式（９７）より、Ｔ_１’は正数となりＴ_２’は負数となる。このため、式（９６）〜式（９８）に示す変数変換と第２最小化演算処理を実行することで、関節トルク指令値Ｔ_１が負数となり、関節トルク指令値Ｔ_２が正数となる場合の場合分けを行うことが可能となることがわかる。そこで、図１５に示すように、まず、式（９６）〜式（９８）に示す変数変換を行う。次に、第２最小化演算処理を実行して評価関数Ｊを最小化する収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’を求める。さらに、式（９６）に示す変数変換を行い、収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’からｕ_ｆｉ，ｕ_ｅｉを得る。

It can be expressed as. From equation (97), T ₁ ′ is a positive number and T ₂ ′ is a negative number. Therefore, by executing the variable conversion and a second minimization operation processing shown in equation (96) to (98), joint torque command value T ₁ is becomes negative, becomes joint torque command value T ₂ is a positive number It can be seen that cases can be classified. Therefore, as shown in FIG. 15, first, variable conversion shown in Expression (96) to Expression (98) is performed. Next, the contraction force command values u _fi 'and u _ei ' for minimizing the evaluation function J are obtained by executing the second minimization calculation process. Furthermore, variable conversion shown in Expression (96) is performed, and u _fi and u _ei are obtained from the contractile force command values u _fi ′ and u _ei ′.

In the above derivation, it is assumed that the elastic force of the muscle is generated in proportion to the angle from θ _i = 0 rad. In order to generate a feedforward control input with an arbitrary angle as a reference angle (hereinafter referred to as a neutral angle) for generating elastic force of a muscle, θ _ci (i = 1, 2, 3), a coefficient K _fi , K _ei (i = 1, 2, 3) is replaced with equation (12),

とすればよい。ただし、角度θ_ｃ１，θ_ｃ２，θ_ｃ３はそれぞれ、第一単関節筋、第二単関節筋、二関節筋の弾性力の中立角度とする。

And it is sufficient. However, the angles θ _c1 , θ _c2 , and θ _c3 are neutral angles of the first single joint muscle, the second single joint muscle, and the two joint muscles, respectively.

(3 simulation)
In this section, simulation is performed using the control system designed in the previous section. The contraction force that minimizes the evaluation function J is derived by the method described in the previous section. In the second embodiment, the moments of inertia I ₁ and I ₂ of the links 101 and 102 are 2.5 × 10 ⁻³ kgm ² , the lengths L ₁ and L ₂ of the links 101 and 102 are 0.2 m, and the second link. The mass m ₂ of 102 is 1 kg. The pulley diameter r is 5.0 × 10 ⁻² m, the elastic force constant k of the muscle is 10 m ⁻¹ , and the viscous force constant c is 10 m ⁻¹ sec ⁻¹ . Further, the target joint angles θ _r1 and θ _r2 are set to the minimum jerk trajectory that increases from 0.785 rad to 1.047 rad and from 1.571 rad to 2.094 rad during 0 sec to 0.3 sec, respectively, and the sampling period T _s is set. 1.0 × 10 ⁻⁴ sec.

21A and 21B, the joint angles θ ₁ and θ ₂ of the first and second links are represented by solid lines, the target trajectories θ _r1 and θ _r2 are represented by broken _lines , and FIG. the value T ₁ by a solid line, represents the joint torque command value T ₂ by a broken line. 23 (a), 23 (b), and 23 (c), the contraction force command values u _f1 , u _f2 , u _f3 are shown using solid _lines , and the contraction force command values u _e1 , u _e2 and u _e3 are respectively represented. Further, FIG. 24 shows a state in which the contraction force calculation flow in the first to eighth calculation processes is switched according to changes in the coefficients K _fi and K _ei and the joint torque command values T ₁ and T ₂ .

  21 (a) and 21 (b), it can be seen that the joint angle indicated by the solid line overlaps the target trajectory indicated by the broken line, and the joint angle follows the target trajectory.

From FIG. 22, the joint torque command values T ₁ and T ₂ are positive numbers from 0 sec to 0.11 sec. Therefore, the contraction force command value calculation unit 152 executes the first minimization calculation process to calculate the contraction force. It can be seen that it is minimized. Further, since the joint torque command value T ₁ is a negative number and the joint torque command value T ₂ is a positive number between 0.11 sec and 0.3 sec, the contraction force command value calculation unit 152 performs the second minimization calculation process. It can be seen that the contraction force is minimized by execution.

From FIG. 23A, FIG. 23B, and FIG. 23C, it can be seen that the contraction force command value u _fi indicated by the solid line and the contraction force command value u _ei indicated by the broken line are both positive numbers. Also, it can be seen that when one of the contraction force command value u _fi and the contraction force command value u _ei is 0, the other is 0. From these facts, the first minimization calculation process and the second minimization process are performed. It can be seen that the contractile force derived by executing the arithmetic processing satisfies Expression (16) and minimizes the evaluation function J.

Moreover, it can be seen from FIGS. 23B and 23C that the responses of the contraction force command values u _f2 and u _f3 change sharply at 0.11 sec and 0.12 sec. Further, FIG. 24 shows that the contraction force command value calculation unit 152 switches the calculation flow from Case ₁₁ to Case ₁₄ in 0.11 sec and from Case ₅₁ to Case _{52 in} 0.12 sec.

  From these, it can be seen that when the calculation flow is switched, a control input containing a large amount of high-frequency components is generated. If a high-frequency component is included in the control input, it is considered that the natural vibration of the robot is excited. However, the high-frequency component can be reduced by using, for example, a zero-phase low-pass filter, although the minimum value of the contractile force slightly increases.

[Third Embodiment]
Next, a robot according to a third embodiment of the invention will be described. In the third embodiment, although a control input is not minimized, a control method for simultaneously following a target trajectory of joint stiffness and joint angle is shown.

(1 model derivation)
In the third embodiment, the same model as in the second embodiment is used.

(2 control system design)
In the two-link manipulator, since the hand is in direct contact with the outside world, it is important to control the rigidity of the hand. The stiffness of the hand is represented by a stiffness ellipse C as shown in FIG. This ellipse C shows the distribution of rigidity in each direction, and indicates that the rigidity increases as the distance between the hand and the ellipse C increases.

Further, it is known that a two-link manipulator having a 3 to 6 muscle structure can independently control the rigidity of the hand and the torque acting around the joint. The difference between the contraction force command values u _fi and u _ei (i = 1, 2, 3) is U _di (i = 1, 2, 3), and the sum is U _si (i = 1, 2, 3).

Using U _di and U _si , equation (3) becomes

となる。式（１０３）より、Ｕ_ｄｉが関節まわりのトルクを、Ｕ_ｓｉが関節に対する剛性および粘性を増減させることがわかる。したがって、Ｕ_ｄｉ，Ｕ_ｓｉをそれぞれトルク指令値、関節剛性指令値とすると、式（１０３）より、アクチュエータの収縮力により生じるトルクと、筋の粘弾性により生じるトルクを独立に制御できることがわかる。例えば、関節剛性指令値Ｕ_ｓ１，Ｕ_ｓ２，Ｕ_ｓ３を、

It becomes. From equation (103), it can be seen that U _di increases and decreases the torque around the joint, and U _si increases and decreases the rigidity and viscosity of the joint. Therefore, when U _di and U _si are the torque command value and the joint stiffness command value, respectively, it can be seen from equation (103) that the torque generated by the contraction force of the actuator and the torque generated by the viscoelasticity of the muscle can be controlled independently. For example, the joint stiffness command values U _s1 , U _s2 and U _s3 are

と制御すると、スティフネス楕円Ｃの長軸は第１関節と手先を結ぶ方向を向くことが知られている。

It is known that the long axis of the stiffness ellipse C faces the direction connecting the first joint and the hand.

However, in the control system of the first embodiment and the second embodiment, the joint stiffness becomes equal to the contraction force by the control side shown in the equation (16), so the hand stiffness during driving is arbitrarily designated. It is not possible. Therefore, in the third embodiment, the preliminary contraction force is added to the contraction force output from the contraction force command value calculation unit 152 so as to realize desired hand rigidity during driving. Therefore, the contraction force in the control system of the third embodiment increases compared to the first and second embodiments in which the rigidity control is not performed. Assuming that the preliminary contraction force is Δu _fi and Δu _ei , the contraction force command values u _fi ′ and u _ei ′ actually given to the two-link manipulator are

となる。式（１０５）より、収縮力の和を関節剛性の目標軌道Ｕ_ｓｉ（ｉ＝１，２，３）と等しくするためには、予備収縮力Δｕ_ｆｉ，Δｕ_ｅｉを、

It becomes. From equation (105), in order to make the sum of the contraction forces equal to the joint stiffness target trajectory U _si (i = 1, 2, 3), the preliminary contraction forces Δu _fi and Δu _ei are

とすればよいことがわかる。また、予備収縮力を考慮すると、筋トルクτ_ｉは、

You can see that. In addition, considering the precontraction force, the muscle torque τ _i is

となる。ここで、右辺の第３項、第４項は予備収縮力により関節周りに生じるトルクを表し、マニピュレータに印加される外乱となる。そこで、予備収縮力による筋トルクを発生させないように、予備収縮力Δｕ_ｆｉ，Δｕ_ｅｉを、

It becomes. Here, the third term and the fourth term on the right side represent torque generated around the joint by the preliminary contraction force, and are disturbances applied to the manipulator. Therefore, in order not to generate muscle torque due to the precontraction force, the precontraction force Δu _fi and Δu _ei are

を満たすよう制御する。式（１０６）と式（１０８）を予備収縮力Δｕ_ｆｉ，Δｕ_ｅｉについて解くと、

Control to meet. Solving the equations (106) and (108) for the preliminary contraction forces Δu _fi and Δu _ei ,

となる。

It becomes.

FIG. 26 is a block diagram showing a robot control apparatus according to the third embodiment of the present invention. In FIG. 26, the control device 150 </ b> A includes a feedforward compensation unit 151 as a torque command value generation unit, a contraction force command value calculation unit 152, and a contraction force generation unit 153. Furthermore, the control device 150 </ b> A of the third embodiment has a stiffness control unit 154. In FIG. 26, the stiffness control unit 154 receives the contraction force command values u _fi and u _ei output from the contraction force command value calculation unit 152, and uses the equations (109) and (110) to perform the preliminary contraction force Δu _fi. , Δu _ei is calculated.

(3 simulation)
Perform a simulation using the control system in the previous section. The physical parameters of the first link 101 and the second link 102 and the target trajectory of the joint angle are the same as those in the second embodiment. Further, the joint stiffness target trajectory U _si (i = 1, 2, 3) is set to the jerk minimum trajectory that increases from 8N to 12N during 0 sec to 0.3 sec.

FIG. 27 shows the joint trajectory target trajectory Usi (i = 1, 2, 3). 28 (a), 28 (b) and 28 (c), the contraction force command values u _f1 ′, u _f2 ′, u _f3 ′ are shown by solid lines, and the contraction force command values u _e1 ′, u _e2. ', U _e3 ' is represented by a broken line, and the sum of contraction forces is represented by a dotted line. 28 (a), 28 (b), and 28 (c), it can be seen that the contraction force command values u _fi 'and u _ei ' are both 0 N or more. This is because the preliminary contraction forces Δu _fi and Δu _ei are added to the contraction force command values u _fi and u _ei output from the contraction force command value calculation unit 152. Furthermore, since the solid line in FIG. 27 and the dotted lines in FIGS. 28 (a), 28 (b), and 28 (c) match, the sum of the contractile force command values u _fi ', u _ei ' is the joint. It can be seen that it matches the rigidity target trajectory _Usi . From these, it can be seen that the flexibility of the hand can be controlled by the control system of the third embodiment.

  The present invention is not limited to the embodiments described above, and many modifications can be made by those having ordinary knowledge in the art within the technical idea of the present invention.

DESCRIPTION OF SYMBOLS 100 ... Robot, 101 ... 1st link, 102 ... 2nd link, 103 ... Pulley (base), 111 ... 1st joint, 112 ... 2nd joint, 150 ... Control apparatus, 151 ... Feedforward compensation part (torque command Value generating unit), 152 ... contraction force command value calculation unit, 153 ... contraction force generation unit, e ₁ ... second artificial muscle actuator, e ₂ ... fourth artificial muscle actuator, e ₃ ... sixth artificial muscle actuator, f ₁ ... 1st artificial muscle actuator, f ₂ ... 3rd artificial muscle actuator, f ₃ ... 5th artificial muscle actuator

Claims (4)

A first link connected to the base by a first joint;
A second link connected to the first link by a second joint;
A first artificial muscle actuator that pivots the first link around the first joint in a first pivoting direction by contraction;
A second artificial muscle actuator that rotates the first link around the first joint in a second turning direction opposite to the first turning direction by contraction;
A third artificial muscle actuator that pivots the second link around the second joint in the first pivot direction by contraction;
A fourth artificial muscle actuator for rotating the second link around the second joint in the second turning direction by contraction;
A fifth artificial muscle actuator that simultaneously turns the first and second links around the first and second joints in the first turning direction by contraction;
A sixth artificial muscle actuator that simultaneously turns the first and second links around the first and second joints in the second turning direction by contraction;
A torque command value generating unit that generates a first joint torque command value of the first joint and a second joint torque command value of the second joint;
Based on the first joint torque command value and the second joint torque command value, a first contraction force command value of the first artificial muscle actuator, a second contraction force command value of the second artificial muscle actuator, and the third The third contraction force command value of the artificial muscle actuator, the fourth contraction force command value of the fourth artificial muscle actuator, the fifth contraction force command value of the fifth artificial muscle actuator, and the sixth contraction of the sixth artificial muscle actuator A contraction force command value calculation unit for obtaining a force command value;
The first artificial muscle actuator is contracted according to the first contraction force command value, the second artificial muscle actuator is contracted according to the second contraction force command value, and according to the third contraction force command value. Contracting the third artificial muscle actuator, contracting the fourth artificial muscle actuator according to the fourth contraction force command value, contracting the fifth artificial muscle actuator according to the fifth contraction force command value, A contraction force generation unit that contracts the sixth artificial muscle actuator according to the sixth contraction force command value;
The contraction force command value calculation unit is
The first joint torque command value is a constant, the first, second, fifth, and sixth contraction force command values are used as variables, the first function, and the second joint torque command value is a constant, and the third , In the second function that holds the fourth, fifth and sixth contraction force command values as variables, one of the first and second contraction force command values, one of the third and fourth contraction force command values, and the above When one of the fifth and sixth contraction force command values is 0, the other of the first and second contraction force command values, the other of the third and fourth contraction force command values, and the fifth and sixth contraction force command values. The other one of the first and second contraction force command values, the other one of the third and fourth contraction force command values, and the other one of the other six contraction force command values, Command value calculation processing for calculating the other of the fifth and sixth contraction force command values;
And a command value output process for outputting the first to sixth contraction force command values calculated in the command value calculation process to the contraction force generation unit. The first contraction force command value is u _f1 , the second contraction force command value is u _e1 , the third contraction force command value is u _f2 , the fourth contraction force command value is u _e2 , and the fifth contraction force command is The value is u _f3 , the sixth contractile force command value is u _e3 , the first joint torque command value is T ₁ , and the second joint torque command value is T ₂ ,
Using the positive coefficients K _f1 , K _e1 , K _f2 , K _e2 , K _f3 , and K _e3 , the first function is expressed as T ₁ = K _f1 × u _f1 −K _e1 × u _e1 + K _f3 × u _f3 − K _e3 × u _e3 and the second function is T ₂ = K _f2 × u _f2 −K _e2 × u _e2 + K _f3 × u _f3 −K _e3 × u _e3
When the evaluation function is J = max {u _f1 , u _e1 , u _f2 , u _e2 , u _f3 , u _e3 },
The contraction force command value calculation unit is
As the command value calculation process,
In the first function and the second function, when u _e1 = 0, u _e2 = 0, u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A first calculation process for calculating one command value candidate;
In the first function and the second function, when u _e1 = 0, u _f2 = 0, and u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A second calculation process for calculating two command value candidates;
In the first function and the second function, when u _f1 = 0, u _e2 = 0, and u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A third calculation process for calculating three command value candidates;
In the first function and the second function, when u _f1 = 0, u _f2 = 0, u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A fourth calculation process for calculating four command value candidates;
A command selection process for selecting a command value candidate having the smallest evaluation function among the first to fourth command value candidates as the first to sixth contraction force command values to be output in the command value output process; The robot according to claim 1, wherein: The first contraction force command value is u _f1 , the second contraction force command value is u _e1 , the third contraction force command value is u _f2 , the fourth contraction force command value is u _e2 , and the fifth contraction force command is The value is u _f3 , the sixth contractile force command value is u _e3 , the first joint torque command value is T ₁ , and the second joint torque command value is T ₂ ,
Using the positive coefficients K _f1 , K _e1 , K _f2 , K _e2 , K _f3 , and K _e3 , the first function is expressed as T ₁ = K _f1 × u _f1 −K _e1 × u _e1 + K _f3 × u _f3 − K _e3 × u _e3 and the second function is T ₂ = K _f2 × u _f2 −K _e2 × u _e2 + K _f3 × u _f3 −K _e3 × u _e3
When the evaluation function is J = max {u _f1 , u _e1 , u _f2 , u _e2 , u _f3 , u _e3 },
The contraction force command value calculation unit is
As the command value calculation process,
When the first joint torque command value and the second joint torque command value have the same sign,
In the first function and the second function, when u _e1 = 0, u _e2 = 0, u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A first calculation process for calculating one command value candidate;
In the first function and the second function, when u _e1 = 0, u _f2 = 0, and u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A second calculation process for calculating two command value candidates;
In the first function and the second function, when u _f1 = 0, u _e2 = 0, and u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A third calculation process for calculating three command value candidates;
In the first function and the second function, when u _f1 = 0, u _f2 = 0, u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A fourth calculation process for calculating four command value candidates;
A first command selection process for selecting a command value candidate having the smallest evaluation function among the first to fourth command value candidates as the first to sixth contraction force command values to be output in the command value output process. And run
When the first joint torque command value and the second joint torque command value have different signs,
In the first function and the second function, when u _e1 = 0, u _f2 = 0, and u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A fifth calculation process for calculating five command value candidates;
In the first function and the second function, when u _f1 = 0, u _f2 = 0, u _e3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A sixth calculation process for calculating six command value candidates;
In the first function and the second function, when u _e1 = 0, u _e2 = 0, and u _f3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. A seventh calculation process for calculating seven command value candidates;
In the first function and the second function, when u _e1 = 0, u _f2 = 0, and u _f3 = 0, the first to sixth contraction force command values that minimize the evaluation function are set. An eighth calculation process for calculating eight command value candidates;
Second command selection process for selecting command value candidates having the smallest evaluation function among the fifth to eighth command value candidates as the first to sixth contraction force command values to be output in the command value output process The robot according to claim 1, wherein: A first link connected to the base by a first joint;
A second link connected to the first link by a second joint;
A first artificial muscle actuator that pivots the first link around the first joint in a first pivoting direction by contraction;
A second artificial muscle actuator that rotates the first link around the first joint in a second turning direction opposite to the first turning direction by contraction;
A third artificial muscle actuator that pivots the second link around the second joint in the first pivot direction by contraction;
A fourth artificial muscle actuator for rotating the second link around the second joint in the second turning direction by contraction;
A fifth artificial muscle actuator that simultaneously turns the first and second links around the first and second joints in the first turning direction by contraction;
A sixth artificial muscle actuator that simultaneously turns the first and second links around the first and second joints in the second turning direction by contraction;
A torque command value generating unit that generates a first joint torque command value of the first joint and a second joint torque command value of the second joint;
Based on the first joint torque command value and the second joint torque command value, a first contraction force command value of the first artificial muscle actuator, a second contraction force command value of the second artificial muscle actuator, and the third The third contraction force command value of the artificial muscle actuator, the fourth contraction force command value of the fourth artificial muscle actuator, the fifth contraction force command value of the fifth artificial muscle actuator, and the sixth contraction of the sixth artificial muscle actuator A contraction force command value calculation unit for obtaining a force command value;
The first artificial muscle actuator is contracted according to the first contraction force command value, the second artificial muscle actuator is contracted according to the second contraction force command value, and according to the third contraction force command value. Contracting the third artificial muscle actuator, contracting the fourth artificial muscle actuator according to the fourth contraction force command value, contracting the fifth artificial muscle actuator according to the fifth contraction force command value, In a robot control method for controlling a robot, comprising: a contraction force generation unit configured to contract the sixth artificial muscle actuator according to the sixth contraction force command value;
The first contraction force command value calculation unit uses the first joint torque command value as a constant and the first, second, fifth, and sixth contraction force command values as variables, and the second joint torque. In the second function that is established with the command value as a constant and the third, fourth, fifth, and sixth contraction force command values as variables, one of the first and second contraction force command values, the third and fourth When one of the contraction force command values and one of the fifth and sixth contraction force command values is set to 0, the other of the first and second contraction force command values, the third and fourth contraction force command values. And the other of the first and second contraction force command values, the third and third contraction command values so that the contraction force command value that is the maximum value among the other of the fifth and sixth contraction force command values is minimized. A finger for calculating the other of the four contraction force command values and the other of the fifth and sixth contraction force command values. Value calculation step,
The contraction force command value calculation unit includes a command value output processing step of outputting the first to sixth contraction force command values calculated in the command value calculation processing step to the contraction force generation unit. A robot control method.

Images