JP5943734B2 - ロボット及びロボット制御方法 - Google Patents

Description

本発明は、人工筋肉アクチュエータがリンクに対して拮抗配置されたロボット及び該ロボットを制御するロボット制御方法に関する。

マニピュレータの制御方法において、手先が対象物に柔軟に接触できることが重要になってきている。これを産業ロボットに応用すれば、人間との共同作業が実現でき、手先の柔軟性の方向を制御することにより部品の嵌め合い作業等が容易になる。また、これを脚式移動ロボットに応用すれば、地面に柔らかく接地することで胴体に対する衝撃を和らげることができ、段差を吸収することで不整地を安定に歩行することを可能とする。

手先の柔軟性の制御を実現するために、手先に力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉の中でも、特にＭｃＫｉｂｂｅｎ型人工筋肉に代表される空気圧ゴム人工筋肉は、粘弾性特性が筋に類似している。そこで、ロボットにおける人工筋肉アクチュエータの柔らかさ制御することで、任意の手先の柔軟性で対象物に接触することができる。しかし、人工筋肉アクチュエータは、粘弾性特性に非線系性を有している、収縮方向のみに力を発生するため拮抗配置して制御を行う必要がある、などの理由で制御性に難があることが知られている。

従来、筋の粘弾性特性を含むマニピュレータのモデルと、修正値計算部を用いて関節角度と手先の柔軟性を同時制御するためのフィードフォワード入力を生成しているものが提案されている（特許文献１参照）。特許文献１では、モデルを用いて制御入力が与えられた時の関節角度と人工筋肉アクチュエータの粘弾性係数を出力し、それを目標値と比較する。そして、目標値との誤差を修正値計算部に逆伝播させ、フィードフォワード入力を修正する。修正されたフィードフォワード入力を再びモデルに与える操作を繰り返し、徐々にフィードフォワード入力を求めている。

特許第３４３６３２０号公報

上記特許文献１では、フィードフォワード制御により関節角度と手先の柔軟性の同時制御を行っている。しかし、人工筋肉アクチュエータを最小の制御入力で拮抗駆動するための研究例はこれまで見られない。

制御入力を最小化するには、関節角度の目標軌道に対応する手先柔軟性の目標軌道を考慮し、それを制御入力と同時に最適化する必要がある。しかし、上記特許文献１のフィードフォワード制御は、手先柔軟性の目標軌道をあらかじめ与えており、それを最適化するアルゴリズムは含まれていない。

そこで、本発明は、人工筋肉アクチュエータを駆動源に用いる２リンクの関節角度の軌道制御において、最小の収縮力指令値で拮抗駆動を行うことを目的とするものである。

本発明のロボットは、基台に第１関節で連結された第１リンクと、前記第１リンクに第２関節で連結された第２リンクと、収縮により前記第１リンクを前記第１関節まわりに第１旋回方向に旋回させる第１人工筋肉アクチュエータと、収縮により前記第１リンクを前記第１関節まわりに前記第１旋回方向に対して反対の第２旋回方向に旋回させる第２人工筋肉アクチュエータと、収縮により前記第２リンクを前記第２関節まわりに前記第１旋回方向に旋回させる第３人工筋肉アクチュエータと、収縮により前記第２リンクを前記第２関節まわりに前記第２旋回方向に旋回させる第４人工筋肉アクチュエータと、収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第１旋回方向に旋回させる第５人工筋肉アクチュエータと、収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第２旋回方向に旋回させる第６人工筋肉アクチュエータと、前記第１関節の第１関節トルク指令値及び前記第２関節の第２関節トルク指令値を生成するトルク指令値生成部と、前記第１関節トルク指令値及び前記第２関節トルク指令値に基づいて、前記第１人工筋肉アクチュエータの第１収縮力指令値、前記第２人工筋肉アクチュエータの第２収縮力指令値、前記第３人工筋肉アクチュエータの第３収縮力指令値、前記第４人工筋肉アクチュエータの第４収縮力指令値、前記第５人工筋肉アクチュエータの第５収縮力指令値、及び前記第６人工筋肉アクチュエータの第６収縮力指令値を求める収縮力指令値演算部と、前記第１収縮力指令値に応じて前記第１人工筋肉アクチュエータを収縮させ、前記第２収縮力指令値に応じて前記第２人工筋肉アクチュエータを収縮させ、前記第３収縮力指令値に応じて前記第３人工筋肉アクチュエータを収縮させ、前記第４収縮力指令値に応じて前記第４人工筋肉アクチュエータを収縮させ、前記第５収縮力指令値に応じて前記第５人工筋肉アクチュエータを収縮させ、前記第６収縮力指令値に応じて前記第６人工筋肉アクチュエータを収縮させる収縮力発生部と、を備え、前記収縮力指令値演算部は、前記第１関節トルク指令値を定数とし、前記第１、第２、第５及び第６収縮力指令値を変数として成立する第１関数、並びに前記第２関節トルク指令値を定数とし、前記第３、第４、第５及び第６収縮力指令値を変数として成立する第２関数において、前記第１及び第２収縮力指令値の一方、前記第３及び第４収縮力指令値の一方、並びに前記第５及び第６収縮力指令値の一方を０とした場合に、前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方のうち最大値となる収縮力指令値が最小となるように前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方を演算する指令値演算処理と、前記指令値演算処理で演算した前記第１から第６収縮力指令値を前記収縮力発生部に出力する指令値出力処理と、を実行することを特徴とする。

本発明によれば、手先柔軟性の目標軌道が一意に決定され、各人工筋肉アクチュエータが最小の収縮力指令値で拮抗駆動されるので、収縮力発生部において各人工筋肉アクチュエータに収縮力を発生させるのに必要な例えば空気等の消費量を最小にすることができる。したがって、ロボットの駆動時間を延長することが可能となる。

本発明の第１実施形態に係る人工筋肉アクチュエータの粘弾性モデルを示す説明図である。 本発明の第１実施形態に係るロボットの概略構成を示す説明図である。 本発明の第１実施形態に係るロボットの制御装置を示すブロック線図である。 本発明の第１実施形態に係る筋トルクの符号の組み合わせを示す図である。 本発明の第１実施形態に係る収縮力指令値演算部の処理動作を示すフローチャートである。 本発明の第１実施形態に係る収縮力指令値演算部による第１最小化演算処理のフローチャートである。 直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の一例を示すグラフである。 直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の別の例を示すグラフである。 直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の別の例を示すグラフである。 本発明の第１実施形態に係る収縮力指令値演算部による第１演算処理のフローチャートである。 本発明の第１実施形態に係る収縮力指令値演算部による第２演算処理のフローチャートである。 本発明の第１実施形態に係る収縮力指令値演算部による第３演算処理のフローチャートである。 本発明の第１実施形態に係る収縮力指令値演算部による第４演算処理のフローチャートである。 本発明の第１実施形態に係る収縮力指令値演算部による第１指令選択処理のフローチャートである。 本発明の第２実施形態に係る本発明の第３実施形態に係るロボットの収縮力指令値演算部の処理動作を示すフローチャートである。 本発明の第２実施形態に係る収縮力指令値演算部による第２最小化演算処理のフローチャートである。 本発明の第２実施形態に係る収縮力指令値演算部による第５演算処理のフローチャートである。 本発明の第２実施形態に係る収縮力指令値演算部による第６演算処理のフローチャートである。 本発明の第２実施形態に係る収縮力指令値演算部による第７演算処理のフローチャートである。 本発明の第２実施形態に係る収縮力指令値演算部による第８演算処理のフローチャートである。 本発明の第２実施形態に係るシミュレーション結果を示す図である。 本発明の第２実施形態に係るシミュレーション結果を示す図である。 本発明の第２実施形態に係るシミュレーション結果を示す図である。 本発明の第２実施形態に係るシミュレーション結果を示す図である。 本発明の第３実施形態に係るスティフネス楕円を表す図である。 本発明の第３実施形態に係るロボットの制御装置を示すブロック線図である。 本発明の第３実施形態に係るシミュレーション結果を示す図である。 本発明の第３実施形態に係るシミュレーション結果を示す図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
本発明の第１実施形態では、駆動源として人工筋肉アクチュエータを用いる２リンクマニピュレータに対して、関節角度の目標軌道に追従するための制御入力を最小化する。なお、第１実施形態では、目標軌道へ追従するために必要となる第１、第２関節まわりのトルクがどちらも正数となるときに限定し、制御入力の導出方法を示す。

（１ モデル導出）
本節では、人工筋肉アクチュエータを駆動源とする、２リンクマニピュレータのモデルを導出する。本第１実施形態で用いる人工筋肉アクチュエータとは、筋の粘弾性特性と類似する特性を有するアクチュエータであり、図１に示すように、力発生要素１１と弾性要素１２と粘性要素１３を用いてモデル化される。力発生要素１１の収縮力をｕ、収縮方向を正とする筋の収縮量をｘとする。収縮速度を、

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

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

図２は、本発明の第１実施形態に係るロボットの概略構成を示す説明図である。このロボット１００は、２リンクマニピュレータである。ロボット１００は、基台であるプーリ１０３と、プーリ１０３に関節（第１関節）１１１で連結されたリンク（第１リンク）１０１と、リンク１０１に関節（第２関節）１１２で連結されたリンク（第２リンク）１０２とを備えている。

本第１実施形態では、ロボット１００は、６つの人工筋肉アクチュエータ（以下、単に「アクチュエータ」という）ｆ_１，ｅ_１，ｆ_２，ｅ_２，ｆ_３，ｅ_３と、これらアクチュエータｆ_１，ｅ_１，ｆ_２，ｅ_２，ｆ_３，ｅ_３を制御する制御装置１５０と、を備えている。

第１人工筋肉アクチュエータであるアクチュエータｆ_１は、収縮により第１リンク１０１を第１関節１１１まわりに第１旋回方向Ｒ_１（図２中、反時計回り方向、屈曲する方向）に旋回させる。第２人工筋肉アクチュエータであるアクチュエータｅ_１は、収縮により第１リンク１０１を第１関節１１１まわりに第１旋回方向Ｒ_１に対して反対の第２旋回方向Ｒ_２（図２中、時計回り方向、伸展する方向）に旋回させる。第３人工筋肉アクチュエータであるアクチュエータｆ_２は、収縮により第２リンク１０２を第２関節１１２まわりに第１旋回方向Ｒ_１に旋回させる。第４人工筋肉アクチュエータであるアクチュエータｅ_２は、収縮により第２リンク１０２を第２関節１１２まわりに第２旋回方向Ｒ_２に旋回させる。第５人工筋肉アクチュエータであるアクチュエータｆ_３は、収縮により第１及び第２リンク１０１，１０２を同時に第１及び第２関節１１１，１１２まわりに第１旋回方向Ｒ_１に旋回させる。第６人工筋肉アクチュエータであるアクチュエータｅ_３は、収縮により第１及び第２リンク１０１，１０２を同時に第１及び第２関節１１１，１１２まわりに第２旋回方向Ｒ_２に旋回させる。

つまり、アクチュエータｆ_１，ｅ_１は第１関節１１１を、アクチュエータｆ_２，ｅ_２は第２関節１１２を駆動し、アクチュエータｆ_３，ｅ_３は二関節１１１，１１２を同時に駆動する。

制御装置１５０は、各アクチュエータｆ_ｉ，ｅ_ｉの収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを演算により求めて、各収縮力指令値ｕ_ｆｉ，ｕ_ｅｉに応じて各アクチュエータｆ_ｉ，ｅ_ｉを収縮させる（ｉ＝１，２，３）。ここで、アクチュエータｆ_１の収縮力指令値を、第１収縮力指令値ｕ_ｆ１とする。アクチュエータｅ_１の収縮力指令値を、第２収縮力指令値ｕ_ｅ１とする。アクチュエータｆ_２の収縮力指令値を、第３収縮力指令値ｕ_ｆ２とする。アクチュエータｅ_２の収縮力指令値を、第４収縮力指令値ｕ_ｅ２とする。アクチュエータｆ_３の収縮力指令値を、第５収縮力指令値ｕ_ｆ３とする。アクチュエータｅ_３の収縮力指令値を、第６収縮力指令値ｕ_ｅ３とする。

アクチュエータｆ_ｉ，ｅ_ｉ（ｉ＝１，２，３）の収縮量ｘ_ｉは、モーメント径ｒと関節角度θ_ｉ（ｉ＝１，２，３）を用いて、

と表わせる。

ここで、アクチュエータｆ_ｉ，ｅ_ｉの弾性力定数をｋ_ｉ、粘性力定数をｃ_ｉ、関節角速度をω_ｉ、関節角加速度をα_ｉとする（ｉ＝１，２，３）。アクチュエータｆ_ｉ，ｅ_ｉの収縮力指令値ｕ_ｆｉ，ｕ_ｅｉ（ｉ＝１，２，３）により生じるトルク（以下、筋トルクと呼ぶ）τ_ｉ（ｉ＝１，２，３）は、

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

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

式（３）より、右辺第１項の収縮力の差が関節に回転トルクを与え、第２項と第３項の収縮力の和が、筋の弾性と粘性を変動させることがわかる。なお、前述のように人工筋肉アクチュエータｆ_ｉ，ｅ_ｉ（ｉ＝１，２，３）は収縮力のみを発揮する。そのため、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉ（ｉ＝１，２，３）は、

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

ここで、第１，第２リンク１０１，１０２の関節１１１，１１２まわりの慣性モーメントをＩ_１，Ｉ_２、リンク１０１，１０２長さをＬ_１，Ｌ_２、リンク１０１，１０２の質量をｍ_１，ｍ_２とする。また、関節１１１，１１２まわりに働くトルク指令値（第１関節トルク指令値及び第２関節トルク指令値）をＴ_１，Ｔ_２とする。本第１実施形態の２リンクマニピュレータの運動方程式は、

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

コリオリ力・遠心力は、

である。

慣性行列及びコリオリ力・遠心力は、以下の式（７）及び式（８）となる。

また、第１関節トルク指令値Ｔ_１、第１関節トルク指令値Ｔ_２は、筋トルクτ_１，τ_２，τ_３を用いて、

となる。

式（３）と式（９）とにより、関節トルク指令値Ｔ_１，Ｔ_２は、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを用いて、

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

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

つまり、式（１０）は、第１関節トルク指令値Ｔ_１を定数とし、第１、第２、第５及び第６収縮力指令値ｕ_ｆ１，ｕ_ｅ１，ｕ_ｆ３，ｕ_ｅ３を変数として成立する第１関数である。式（１１）は、第２関節トルク指令値Ｔ_２を定数とし、第３、第４、第５及び第６収縮力指令値ｕ_ｆ２，ｕ_ｅ２，ｕ_ｆ３，ｕ_ｅ３を変数として成立する第２関数である。

（２ 制御系設計）
本第１実施形態の制御系では、関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］に基づき、収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］の最大値を最小化する。なお、時刻ｋは離散信号であり、制御周期Ｔ_ｓと正の整数ｎを用いて、

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

図３は、本発明の第１実施形態に係るロボットの制御装置を示すブロック線図である。図３において、制御装置１５０は、トルク指令値生成部としてのフィードフォワード補償部１５１と、収縮力指令値演算部１５２と、収縮力発生部１５３と、を備えている。

フィードフォワード補償部１５１は、時刻ｋにおける各関節１１１，１１２の目標関節角度θ_ｒｉ［ｋ］、目標角速度ω_ｒｉ［ｋ］、目標角加速度α_ｒｉ［ｋ］（ｉ＝１，２）を入力とする。そして、

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

また、収縮力指令値演算部１５２は、目標関節角度θ_ｒｉ［ｋ］、目標角速度ω_ｒｉ［ｋ］、及び関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］を入力し、これらに基づいて、最大値が最小となる収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］を求める。

収縮力発生部１５３は、各収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］に応じて各アクチュエータｆ_ｉ，ｅ_ｉを収縮させる。

制御系設計では、まず、収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］の最大値を最小化する問題（以下、収縮力最小化問題と呼ぶ）を定義し、同問題を解くための制御側を導出する。次に、係数Ｋ_ｆｉ，Ｋ_ｅｉと関節トルク指令値Ｔ_１［ｋ］，Ｔ_２［ｋ］に基づく場合分けを行い、収縮力指令値ｕ_ｆｉ［ｋ］，ｕ_ｅｉ［ｋ］を演算する。なお、本第１実施形態の制御系設計では表記を簡潔にするために、時刻を表す［ｋ］を省略する。

（２．１ 関節剛性と収縮力の同時制御）
まず、評価関数をＪとし、Ｊを次式（１５）と定義する。

つまり、Ｊ＝ｍａｘ｛ｕ_ｆ１，ｕ_ｅ１，ｕ_ｆ２，ｕ_ｅ２，ｕ_ｆ３，ｕ_ｅ３｝と定義する。収縮力最小化問題とは、関節トルク指令値Ｔ_１，Ｔ_２と、目標関節角度θ_ｒ１，θ_ｒ２と、目標関節角速度ω_ｒ１，ω_ｒ２とが与えられたとき、式（３）〜式（５）及び式（９）を満たし、かつ評価関数Ｊを最小化する収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを求める問題である。

収縮力最小化問題では、式（１０）、式（１１）、式（１５）の３つが制約条件となるが、未知数である収縮力指令値ｕ_ｆｉ，ｕ_ｅｉの数は６つである。一般に、制約条件の数に比べて未知数の数が多い場合は、未知数を一意に決めることはできない。しかし、筋トルクτ_ｉの正負に応じて収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを、

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

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

式（１６）に示す制御側を適用するためには、筋トルクτ_１，τ_２，τ_３の正負の符号が決定されている必要がある。しかし、式（９）に示すように、筋トルクτ_１，τ_２，τ_３は関節トルク指令値Ｔ_１，Ｔ_２に対して冗長となるため、関節トルク指令値の符号から筋トルクの符号を一意に決定することはできない。

そこで、本第１実施形態では、筋トルクτ_１，τ_２，τ_３の符号の組み合わせのうち一つを仮定し、この仮定の下で評価関数Ｊを最小化する収縮力指令値ｕ_ｆｉ，ｕ_ｅｉの候補を導出する。これを、筋トルクτ_１，τ_２，τ_３の符号の組み合わせ全てについて行い、収縮力の候補の中から評価関数Ｊを最小化する収縮力を選び出す。

図４は、筋トルクτ_１，τ_２，τ_３の符号が取りうる８通りの組み合わせを示す図である。ただし、本第１実施形態のように、関節トルク指令値Ｔ_１，Ｔ_２がどちらも正数となる場合は、筋トルクτ_３を負数とする条件５、条件６、条件７、条件８の組み合わせを仮定する必要はない。なぜなら、筋トルクτ_３を負数とすると、式（９）より、τ_３を正数とする場合と比べて筋トルクτ_１と筋トルクτ_２を増加させなければならず、これは、第一、第二単関節筋の収縮力を増加させてしまうためである。

そこで、本第１実施形態では、条件１、条件２、条件３、条件４の４通りの組み合わせについてのみ、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを導出する。

次に、制御装置１５０によるロボット制御方法について説明する。図５は、収縮力指令値演算部の処理動作を示すフローチャートである。収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２をフィードフォワード補償部１５１から入力すると共に、不図示の上位コントローラから目標関節角度θ_１，θ_２、目標関節角速度ω_１，ω_２を入力する（Ｓ１）。なお、弾性力定数ｋ、粘性力定数ｃ及びモーメントアーム径ｒは、予め設定された値である。

次に、収縮力指令値演算部１５２は、式（１２）を用いて、目標関節角度θ_１，θ_２、目標関節角速度ω_１，ω_２から係数Ｋ_ｆｉ，Ｋ_ｅｉを算出する（Ｓ２）。

次に、収縮力指令値演算部１５２は、式（１０）に示す第１関数、及び式（１１）に示す第２関数に、演算した係数Ｋ_ｆｉ，Ｋ_ｅｉ及び入力した関節トルク指令値Ｔ_１，Ｔ_２を定数として代入する（Ｓ３）。

次に、収縮力指令値演算部１５２は、評価関数Ｊを最小化する指令値候補を演算する第１最小化演算処理を実行する（Ｓ４）。

次に、収縮力指令値演算部１５２は、評価関数Ｊ_ｍｉｎ ^１〜^４及び指令値候補ｕ_ｆｉ ^１〜４，ｕ_ｅｉ ^１〜４に対し（Ｓ５）、評価関数が最小の指令値候補を選択する第１指令選択処理を実行する（Ｓ６）。

即ち、収縮力指令値演算部１５２は、これらＳ２〜Ｓ６において、第１から第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを演算する指令値演算処理を実行する（指令値演算処理工程）。具体的には、収縮力指令値演算部１５２は、式（１０）に示す第１関数及び式（１１）に示す第２関数において、第１及び第２収縮力指令値の一方、第３及び第４収縮力指令値の一方、並びに第５及び第６収縮力指令値の一方を０とする。そして、収縮力指令値演算部１５２は、第１及び第２収縮力指令値の他方、第３及び第４収縮力指令値の他方、並びに第５及び第６収縮力指令値の他方のうち最大値となる収縮力指令値が最小となるようにこれらを演算する。

次に、収縮力指令値演算部１５２は、演算した第１から第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを収縮力発生部１５３に出力する指令値出力処理を実行する（Ｓ７：指令値出力処理工程）。

（２．２ 第１最小化演算処理の設計）
図６は、収縮力指令値演算部による第１最小化演算処理のフローチャートである。収縮力指令値演算部１５２は、図４に示す第１条件を仮定し（Ｓ１１）、第１演算処理を実行し（Ｓ１２）、評価関数Ｊ_ｍｉｎ ^１を最小化する第１〜第６収縮力指令値からなる第１指令値候補ｕ_ｆｉ ^１，ｕ_ｅｉ ^１を出力する（Ｓ１３）。

続いて、収縮力指令値演算部１５２は、図４に示す第２条件を仮定し（Ｓ１４）、第２演算処理を実行し（Ｓ１５）、評価関数Ｊ_ｍｉｎ ^２を最小化する第１〜第６収縮力指令値からなる第２指令値候補ｕ_ｆｉ ^２，ｕ_ｅｉ ^２を出力する（Ｓ１６）。

続いて、収縮力指令値演算部１５２は、図４に示す第３条件を仮定し（Ｓ１７）、第３演算処理を実行し（Ｓ１８）、評価関数Ｊ_ｍｉｎ ^３を最小化する第１〜第６収縮力指令値からなる第３指令値候補ｕ_ｆｉ ^３，ｕ_ｅｉ ^３を出力する（Ｓ１９）。

続いて、収縮力指令値演算部１５２は、図４に示す第４条件を仮定し（Ｓ２０）、第４演算処理を実行し（Ｓ２１）、評価関数Ｊ_ｍｉｎ ^４を最小化する第１〜第６収縮力指令値からなる第４指令値候補ｕ_ｆｉ ^４，ｕ_ｅｉ ^４を出力する（Ｓ２２）。

第１、第２、第３、第４演算処理は、それぞれ、筋トルクτ_１，τ_２の符号を図４の条件１、２、３、４と仮定して、評価関数Ｊの最小値の候補Ｊ_ｍｉｎ ^ｊと収縮力指令値の指令値候補ｕ_ｆｉ ^ｊ、ｕ_ｅｉ ^ｊを演算するサブルーチンである（ｊ＝１，２，３，４）。なお、評価関数の候補Ｊ_ｍｉｎ ^ｊと収縮力指令値の候補ｕ_ｆｉ ^ｊ、ｕ_ｅｉ ^ｊの上付きの添え字ｊは対応する演算処理の番号を表す。また、第１〜第４演算処理が評価関数の候補Ｊ_ｍｉｎ ^ｊと収縮力指令値の指令値候補ｕ_ｆｉ ^ｊ，ｕ_ｅｉ ^ｊを演算する方法は同様であるため、本第１実施形態では第１演算処理における演算方法を詳細に述べ、第２〜第４演算処理における演算方法は簡潔に述べる。

（２．２．１ 第１演算処理の設計）
第１演算処理における収縮力指令値の第１指令値候補ｕ_ｆｉ ^１，ｕ_ｅｉ ^１のうち、第１指令値候補ｕ_ｅ１ ^１、ｕ_ｅ２ ^１、ｕ_ｅ３ ^１は、図４の条件１と式（１６）により、

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

また、式（１０）、式（１１）及び式（１８）より、関節トルク指令値Ｔ_１，Ｔ_２は、収縮力指令値の第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１を用いて、

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

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

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

式（２１）〜式（２３）により、第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１は全てｕ_ｆ３ ^１’を変数とする一次関数となることがわかる。そこで、本第１実施形態では、Ｘ軸を変数ｕ_ｆ３ ^１’と、Ｙ軸を第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１とする直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１のグラフを描画する。そして、変数ｕ_ｆ３ ^１’を増減させて、Ｘ座標を変数ｕ_ｆ３ ^１’とするＹ軸と平行な直線をＸ軸の正側と負側に移動させることで、交点の軌跡から評価関数Ｊの最小値を求める。

図７は、直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の一例を示すグラフである。直線ｕ_ｆ１ ^１は点線、直線ｕ_ｆ２ ^１は破線、直線ｕ_ｆ３ ^１は実線を用いて表している。また、Ｙ軸と平行な直線を、一点鎖線を用いて表している。さらに、変数ｕ_ｆ３ ^１’の変域を、矢印を用いて表している。そして、直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１とＹ軸と平行な直線の交点のうち、Ｙ座標が最大となる点を点Ｚと定義する。ここで、Ｙ軸と平行な直線と、直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の交点のＹ座標が求めるべき第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１であることから、点ＺのＹ座標は評価関数Ｊの値を表すことがわかる。

図７に示す例では、直線ｕ_ｆ１ ^１と直線ｕ_ｆ３ ^１の交点において評価関数Ｊが最小となる。なぜなら、変数ｕ_ｆ３ ^１’を減少させると点Ｚは直線ｕ_ｆ１ ^１に従って移動し、変数ｕ_ｆ３ ^１’を増加させると直線ｕ_ｆ３ ^１に従って移動するため、どちらの場合においても評価関数Ｊの値が増加してしまうためである。

ただし、評価関数Ｊは常に直線ｕ_ｆ１ ^１とｕ_ｆ３ ^１の交点において最小となるわけではない。それは、各直線のＸ切片、Ｙ切片、交点のＸ座標とＹ座標の大きさが、係数Ｋ_ｆ１，Ｋ_ｆ２，Ｋ_ｆ３と関節トルク指令値Ｔ_１，Ｔ_２に依存して変化するためである。

そこで、本第１実施形態では、各直線のＸ切片、Ｙ切片、交点のＸ座標とＹ座標の大きさに基づく場合分けの条件を導出し、それぞれの場合において評価関数Ｊが最小となる点を求める。ただし、直線ｕ_ｆ１ ^１と直線ｕ_ｆ２ ^１の両方を動かすと、場合分けの条件が複雑となってしまう。そこで、本第１実施形態では説明を容易にするために、直線ｕ_ｆ１ ^１のＸ切片とＹ切片を固定して図示する。そして、直線ｕ_ｆ２ ^１のＸ切片とＹ切片を原点からＸ軸とＹ軸の正の方向に、徐々に図上で移動させるときの直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１のグラフを図示し、評価関数Ｊが最小値を取る点を求める。

図８及び図９は、直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１の別の例を示すグラフである。まず、図８（ａ）及び図８（ｂ）と図９（ａ）及び図９（ｂ）とに、直線ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１と補助線を点線、破線、実線、一点鎖線を用いて、それぞれ表す。なお、図８（ａ）は、直線ｕ_ｆ２ ^１のＸ切片及びＹ切片が、直線ｕ_ｆ１ ^１のＸ切片及びＹ切片と比べて十分に小さいときのグラフを、図８（ｂ）は、直線ｕ_ｆ２ ^１のＸ切片をＸ軸の正方向へ移動させたときのグラフを表す。また、図９（ａ）は、図８（ｂ）と比べて直線ｕ_ｆ２ ^１のＹ切片をＹ軸の正方向へ移動させたときのグラフを、図９（ｂ）は、直線ｕ_ｆ２ ^１のＸ切片及びＹ切片をＸ軸及びＹ軸の正方向に十分に大きく移動させたときのグラフを表す。

次に、直線ｕ_ｆ１ ^１と直線ｕ_ｆ３ ^１の交点を点Ａ_１、点Ａ_１を通りＹ軸と平行な直線と直線ｕ_ｆ２ ^１の交点を点Ｂ_１、直線ｕ_ｆ２ ^１とＸ軸との交点をＣ_１、点Ｃ_１を通りＹ軸と平行な直線と直線ｕ_ｆ１ ^１との交点をＤ_１とする。また、直線ｕ_ｆ２ ^１と直線ｕ_ｆ３ ^１の交点を点Ｅ_１、点Ｅ_１を通りＹ軸と平行な直線と直線ｕ_ｆ１ ^１の交点を点Ｆ_１、直線ｕ_ｆ１ ^１とＸ軸との交点を点Ｇ_１、点Ｇ_１を通りＹ軸と平行な直線と直線ｕ_ｆ２ ^１との交点をＨ_１とする。なお、各点の下付きの添え字は対応する演算処理の番号を表す。

ここで、式（１８）〜式（２０）より、点Ａ_１，Ｂ_１，Ｃ_１，Ｄ_１，Ｅ_１，Ｆ_１，Ｇ_１，Ｈ_１のｘ座標ｘ_ａ１，ｘ_ｂ１，ｘ_ｃ１，ｘ_ｄ１，ｘ_ｅ１，ｘ_ｆ１，ｘ_ｇ１，ｘ_ｈ１とｙ座標ｙ_ａ１，ｙ_ｂ１，ｙ_ｃ１，ｙ_ｄ１，ｙ_ｅ１，ｙ_ｆ１，ｙ_ｇ１，ｙ_ｈ１は、

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

図８（ａ）及び図８（ｂ）に示すように、直線ｕ_ｆ２ ^１のＹ切片が直線ｕ_ｆ１ ^１のＹ切片と比べて十分に小さいとき、点Ａ_１が点Ｂ_１に対してＹ軸の正側に存在するため、座標ｙ_ａ１，ｙ_ｂ１は、

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

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

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

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

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

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

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

となる。

さらに、図９（ａ）及び図９（ｂ）に示すように、点Ｂ_１が点Ａ_１に対してＹ軸の正側に存在するように直線ｕ_ｆ２ ^１のＹ切片を移動すると、座標ｙ_ａ１，ｙ_ｂ１は、

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

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

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

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

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

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

となる。

なお、本第１実施形態では、式（３５）、式（３８）、式（４２）、式（４５）を用いて評価関数Ｊの最小値Ｊ_ｍｉｎ ^１を演算する場合を、それぞれＣａｓｅ_１１、Ｃａｓｅ_１２、Ｃａｓｅ_１３、Ｃａｓｅ_１４と定義する。ただし、Ｃａｓｅに続く番号は、第１〜４演算処理のうち、対応する演算処理の番号を表す。

図１０は、収縮力指令値演算部による第１演算処理のフローチャートである。図１０に示すように、収縮力指令値演算部１５２は、第１演算処理として、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（１８）に示すように、ｕ_ｅ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とする（Ｓ３１）。

次に、収縮力指令値演算部１５２は、式（２４）及び式（２５）を演算する（Ｓ３２）。次に、収縮力指令値演算部１５２は、ｙ_ａ１≧ｙ_ｂ１であるか否かを判断する（Ｓ３３）。収縮力指令値演算部１５２は、ｙ_ａ１≧ｙ_ｂ１である場合（Ｓ３３：Ｙｅｓ）、式（２６）を演算する（Ｓ３４）。次に、収縮力指令値演算部１５２は、ｘ_ａ１≧ｘ_ｃ１であるか否かを判断する（Ｓ３５）。収縮力指令値演算部１５２は、ｘ_ａ１≧ｘ_ｃ１である場合（Ｓ３５：Ｙｅｓ）、式（２７）を演算し（Ｓ３６）、式（３５）及び式（３６）を導き出す（Ｓ３７：Ｃａｓｅ_１１）。

また、収縮力指令値演算部１５２は、ｘ_ａ１≧ｘ_ｃ１ではない場合（Ｓ３５：Ｎｏ）、式（３８）及び式（３９）を導き出す（Ｓ３８：Ｃａｓｅ_１２）。

また、収縮力指令値演算部１５２は、ｙ_ａ１≧ｙ_ｂ１ではない場合（Ｓ３３：Ｎｏ）、式（２８）、式（２９）及び式（３０）を演算する（Ｓ３９）。次に、収縮力指令値演算部１５２は、ｘ_ｅ１＜ｘ_ｇ１であるか否かを判断する（Ｓ４０）。次に、収縮力指令値演算部１５２は、ｘ_ｅ１＜ｘ_ｇ１である場合（Ｓ４０：Ｙｅｓ）、式（４２）及び式（４３）を導き出す（Ｓ４１：Ｃａｓｅ_１３）。

また、収縮力指令値演算部１５２は、ｘ_ｅ１＜ｘ_ｇ１ではない場合（Ｓ４０：Ｎｏ）、式（３０）及び式（３１）を演算し（Ｓ４２）、式（４５）及び式（４６）を導き出す（Ｓ４３：Ｃａｓｅ_１４）。

このように、収縮力指令値演算部１５２は、式（３２）、式（３３）及び式（４１）に示す場合分けを行う。そして、それぞれの場合について、評価関数Ｊの最小値Ｊ_ｍｉｎ ^１と第１指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第１指令値候補を演算する第１演算処理を実行する。

なお、本実施形態では式（２１）で定義する変数ｕ_ｆ３ ^１’を用いて、評価関数Ｊを最小化する指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１を導出するが、評価関数Ｊの最小値と指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１は変数の選び方に依存しない。なぜなら、評価関数Ｊの最小値は指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１によって決定されるが、指令値候補ｕ_ｆ１ ^１，ｕ_ｆ２ ^１，ｕ_ｆ３ ^１は変数の選び方によらず式（１９）と式（２０）を満たすように増減するためである。このことは、以下に述べる第２〜４演算処理においても同様に成り立つ。

（２．２．２ 第２演算処理の設計）
第２演算処理における収縮力指令値の第２指令値候補ｕ_ｅ１ ^２，ｕ_ｆ２ ^２，ｕ_ｅ３ ^２は、図４の条件２と式（１６）より、

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

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

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

図１１は、収縮力指令値演算部による第２演算処理のフローチャートである。図１１に示すように、収縮力指令値演算部１５２は、第２演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（４７）に示すように、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とする（Ｓ５１）。

次に、収縮力指令値演算部１５２は、ｙ_ａ２，ｙ_ｂ２を演算し（Ｓ５２）、ｙ_ｂ２≦ｙ_ａ２であるか否かを判断する（Ｓ５３）。収縮力指令値演算部１５２は、ｙ_ｂ２≦ｙ_ａ２である場合（Ｓ５３：Ｙｅｓ）、ｘ_ｃ２，ｘ_ａ２を演算し（Ｓ５４）、ｘ_ｃ２＜ｘ_ａ２であるか否かを判断する（Ｓ５５）。収縮力指令値演算部１５２は、ｘ_ｃ２＜ｘ_ａ２である場合（Ｓ５５：Ｙｅｓ）、ｕ_ｆ１ ^２＝ｙ_ａ２，ｕ_ｅ２ ^２＝ｙ_ｂ２，ｕ_ｆ３ ^２＝ｙ_ａ２，Ｊ_ｍｉｎ ^２＝ｙ_ａ２を導き出す（Ｓ５６：Ｃａｓｅ_２１）。

また、収縮力指令値演算部１５２は、ｘ_ｃ２＜ｘ_ａ２ではない場合（Ｓ５５：Ｎｏ）、ｘ_ｃ２≦ｘ_ｄ２であるか否かを判断する（Ｓ５７）。次に、収縮力指令値演算部１５２は、ｘ_ｃ２≦ｘ_ｄ２である場合（Ｓ５７：Ｙｅｓ）、ｙ_ｅ２，ｙ_ｆ２を演算し（Ｓ５８）、ｕ_ｆ１ ^２＝ｙ_ｆ２，ｕ_ｅ２ ^２＝０，ｕ_ｆ３ ^２＝ｙ_ｅ２，Ｊ_ｍｉｎ ^２＝ｙ_ｅ２を導き出す（Ｓ５９：Ｃａｓｅ_２２）。

また、収縮力指令値演算部１５２は、ｘ_ｃ２≦ｘ_ｄ２ではない場合（Ｓ５７：Ｎｏ）、ｕ_ｆ１ ^２＝ｕ_ｅ２ ^２＝ｕ_ｆ３ ^２＝０，Ｊ_ｍｉｎ ^２＝０を導き出す（Ｓ６０：Ｃａｓｅ_２３）。

また、収縮力指令値演算部１５２は、ｙ_ｂ２≦ｙ_ａ２ではない場合（Ｓ５３：Ｎｏ）、ｙ_ｇ２，ｙ_ｈ２を演算し（Ｓ６１）、ｕ_ｆ１ ^２＝０，ｕ_ｅ２ ^２＝ｙ_ｈ２，ｕ_ｆ３ ^２＝ｙ_ｇ２，Ｊ_ｍｉｎ ^２＝ｙ_ｇ２を導き出す（Ｓ６２：Ｃａｓｅ_２４）。

なお、Ｃａｓｅ_２３では、第２指令値候補ｕ_ｆ１ ^２，ｕ_ｅ２ ^２，ｕ_ｆ３ ^２を全て正数とすることができない。そこで、後述する筋指令選択部において候補から外すために、第２指令値候補ｕ_ｆｉ ^２，ｕ_ｅｉ ^２と最小値Ｊ_ｍｉｎ ^２を０とおく。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^２と第２指令値候補ｕ_ｆ１ ^２，ｕ_ｅ２ ^２，ｕ_ｆ３ ^２を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第２指令値候補を演算する第２演算処理を実行する。

（２．２．３ 第３演算処理の設計）
第３演算処理における収縮力指令値の第３指令値候補ｕ_ｆ１ ^３，ｕ_ｅ２ ^３，ｕ_ｅ３ ^３は、図４の条件３と式（１６）より、

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

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

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

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

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

図１２は、収縮力指令値演算部による第３演算処理のフローチャートである。図１２に示すように、本第１実施形態の第３演算処理は、式（５５）〜式（５７）に示す変数変換と第２演算処理を用いて場合分けを行う。

即ち、収縮力指令値演算部１５２は、第３演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（５１）に示すように、ｕ_ｆ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とする（Ｓ７１）。次に、収縮力指令値演算部１５２は、式（５６）及び式（５７）のように定義し（Ｓ７２）、第２演算処理と同様の演算を行う（Ｓ７３）。その際、収縮力指令値演算部１５２は、式（５５）のように定義する（Ｓ７４）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^３と第３指令値候補ｕ_ｅ１ ^３，ｕ_ｆ２ ^３，ｕ_ｆ３ ^３を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第３指令値候補を演算する第３演算処理を実行する。

（２．２．４ 第４演算処理の設計）
第４演算処理における収縮力指令値の第４指令値候補ｕ_ｆ１ ^４，ｕ_ｆ２ ^４，ｕ_ｅ３ ^４は、図４の条件４と式（１６）より、

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

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

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

図１３は、収縮力指令値演算部による第４演算処理のフローチャートである。収縮力指令値演算部１５２は、第４演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（６０）に示すように、ｕ_ｆ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とする（Ｓ８１）。

次に、収縮力指令値演算部１５２は、ｘ_ａ４，ｘ_ｂ４を演算する（Ｓ８２）。次に、収縮力指令値演算部１５２は、ｘ_ｂ４＜ｘ_ａ４であるか否かを判断する（Ｓ８３）。収縮力指令値演算部１５２は、ｘ_ｂ４＜ｘ_ａ４であると判断した場合は（Ｓ８３：Ｙｅｓ）、ｙ_ｃ４，ｙ_ｄ４を演算する（Ｓ８４）。そして、収縮力指令値演算部１５２は、ｕ_ｅ１ ^４＝０，ｕ_ｅ２ ^４＝ｙ_ｅ４，ｕ_ｆ３ ^４＝ｙ_ｃ４を導き出す（Ｓ８５）。

次に、収縮力指令値演算部１５２は、ｙ_ｄ４＜ｙ_ｃ４であるか否かを判断する（Ｓ８６）。そして、収縮力指令値演算部１５２は、ｙ_ｄ４＜ｙ_ｃ４であると判断した場合は（Ｓ８６：Ｙｅｓ）、Ｊ_ｍｉｎ ^４＝ｙ_ｃ４とする（Ｓ８７：Ｃａｓｅ_４１）。また、収縮力指令値演算部１５２は、ｙ_ｄ４＜ｙ_ｃ４ではないと判断した場合は（Ｓ８６：Ｎｏ）、Ｊ_ｍｉｎ ^４＝ｙ_ｄ４とする（Ｓ８８：Ｃａｓｅ_４２）。

また、収縮力指令値演算部１５２は、ｘ_ｂ４＜ｘ_ａ４ではないと判断した場合は（Ｓ８３：Ｎｏ）、ｙ_ｅ４，ｙ_ｆ４を演算する（Ｓ８９）。そして、収縮力指令値演算部１５２は、ｕ_ｅ１ ^４＝ｙ_ｅ４，ｕ_ｅ２ ^４＝０，ｕ_ｆ３ ^４＝ｙ_ｆ４を導き出す（Ｓ９０）。

次に、収縮力指令値演算部１５２は、ｙ_ｅ４＜ｙ_ｆ４であるか否かを判断する（Ｓ９１）。そして、収縮力指令値演算部１５２は、ｙ_ｅ４＜ｙ_ｆ４であると判断した場合は（Ｓ９１：Ｙｅｓ）、Ｊ_ｍｉｎ ^４＝ｙ_ｆ４とする（Ｓ９２：Ｃａｓｅ_４３）。また、収縮力指令値演算部１５２は、ｙ_ｅ４＜ｙ_ｆ４ではないと判断した場合は（Ｓ９１：Ｎｏ）、Ｊ_ｍｉｎ ^４＝ｙ_ｅ４とする（Ｓ９３：Ｃａｓｅ_４４）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^４と収縮力指令値の第４指令値候補ｕ_ｅ１ ^４，ｕ_ｅ２ ^４，ｕ_ｆ３ ^４を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第４指令値候補を演算する第４演算処理を実行する。

（２．３ 第１指令選択処理）
収縮力指令値演算部１５２は、第１指令選択処理では、評価関数の最小値Ｊ_ｍｉｎの候補Ｊ_ｍｉｎ ^１，Ｊ_ｍｉｎ ^２，Ｊ_ｍｉｎ ^３，Ｊ_ｍｉｎ ^４のうち、最小となるものを選び出す。そして、収縮力指令値演算部１５２は、そのときの指令値候補を、収縮力指令値ｕ_ｆｉ，ｕ_ｅｉとして出力する。

図１４は、収縮力指令値演算部による第１指令選択処理のフローチャートである。まず、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎと収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを、第１演算処理で演算した最小値の候補Ｊ_ｍｉｎ ^１と収縮力指令値の第１指令値候補ｕ_ｆｉ ^１，ｕ_ｅｉ ^１とする（Ｓ１０１）。次に、収縮力指令値演算部１５２は、Ｊ_ｍｉｎ ^２＞０であるか否かを判断し（Ｓ１０２）、Ｊ_ｍｉｎ ^２＞０である場合（Ｓ１０２：Ｙｅｓ）、以下の式（６４）、即ちＪ_ｍｉｎ＞Ｊ_ｍｉｎ ^２であるか否かを判断する（Ｓ１０３）。

収縮力指令値演算部１５２は、Ｊ_ｍｉｎ＞Ｊ_ｍｉｎ ^２であると判断した場合（Ｓ１０３：Ｙｅｓ）、Ｊ_ｍｉｎをＪ_ｍｉｎ ^２とし、ｕ_ｆｉ，ｕ_ｅｉをｕ_ｆｉ ^２，ｕ_ｅｉ ^２とする（Ｓ１０４）。

次に、収縮力指令値演算部１５２は、Ｊ_ｍｉｎ ^３＞０であるか否かを判断し（Ｓ１０５）、Ｊ_ｍｉｎ ^３＞０である場合（Ｓ１０５：Ｙｅｓ）、Ｊ_ｍｉｎ＞Ｊ_ｍｉｎ ^３であるか否かを判断する（Ｓ１０６）。収縮力指令値演算部１５２は、Ｊ_ｍｉｎ＞Ｊ_ｍｉｎ ^３であると判断した場合（Ｓ１０６：Ｙｅｓ）、Ｊ_ｍｉｎをＪ_ｍｉｎ ^３とし、ｕ_ｆｉ，ｕ_ｅｉをｕ_ｆｉ ^３，ｕ_ｅｉ ^３とする（Ｓ１０７）。

次に、収縮力指令値演算部１５２は、Ｊ_ｍｉｎ＞Ｊ_ｍｉｎ ^４であるか否かを判断する（Ｓ１０８）。収縮力指令値演算部１５２は、Ｊ_ｍｉｎ＞Ｊ_ｍｉｎ ^４であると判断した場合（Ｓ１０８：Ｙｅｓ）、Ｊ_ｍｉｎをＪ_ｍｉｎ ^４とし、ｕ_ｆｉ，ｕ_ｅｉをｕ_ｆｉ ^４，ｕ_ｅｉ ^４とする（Ｓ１０９）。

ここで、２．２節で述べたように、第２，第３演算処理において、収縮力を全て正数とすることができないときは、最小値の候補Ｊ_ｍｉｎ ^２，Ｊ_ｍｉｎ ^３を０とした。そこで、収縮力指令値演算部１５２は、第１筋指令選択処理では、候補Ｊ_ｍｉｎ ^２，Ｊ_ｍｉｎ ^３が０より大きい場合にのみ、最小値Ｊ_ｍｉｎと収縮力指令値ｕ_ｆｉ，ｕ_ｅｉの更新を行う。

以上、収縮力指令値演算部１５２は、第１〜第４指令値候補のうち、評価関数が最小の指令値候補を、指令値出力処理にて出力する第１〜第６収縮力指令値として選択する指令選択処理を実行する。

本第１実施形態によれば、収縮力指令値である制御入力を最適化する場合分けアルゴリズムにより、手先柔軟性の目標軌道が一意に決定されるため、最小の制御入力で人工筋肉アクチュエータｆ_ｉ，ｅ_ｉを拮抗駆動することが可能となる。制御入力を最小にすることで、例えばＭｃＫｉｂｂｅｎ型人工筋肉では消費する空気の量を最小にすることができる。これにより、タンク等の搭載型の空気源を用いるロボット１００では稼働時間を延長することが可能となる。

［第２実施形態］
次に、本発明の第２実施形態のロボットについて説明する。本第２実施形態では、上記第１実施形態の制御方法にさらに分岐条件を加えることによって、各関節トルク指令値が正数と負数の両方を取るときにも制御入力である収縮力指令値の導出が可能であることを示す。なお、本第２実施形態において、装置構成は、上記第１実施形態における図２及び図３と同様であり、制御装置の制御動作が異なるものであるため、装置構成についての詳細な説明は省略し、制御装置の制御動作について詳細に説明する。

本第２実施形態では、関節トルク指令値Ｔ_１，Ｔ_２が負数となる場合を考慮する収縮力の制御方法を導出する。これにより、任意の関節角度の目標軌道において、収縮力の最大値を最小化することが可能となる。

（１モデル導出）
本第２実施形態では第１実施形態と同じモデルを用いる。

（２ 制御系設計）
本第２実施形態において、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２の符号に基づいて、筋トルクτ_１，τ_２，τ_３の符号の組み合わせを仮定する点が上記第１実施形態と異なる。

（２．１ 関節トルク指令値の符号に基づく場合分け）
関節トルク指令値が負数となる場合は、図４に示す筋トルクτ_１，τ_２，τ_３の符号の組み合わせのうち、筋トルクτ_３が負数となる場合を考慮する必要がある。なぜなら、関節トルク指令値Ｔ_１が負数のとき、筋トルクτ_３を負数とすれば第１単関節筋の収縮力を低減することが可能となるためである。

このように、関節トルク指令値Ｔ_１，Ｔ_２のどちらかが負数となるときと、どちらも正数となるときでは、考慮すべき筋トルクτ_１，τ_２，τ_３の符号の組み合わせが異なる。そこで、関節トルク指令値Ｔ_１，Ｔ_２の符号に基づいて、考慮すべき筋トルクτ_１，τ_２，τ_３の符号の組み合わせの場合分けを行う。

まず、関節トルク指令値Ｔ_１が正数となりＴ_２が負数となるときを考える。図４において、筋トルクτ_１，τ_３をどちらも負数とする条件７及び条件８の組み合わせを仮定すると、式（９）より、関節トルク指令値Ｔ_１は負数となる。また、筋トルクτ_２，τ_３をどちらも正数とする条件１と条件３の組み合わせを仮定すると、関節トルク指令値Ｔ_２は正数となる。これらのことから、関節トルク指令値Ｔ_１が正数となり関節トルク指令値Ｔ_２が負数となるときは、条件２、条件４、条件５、条件６の組み合わせを仮定して、評価関数Ｊを最小化するｕ_ｆｉ，ｕ_ｅｉを導出すればよいことが分かる。

なお、本第２実施形態では、関節トルク指令値Ｔ_１が正数となり関節トルク指令値Ｔ_２が負数となるときは、関節トルク指令値Ｔ_１が負数となり関節トルク指令値Ｔ_２が正数となるときの演算方法を用いて収縮力指令値を演算する。

また、関節トルク指令値Ｔ_１，Ｔ_２がどちらも負数となるときは、関節トルク指令値Ｔ_１，Ｔ_２がどちらも正数となる場合の演算方法を用いて収縮力を演算する。さらに、本発明では関節トルク指令値Ｔ_１，Ｔ_２が０となる場合は、正数である場合と同一の処理を行う。

収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２の符号に基づく場合分けを行い、筋トルクτ_１，τ_２，τ_３の符号の候補を選択する。そして、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２のうち、一方が正数となり他方が負数となる場合、即ち、関節トルク指令値Ｔ_１と関節トルク指令値Ｔ_２とが異符号の場合には、第２最小化演算処理と第２指令選択処理を実行する。これにより収縮力指令値を演算する。

また、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２がどちらも正数又は負数となる場合、即ち、関節トルク指令値Ｔ_１と関節トルク指令値Ｔ_２とが同符号の場合には、上記第１実施形態と同様に、第１最小化演算処理と第１指令選択処理を実行する。これにより収縮力指令値を演算する。

図１５は、本発明の第２実施形態に係るロボットの収縮力指令値演算部の処理動作を示すフローチャートである。まず、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１，Ｔ_２をフィードフォワード補償部１５１から入力すると共に、不図示の上位コントローラから目標関節角度θ_１，θ_２、目標関節角速度ω_１，ω_２を入力する（Ｓ１１０）。なお、弾性力定数ｋ、粘性力定数ｃ及びモーメントアーム径ｒは、予め設定された値である。

次に、収縮力指令値演算部１５２は、式（１２）を用いて、目標関節角度θ_１，θ_２、目標関節角速度ω_１，ω_２から係数Ｋ_ｆｉ，Ｋ_ｅｉを算出する（Ｓ１１１）。次に、収縮力指令値演算部１５２は、Ｔ_１≧０であるか否かを判断する（Ｓ１１２）。収縮力指令値演算部１５２は、Ｔ_１≧０であると判断した場合（Ｓ１１２：Ｙｅｓ）、Ｔ_２≧０であるか否かを判断する（Ｓ１１３）。

収縮力指令値演算部１５２は、Ｔ_２≧０であると判断した場合（Ｓ１１３：Ｙｅｓ）、式（１０）に示す第１関数、及び式（１１）に示す第２関数に、係数Ｋ_ｆｉ，Ｋ_ｅｉ及び関節トルク指令値Ｔ_１，Ｔ_２を定数として代入する（Ｓ１１４）。即ち、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１と関節トルク指令値Ｔ_２とが同符号の場合、ステップＳ１１４の処理を実行する。

次に、収縮力指令値演算部１５２は、上記第１実施形態で説明した第１最小化演算処理を実行する（Ｓ１１５）。次に、収縮力指令値演算部１５２は、評価関数Ｊ_ｍｉｎ ^１〜^４及び指令値候補ｕ_ｆｉ ^１〜４，ｕ_ｅｉ ^１〜４に対し（Ｓ１１６）、評価関数が最小の指令値候補を選択する、上記第１実施形態と同様の第１指令選択処理を実行する（Ｓ１１７）。次に、収縮力指令値演算部１５２は、演算した第１〜第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを収縮力発生部１５３に出力する指令値出力処理を実行する（Ｓ１１８：指令値出力処理工程）。

また、収縮力指令値演算部１５２は、Ｔ_２≧０ではないと判断した場合（Ｓ１１３：Ｎｏ）、式（１０）に示す第１関数、及び式（１１）に示す第２関数に、係数Ｋ_ｆｉ，Ｋ_ｅｉ及び関節トルク指令値Ｔ_１，Ｔ_２を定数として代入する（Ｓ１１９）。即ち、収縮力指令値演算部１５２は、関節トルク指令値Ｔ_１と関節トルク指令値Ｔ_２とが同符号の場合、ステップＳ１１９の処理を実行する。

次に、収縮力指令値演算部１５２は、後述する、評価関数Ｊを最小化する指令値候補を演算する第２最小化演算処理を実行する（Ｓ１２０）。次に、収縮力指令値演算部１５２は、評価関数Ｊ_ｍｉｎ ^５〜８及び指令値候補ｕ_ｆｉ ^５〜８，ｕ_ｅｉ ^５〜８に対し（Ｓ１２１）、評価関数が最小の指令値候補を選択する、後述する第２指令選択処理を実行する（Ｓ１２２）。次に、収縮力指令値演算部１５２は、演算した第１〜第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを収縮力発生部１５３に出力する指令値出力処理を実行する（Ｓ１１８：指令値出力処理工程）。

また、収縮力指令値演算部１５２は、Ｔ_１≧０ではないと判断した場合（Ｓ１１２：Ｎｏ）、Ｔ_２≧０であるか否かを判断する（Ｓ１２３）。次に、収縮力指令値演算部１５２は、Ｔ_２≧０であると判断した場合（Ｓ１２３：Ｙｅｓ）、Ｋ_ｆ１’＝Ｋ_ｅ２，Ｋ_ｅ１’＝Ｋ_ｆ２，Ｋ_ｆ２’＝Ｋ_ｅ１，Ｋ_ｅ２’＝Ｋ_ｆ１，Ｔ_１’＝Ｔ_２，Ｔ_２’＝Ｔ_１を定義する（Ｓ１２４）。そして、収縮力指令値演算部１５２は、Ｋ_ｆｉ’，Ｋ_ｅｉ’，Ｔ_１’，Ｔ_２’を、式（１０）に示す第１関数、及び式（１１）に示す第２関数に代入する（Ｓ１２５）。

次に、収縮力指令値演算部１５２は、後述する、評価関数Ｊを最小化する指令値候補を演算する第２最小化演算処理を実行する（Ｓ１２６）。次に、収縮力指令値演算部１５２は、ｕ_ｆ１＝ｕ_ｆ２’，ｕ_ｅ１＝ｕ_ｅ２’，ｕ_ｆ２＝ｕ_ｆ１’，ｕ_ｅ２’＝ｕ_ｆ１とする（Ｓ１２７）。次に、収縮力指令値演算部１５２は、評価関数Ｊ_ｍｉｎ ^５〜^８及び指令値候補ｕ_ｆｉ ^５〜８，ｕ_ｅｉ ^５〜８に対し（Ｓ１２８）、評価関数が最小の指令値候補を選択する、後述する第２指令選択処理を実行する（Ｓ１２９）。次に、収縮力指令値演算部１５２は、演算した第１〜第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを収縮力発生部１５３に出力する指令値出力処理を実行する（Ｓ１１８：指令値出力処理工程）。

また、収縮力指令値演算部１５２は、Ｔ_２≧０ではないと判断した場合（Ｓ１２３：Ｎｏ）、Ｋ_ｆｉ’＝Ｋ_ｅｉ，Ｋ_ｅｉ’＝Ｋ_ｆｉ，Ｔ_１’＝−Ｔ_１，Ｔ_２’＝−Ｔ_２を定義する（Ｓ１３０）。そして、収縮力指令値演算部１５２は、Ｋ_ｆｉ’，Ｋ_ｅｉ’，Ｔ_１’，Ｔ_２’を、式（１０）に示す第１関数、及び式（１１）に示す第２関数に代入する（Ｓ１３１）。

次に、収縮力指令値演算部１５２は、上記第１実施形態で説明した第１最小化演算処理を実行する（Ｓ１３２）。次に、収縮力指令値演算部１５２は、ｕ_ｆｉ’＝ｕ_ｅｉ，ｕ_ｅｉ’＝ｕ_ｆｉとする（Ｓ１３３）。次に、収縮力指令値演算部１５２は、評価関数Ｊ_ｍｉｎ ^１〜^４及び指令値候補ｕ_ｆｉ ^１〜４，ｕ_ｅｉ ^１〜４に対し（Ｓ１３４）、評価関数が最小の指令値候補を選択する、上記第１実施形態と同様の第１指令選択処理を実行する（Ｓ１３５）。

即ち、収縮力指令値演算部１５２は、これらＳ１１１〜Ｓ１３５において、第１から第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを演算する指令値演算処理を実行する（指令値演算処理工程）。

次に、収縮力指令値演算部１５２は、演算した第１〜第６収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを収縮力発生部１５３に出力する指令値出力処理を実行する（Ｓ１１８：指令値出力処理工程）。

なお、第２最小化計算処理と第２指令値選択処理とは、上記第１実施形態の第１最小化計算処理と第１指令値選択処理と同様であるため、詳細な演算方法を省略する。

（２．２ 第２最小化演算処理における場合分け）
図１６は、収縮力指令値演算部による第２最小化演算処理のフローチャートである。収縮力指令値演算部１５２は、図４に示す第２条件を仮定し（Ｓ１４１）、第５演算処理を実行し（Ｓ１４２）、評価関数Ｊ_ｍｉｎ ^５を最小化する第１〜第６収縮力指令値からなる第５指令値候補ｕ_ｆｉ ^５，ｕ_ｅｉ ^５を出力する（Ｓ１４３）。

続いて、収縮力指令値演算部１５２は、図４に示す第４条件を仮定し（Ｓ１４４）、第６演算処理を実行し（Ｓ１４５）、評価関数Ｊ_ｍｉｎ ^６を最小化する第１〜第６収縮力指令値からなる第６指令値候補ｕ_ｆｉ ^６，ｕ_ｅｉ ^６を出力する（Ｓ１４６）。

続いて、収縮力指令値演算部１５２は、図４に示す第５条件を仮定し（Ｓ１４７）、第７演算処理を実行し（Ｓ１４８）、評価関数Ｊ_ｍｉｎ ^７を最小化する第１〜第６収縮力指令値からなる第７指令値候補ｕ_ｆｉ ^７，ｕ_ｅｉ ^７を出力する（Ｓ１４９）。

続いて、収縮力指令値演算部１５２は、図４に示す第６条件を仮定し（Ｓ１５０）、第８演算処理を実行し（Ｓ１５１）、評価関数Ｊ_ｍｉｎ ^８を最小化する第１〜第６収縮力指令値からなる第８指令値候補ｕ_ｆｉ ^８，ｕ_ｅｉ ^８を出力する（Ｓ１５２）。

第５、第６、第７、第８演算処理は、それぞれ、筋トルクτ_１，τ_２の符号を図４の条件２、４、５、６と仮定して、評価関数Ｊの最小値の候補Ｊ_ｍｉｎ ^ｊと収縮力指令値の指令値候補ｕ_ｆｉ ^ｊ、ｕ_ｅｉ ^ｊを演算するサブルーチンである（ｊ＝５，６，７，８）。

（２．２．１ 第５演算処理の設計）
第５演算処理における収縮力指令値の第５指令値候補ｕ_ｆｉ ^５，ｕ_ｅｉ ^５のうち、第５指令値候補ｕ_ｅ１ ^５，ｕ_ｆ２ ^５，ｕ_ｅ３ ^５は、図４の条件２と式（１６）より、

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

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

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

図１７は、収縮力指令値演算部による第５演算処理のフローチャートである。図１７に示すように、収縮力指令値演算部１５２は、第５演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（６５）に示すように、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とする（Ｓ１６１）。

次に、収縮力指令値演算部１５２は、ｙ_ａ５，ｙ_ｂ５を演算し（Ｓ１６２）、ｘ_ｃ５，ｙ_ｃ５を演算する（Ｓ１６３）。次に、収縮力指令値演算部１５２は、ｙ_ｂ５＜ｙ_ａ５であるか否かを判断する（Ｓ１６４）。

収縮力指令値演算部１５２は、ｙ_ｂ５＜ｙ_ａ５である場合（Ｓ１６４：Ｙｅｓ）、ｙ_ｃ５＜ｙ_ｄ５であるか否かを判断する（Ｓ１６５）。収縮力指令値演算部１５２は、ｙ_ｃ５＜ｙ_ｄ５である場合（Ｓ１６５：Ｙｅｓ）、ｙ_ｅ５，ｙ_ｆ５を演算し（１６６）、ｕ_ｆ１ ^５＝ｙ_ｅ５，ｕ_ｅ２ ^５＝ｙ_ｆ５，ｕ_ｆ３ ^５＝ｙ_ｅ５，Ｊ_ｍｉｎ ^５＝ｙ_ｅ５を導き出す（Ｓ１６７：Ｃａｓｅ_５１）。

また、収縮力指令値演算部１５２は、ｙ_ｃ５＜ｙ_ｄ５ではない場合（Ｓ１６５：Ｎｏ）、ｕ_ｆ１ ^５＝ｙ_ｃ５，ｕ_ｅ２ ^５＝ｙ_ｃ５，ｕ_ｆ３ ^５＝ｙ_ｄ５，Ｊ_ｍｉｎ ^５＝ｙ_ｄ５を導き出す（Ｓ１６８：Ｃａｓｅ_５２）。

また、収縮力指令値演算部１５２は、ｙ_ｂ５＜ｙ_ａ５ではない場合（Ｓ１６４：Ｎｏ）、ｕ_ｆ１ ^５＝ｙ_ａ５，ｕ_ｅ２ ^５＝ｙ_ｂ５，ｕ_ｆ３ ^５＝０，Ｊ_ｍｉｎ ^５＝ｙ_ｂ５を導き出す（Ｓ１６９：Ｃａｓｅ_５３）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^５と第５指令値候補ｕ_ｆ１ ^５，ｕ_ｅ２ ^５，ｕ_ｆ３ ^５を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第５指令値候補を演算する第５演算処理を実行する。

（２．２．２ 第６演算処理の設計）
第６演算処理部における収縮力指令値の第６指令値候補ｕ_ｆｉ ^６，ｕ_ｅｉ ^６のうち、第６指令値候補ｕ_ｆ１ ^６，ｕ_ｆ２ ^６，ｕ_ｅ３ ^６は、図４の条件４と式（１６）より、

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

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

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

図１８は、収縮力指令値演算部による第６演算処理のフローチャートである。図１８に示すように、収縮力指令値演算部１５２は、第６演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（６９）に示すように、ｕ_ｆ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とする（Ｓ１７１）。

次に、収縮力指令値演算部１５２は、ｘ_ａ６，ｙ_ｂ６を演算し（Ｓ１７２）、ｙ_ｃ６＝ｘ_ａ６とする（Ｓ１７３）。次に、収縮力指令値演算部１５２は、ｕ_ｅ１ ^６＝０，ｕ_ｅ２ ^６＝ｙ_ｂ６，ｕ_ｆ３ ^６＝ｙ_ｃ６とする（Ｓ１７４）。次に、収縮力指令値演算部１５２は、ｙ_ｂ６＜ｙ_ｃ６であるか否かを判断する（Ｓ１７５）。

収縮力指令値演算部１５２は、ｙ_ｂ６＜ｙ_ｃ６である場合（Ｓ１７５：Ｙｅｓ）、Ｊ_ｍｉｎ ^６＝ｙ_ｃ６を導き出し（Ｓ１７６：Ｃａｓｅ_６１）、ｙ_ｂ６＜ｙ_ｃ６ではない場合（Ｓ１７５：Ｎｏ）、Ｊ_ｍｉｎ ^６＝ｙ_ｂ６を導き出す（Ｓ１７７：Ｃａｓｅ_６２）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^６と第６指令値候補ｕ_ｅ１ ^６，ｕ_ｅ２ ^６，ｕ_ｆ３ ^６を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第５指令値候補を演算する第５演算処理を実行する。

（２．２．３ 第７演算処理の設計）
第７演算処理における収縮力指令値の第７指令値候補ｕ_ｆｉ ^７，ｕ_ｅｉ ^７のうち、第７指令値候補ｕ_ｅ１ ^７，ｕ_ｅ２ ^７，ｕ_ｆ３ ^７は、図４の条件５と式（１６）より、

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

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

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

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

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

図１９は、収縮力指令値演算部による第７演算処理のフローチャートである。図１９に示すように、本第２実施形態の第７演算処理は、式（７７）〜式（７９）に示す変数変換と第７演算処理を用いて場合分けを行う。

即ち、収縮力指令値演算部１５２は、第７演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（７３）に示すように、ｕ_ｅ１＝０，ｕ_ｅ２＝０，ｕ_ｆ３＝０とする（Ｓ１８１）。次に、収縮力指令値演算部１５２は、式（７８）及び式（７９）のように定義し（Ｓ１８２）、第６演算処理と同様の演算を行う（Ｓ１８３）。その際、収縮力指令値演算部１５２は、式（７７）のように定義する（Ｓ１８４）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^７と第７指令値候補ｕ_ｆ１ ^７，ｕ_ｆ２ ^７，ｕ_ｅ３ ^７を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第７指令値候補を演算する第７演算処理を実行する。

（２．２．４ 第８演算処理の設計）
第８演算処理における収縮力指令値の第８指令値候補ｕ_ｅ１ ^８，ｕ_ｆ２ ^８，ｕ_ｆ３ ^８は、図４の条件６と式（１６）より、

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

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

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

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

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

図２０に、収縮力指令値演算部による第８演算処理のフローチャートである。図２０に示すように、本第２実施形態の第８演算処理は、式（８６）〜式（８８）に示す変数変換と第７演算処理を用いて場合分けを行う。

即ち、収縮力指令値演算部１５２は、第８演算処理として、まず、式（１０）に示す第１関数及び式（１１）に示す第２関数において、式（８２）に示すように、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｆ３＝０とする（Ｓ１９１）。次に、収縮力指令値演算部１５２は、式（８７）及び式（８８）のように定義し（Ｓ１９２）、第５演算処理と同様の演算を行う（Ｓ１９３）。その際、収縮力指令値演算部１５２は、式（８６）のように定義する（Ｓ１９４）。

以上、収縮力指令値演算部１５２は、評価関数Ｊの最小値Ｊ_ｍｉｎ ^８と第８指令値候補ｕ_ｆ１ ^８，ｕ_ｅ２ ^８，ｕ_ｅ３ ^８を演算する。つまり、収縮力指令値演算部１５２は、評価関数を最小化する第１〜第６収縮力指令値からなる第８指令値候補を演算する第８演算処理を実行する。

（２．３ 関節トルク指令値Ｔ_１、Ｔ_２がどちらも負数となる場合の場合分け）
上記第１実施形態の式（１０）と式（１１）に対して、

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

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

（２．４ 関節トルク指令値Ｔ_１が負数となり関節トルク指令値Ｔ_２が正数となる場合の場合分け）
式（１０）と（１１）に対して、

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

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

以上の導出では、筋の弾性力がθ_ｉ＝０ｒａｄからの角度に比例して発生することを仮定していた。任意の角度を筋の弾性力発生の基準となる角度（以下、中立角度と呼ぶ）をθ_ｃｉ（ｉ＝１，２，３）としてフィードフォワード制御入力を生成するためには、係数Ｋ_ｆｉ，Ｋ_ｅｉ（ｉ＝１，２，３）を、式（１２）に代わり、

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

（３ シミュレーション）
本節では前節で設計した制御系を用いて、シミュレーションを行う。また、評価関数Ｊを最小化する収縮力は、前節で示した方法により導出する。本第２実施形態では、リンク１０１，１０２の慣性モーメントＩ_１，Ｉ_２を２．５×１０^−３ｋｇｍ^２、リンク１０１，１０２の長さＬ_１，Ｌ_２を０．２ｍ、第２リンク１０２の質量ｍ_２を１ｋｇとする。また、プーリ径ｒを５．０×１０^−２ｍ、筋の弾性力定数ｋを１０ｍ^−１、粘性力定数ｃを１０ｍ^−１ｓｅｃ^−１とする。さらに、目標関節角度θ_ｒ１，θ_ｒ２をそれぞれ、０ｓｅｃから０．３ｓｅｃの間に０．７８５ｒａｄから１．０４７ｒａｄに、１．５７１ｒａｄから２．０９４ｒａｄに増加するジャーク最小軌道とし、サンプリング周期Ｔ_ｓを１．０×１０^−４ｓｅｃとする。

図２１（ａ）及び図２１（ｂ）に、第１、第２リンクの関節角度θ_１，θ_２を実線で、目標軌道θ_ｒ１，θ_ｒ２を破線で表し、図２２に、関節トルク指令値Ｔ_１を実線で、関節トルク指令値Ｔ_２を破線で表す。また、図２３（ａ）、図２３（ｂ）及び図２３（ｃ）に、実線を用いて収縮力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３を、破線を用いて収縮力指令値ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３を、それぞれ表す。さらに、図２４に、係数Ｋ_ｆｉ，Ｋ_ｅｉと関節トルク指令値Ｔ_１，Ｔ_２の変化によって、第１〜８演算処理における収縮力の演算フローが切り替わる様子を表す。

図２１（ａ）及び図２１（ｂ）より、実線で示す関節角度は、破線で示す目標軌道と重なっており、関節角度は目標軌道に追従していることがわかる。

図２２より、０ｓｅｃから０．１１ｓｅｃの間は、関節トルク指令値Ｔ_１，Ｔ_２が正数となるため、収縮力指令値演算部１５２は第１最小化演算処理を実行して収縮力を最小化していることがわかる。また、０．１１ｓｅｃから０．３ｓｅｃの間は、関節トルク指令値Ｔ_１は負数となり関節トルク指令値Ｔ_２は正数となるため、収縮力指令値演算部１５２は第２最小化演算処理を実行して収縮力を最小化していることがわかる。

図２３（ａ）、図２３（ｂ）及び図２３（ｃ）より、実線で示す収縮力指令値ｕ_ｆｉと破線で示す収縮力指令値ｕ_ｅｉはどちらも必ず正数となることがわかる。また、収縮力指令値ｕ_ｆｉと収縮力指令値ｕ_ｅｉは一方が０となる場合には他方が０となっていることがわかる
これらのことから、第１最小化演算処理と第２最小化演算処理を実行して導出する収縮力は、どちらも式（１６）を満たし、評価関数Ｊを最小化していることがわかる。

また、図２３（ｂ）及び図２３（ｃ）より、０．１１ｓｅｃと０．１２ｓｅｃにおいて、収縮力指令値ｕ_ｆ２，ｕ_ｆ３の応答が急峻に変化していることが分かる。さらに、図２４より、収縮力指令値演算部１５２は０．１１ｓｅｃにおいてＣａｓｅ_１１からＣａｓｅ_１４へ、０．１２ｓｅｃにおいてＣａｓｅ_５１からＣａｓｅ_５２へ、それぞれ演算フローを切り替えていることがわかる。

これらのことから、演算フローを切り替えるとき、高周波成分を多く含む制御入力が発生してしまうことがわかる。制御入力に高周波成分が含まれると、ロボットの固有振動が励起されることが考えられる。しかし、この高周波成分は、収縮力の最小値は若干増加するものの、例えば零位相ローパスフィルタを用いることで低減することが可能である。

［第３実施形態］
次に、本発明の第３実施形態に係るロボットについて説明する。本第３実施形態では、制御入力は最小とならないが、関節剛性と関節角度の目標軌道に同時に追従するための制御方法を示す。

（１モデル導出）
本第３実施形態では第２実施形態と同じモデルを用いる。

（２制御系設計）
２リンクマニピュレータでは、手先が外界と直に接触するため、手先の剛性を制御することが重要である。手先の剛性は、図２５に示すようなスティフネス楕円Ｃによって表される。この楕円Ｃは、各方向に対しての剛性の分布を示し、手先と楕円Ｃの距離が離れるほど剛性が高いことを表している。

また、３対６筋構造を備える２リンクマニピュレータでは、手先の剛性と関節周りに働くトルクを独立に制御することが可能であることが知られている。収縮力指令値ｕ_ｆｉ，ｕ_ｅｉ（ｉ＝１，２，３）の差をＵ_ｄｉ（ｉ＝１，２，３）、和をＵ_ｓｉ（ｉ＝１，２，３）とする。

Ｕ_ｄｉとＵ_ｓｉを用いると、式（３）は、

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

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

しかし、上記第１実施形態と上記第２実施形態の制御系では、式（１６）に示す制御側によって、関節剛性は収縮力と等しくなってしまうため、駆動中の手先剛性を任意に指定することはできない。そこで、本第３実施形態では、駆動中に所望の手先剛性を実現するように、収縮力指令値演算部１５２が出力する収縮力に予備収縮力を加算する。そのため、本第３実施形態の制御系における収縮力は、剛性制御を行わない上記第１及び第２実施形態と比べて増加してしまう。予備収縮力をΔｕ_ｆｉ，Δｕ_ｅｉとすると、実際に２リンクマニピュレータに与える収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’は、

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

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

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

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

となる。

図２６は、本発明の第３実施形態に係るロボットの制御装置を示すブロック線図である。図２６において、制御装置１５０Ａは、トルク指令値生成部としてのフィードフォワード補償部１５１と、収縮力指令値演算部１５２と、収縮力発生部１５３と、を備えている。更に、本第３実施形態の制御装置１５０Ａは、剛性制御部１５４を有している。図２６において、剛性制御部１５４は、収縮力指令値演算部１５２が出力する収縮力指令値ｕ_ｆｉ，ｕ_ｅｉを入力として、式（１０９）と式（１１０）を用いて予備収縮力Δｕ_ｆｉ，Δｕ_ｅｉを演算する。

（３ シミュレーション）
前節の制御系を用いたシミュレーションを行う。第１リンク１０１と第２リンク１０２の物理パラメータと関節角度の目標軌道は上記第２実施形態と同一とする。また、関節剛性の目標軌道Ｕ_ｓｉ（ｉ＝１，２，３）を０ｓｅｃから０．３ｓｅｃの間に８Ｎから１２Ｎに増加するジャーク最小軌道とする。

図２７に関節剛性の目標軌道Ｕｓｉ（ｉ＝１，２，３）を表す。また、図２８（ａ）、図２８（ｂ）及び図２８（ｃ）に、収縮力指令値ｕ_ｆ１’，ｕ_ｆ２’，ｕ_ｆ３’を実線で、収縮力指令値ｕ_ｅ１’，ｕ_ｅ２’，ｕ_ｅ３’を破線で、収縮力の和を点線で、それぞれ表す。図２８（ａ）、図２８（ｂ）及び図２８（ｃ）より、収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’はどちらも０Ｎ以上となることがわかる。これは、収縮力指令値演算部１５２が出力する収縮力指令値ｕ_ｆｉ，ｕ_ｅｉに予備収縮力Δｕ_ｆｉ，Δｕ_ｅｉが加算されているためである。さらに、図２７の実線と図２８（ａ）、図２８（ｂ）及び図２８（ｃ）の点線とが一致していることから、収縮力指令値ｕ_ｆｉ’，ｕ_ｅｉ’の和が関節剛性の目標軌道Ｕ_ｓｉと一致していることがわかる。これらのことから、本第３実施形態の制御系により、手先の柔軟性を制御することが可能となることがわかる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。

１００…ロボット、１０１…第１リンク、１０２…第２リンク、１０３…プーリ（基台）、１１１…第１関節、１１２…第２関節、１５０…制御装置、１５１…フィードフォワード補償部（トルク指令値生成部）、１５２…収縮力指令値演算部、１５３…収縮力発生部、ｅ_１…第２人工筋肉アクチュエータ、ｅ_２…第４人工筋肉アクチュエータ、ｅ_３…第６人工筋肉アクチュエータ、ｆ_１…第１人工筋肉アクチュエータ、ｆ_２…第３人工筋肉アクチュエータ、ｆ_３…第５人工筋肉アクチュエータ

Claims (4)

1. 基台に第１関節で連結された第１リンクと、
   前記第１リンクに第２関節で連結された第２リンクと、
   収縮により前記第１リンクを前記第１関節まわりに第１旋回方向に旋回させる第１人工筋肉アクチュエータと、
   収縮により前記第１リンクを前記第１関節まわりに前記第１旋回方向に対して反対の第２旋回方向に旋回させる第２人工筋肉アクチュエータと、
   収縮により前記第２リンクを前記第２関節まわりに前記第１旋回方向に旋回させる第３人工筋肉アクチュエータと、
   収縮により前記第２リンクを前記第２関節まわりに前記第２旋回方向に旋回させる第４人工筋肉アクチュエータと、
   収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第１旋回方向に旋回させる第５人工筋肉アクチュエータと、
   収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第２旋回方向に旋回させる第６人工筋肉アクチュエータと、
   前記第１関節の第１関節トルク指令値及び前記第２関節の第２関節トルク指令値を生成するトルク指令値生成部と、
   前記第１関節トルク指令値及び前記第２関節トルク指令値に基づいて、前記第１人工筋肉アクチュエータの第１収縮力指令値、前記第２人工筋肉アクチュエータの第２収縮力指令値、前記第３人工筋肉アクチュエータの第３収縮力指令値、前記第４人工筋肉アクチュエータの第４収縮力指令値、前記第５人工筋肉アクチュエータの第５収縮力指令値、及び前記第６人工筋肉アクチュエータの第６収縮力指令値を求める収縮力指令値演算部と、
   前記第１収縮力指令値に応じて前記第１人工筋肉アクチュエータを収縮させ、前記第２収縮力指令値に応じて前記第２人工筋肉アクチュエータを収縮させ、前記第３収縮力指令値に応じて前記第３人工筋肉アクチュエータを収縮させ、前記第４収縮力指令値に応じて前記第４人工筋肉アクチュエータを収縮させ、前記第５収縮力指令値に応じて前記第５人工筋肉アクチュエータを収縮させ、前記第６収縮力指令値に応じて前記第６人工筋肉アクチュエータを収縮させる収縮力発生部と、を備え、
   前記収縮力指令値演算部は、
   前記第１関節トルク指令値を定数とし、前記第１、第２、第５及び第６収縮力指令値を変数として成り立つ第１関数、並びに前記第２関節トルク指令値を定数とし、前記第３、第４、第５及び第６収縮力指令値を変数として成り立つ第２関数において、前記第１及び第２収縮力指令値の一方、前記第３及び第４収縮力指令値の一方、並びに前記第５及び第６収縮力指令値の一方を０とした場合に、前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方のうち最大値となる収縮力指令値が最小となるように前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方を演算する指令値演算処理と、
   前記指令値演算処理で演算した前記第１から第６収縮力指令値を前記収縮力発生部に出力する指令値出力処理と、を実行することを特徴とするロボット。
2. 前記第１収縮力指令値をｕ_ｆ１、前記第２収縮力指令値をｕ_ｅ１、前記第３収縮力指令値をｕ_ｆ２、前記第４収縮力指令値をｕ_ｅ２、前記第５収縮力指令値をｕ_ｆ３、前記第６収縮力指令値をｕ_ｅ３、前記第１関節トルク指令値をＴ_１、前記第２関節トルク指令値をＴ_２とし、
   正の係数Ｋ_ｆ１，Ｋ_ｅ１，Ｋ_ｆ２，Ｋ_ｅ２，Ｋ_ｆ３，Ｋ_ｅ３を用いて、前記第１関数を、Ｔ_１＝Ｋ_ｆ１×ｕ_ｆ１−Ｋ_ｅ１×ｕ_ｅ１＋Ｋ_ｆ３×ｕ_ｆ３−Ｋ_ｅ３×ｕ_ｅ３とし、前記第２関数を、Ｔ_２＝Ｋ_ｆ２×ｕ_ｆ２−Ｋ_ｅ２×ｕ_ｅ２＋Ｋ_ｆ３×ｕ_ｆ３−Ｋ_ｅ３×ｕ_ｅ３とし、
   評価関数を、Ｊ＝ｍａｘ｛ｕ_ｆ１，ｕ_ｅ１，ｕ_ｆ２，ｕ_ｅ２，ｕ_ｆ３，ｕ_ｅ３｝としたとき、
   前記収縮力指令値演算部は、
   前記指令値演算処理として、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第１指令値候補を演算する第１演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第２指令値候補を演算する第２演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｆ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第３指令値候補を演算する第３演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｆ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第４指令値候補を演算する第４演算処理と、
   前記第１から第４指令値候補のうち、前記評価関数が最小の指令値候補を、前記指令値出力処理にて出力する前記第１から第６収縮力指令値として選択する指令選択処理と、を実行することを特徴とする請求項１に記載のロボット。
3. 前記第１収縮力指令値をｕ_ｆ１、前記第２収縮力指令値をｕ_ｅ１、前記第３収縮力指令値をｕ_ｆ２、前記第４収縮力指令値をｕ_ｅ２、前記第５収縮力指令値をｕ_ｆ３、前記第６収縮力指令値をｕ_ｅ３、前記第１関節トルク指令値をＴ_１、前記第２関節トルク指令値をＴ_２とし、
   正の係数Ｋ_ｆ１，Ｋ_ｅ１，Ｋ_ｆ２，Ｋ_ｅ２，Ｋ_ｆ３，Ｋ_ｅ３を用いて、前記第１関数を、Ｔ_１＝Ｋ_ｆ１×ｕ_ｆ１−Ｋ_ｅ１×ｕ_ｅ１＋Ｋ_ｆ３×ｕ_ｆ３−Ｋ_ｅ３×ｕ_ｅ３とし、前記第２関数を、Ｔ_２＝Ｋ_ｆ２×ｕ_ｆ２−Ｋ_ｅ２×ｕ_ｅ２＋Ｋ_ｆ３×ｕ_ｆ３−Ｋ_ｅ３×ｕ_ｅ３とし、
   評価関数を、Ｊ＝ｍａｘ｛ｕ_ｆ１，ｕ_ｅ１，ｕ_ｆ２，ｕ_ｅ２，ｕ_ｆ３，ｕ_ｅ３｝としたとき、
   前記収縮力指令値演算部は、
   前記指令値演算処理として、
   前記第１関節トルク指令値と前記第２関節トルク指令値とが同符号の場合には、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第１指令値候補を演算する第１演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第２指令値候補を演算する第２演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｆ１＝０，ｕ_ｅ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第３指令値候補を演算する第３演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｆ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第４指令値候補を演算する第４演算処理と、
   前記第１から第４指令値候補のうち、前記評価関数が最小の指令値候補を、前記指令値出力処理にて出力する前記第１から第６収縮力指令値として選択する第１指令選択処理と、を実行し、
   前記第１関節トルク指令値と前記第２関節トルク指令値とが異符号の場合には、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第５指令値候補を演算する第５演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｆ１＝０，ｕ_ｆ２＝０，ｕ_ｅ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第６指令値候補を演算する第６演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｅ２＝０，ｕ_ｆ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第７指令値候補を演算する第７演算処理と、
   前記第１関数及び前記第２関数において、ｕ_ｅ１＝０，ｕ_ｆ２＝０，ｕ_ｆ３＝０とした場合に、前記評価関数を最小化する前記第１から第６収縮力指令値からなる第８指令値候補を演算する第８演算処理と、
   前記第５から第８指令値候補のうち、前記評価関数が最小の指令値候補を、前記指令値出力処理にて出力する前記第１から第６収縮力指令値として選択する第２指令選択処理と、を実行することを特徴とする請求項１に記載のロボット。
4. 基台に第１関節で連結された第１リンクと、
   前記第１リンクに第２関節で連結された第２リンクと、
   収縮により前記第１リンクを前記第１関節まわりに第１旋回方向に旋回させる第１人工筋肉アクチュエータと、
   収縮により前記第１リンクを前記第１関節まわりに前記第１旋回方向に対して反対の第２旋回方向に旋回させる第２人工筋肉アクチュエータと、
   収縮により前記第２リンクを前記第２関節まわりに前記第１旋回方向に旋回させる第３人工筋肉アクチュエータと、
   収縮により前記第２リンクを前記第２関節まわりに前記第２旋回方向に旋回させる第４人工筋肉アクチュエータと、
   収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第１旋回方向に旋回させる第５人工筋肉アクチュエータと、
   収縮により前記第１及び第２リンクを同時に前記第１及び第２関節まわりに前記第２旋回方向に旋回させる第６人工筋肉アクチュエータと、
   前記第１関節の第１関節トルク指令値及び前記第２関節の第２関節トルク指令値を生成するトルク指令値生成部と、
   前記第１関節トルク指令値及び前記第２関節トルク指令値に基づいて、前記第１人工筋肉アクチュエータの第１収縮力指令値、前記第２人工筋肉アクチュエータの第２収縮力指令値、前記第３人工筋肉アクチュエータの第３収縮力指令値、前記第４人工筋肉アクチュエータの第４収縮力指令値、前記第５人工筋肉アクチュエータの第５収縮力指令値、及び前記第６人工筋肉アクチュエータの第６収縮力指令値を求める収縮力指令値演算部と、
   前記第１収縮力指令値に応じて前記第１人工筋肉アクチュエータを収縮させ、前記第２収縮力指令値に応じて前記第２人工筋肉アクチュエータを収縮させ、前記第３収縮力指令値に応じて前記第３人工筋肉アクチュエータを収縮させ、前記第４収縮力指令値に応じて前記第４人工筋肉アクチュエータを収縮させ、前記第５収縮力指令値に応じて前記第５人工筋肉アクチュエータを収縮させ、前記第６収縮力指令値に応じて前記第６人工筋肉アクチュエータを収縮させる収縮力発生部と、を備えたロボットを制御するロボット制御方法において、
   前記収縮力指令値演算部が、前記第１関節トルク指令値を定数とし、前記第１、第２、第５及び第６収縮力指令値を変数として成り立つ第１関数、並びに前記第２関節トルク指令値を定数とし、前記第３、第４、第５及び第６収縮力指令値を変数として成り立つ第２関数において、前記第１及び第２収縮力指令値の一方、前記第３及び第４収縮力指令値の一方、並びに前記第５及び第６収縮力指令値の一方を０とした場合に、前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方のうち最大値となる収縮力指令値が最小となるように前記第１及び第２収縮力指令値の他方、前記第３及び第４収縮力指令値の他方、並びに前記第５及び第６収縮力指令値の他方を演算する指令値演算処理工程と、
   前記収縮力指令値演算部が、前記指令値演算処理工程で演算した前記第１から第６収縮力指令値を前記収縮力発生部に出力する指令値出力処理工程と、を備えたことを特徴とするロボット制御方法。

Images