JP5930754B2 - ロボット装置の制御方法及びロボット装置 - Google Patents

Description

本発明は、手先が対象物に柔軟に接触できるロボット装置の制御方法及びロボット装置に関するものである。好適には、作業ロボットや脚式移動ロボットに適用される。

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

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

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

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

上記特許文献１では、フィードフォワード制御により関節角度と手先の柔軟性の同時制御を行っている。しかし、人工筋肉アクチュエータを最小の制御入力で拮抗駆動するための研究例はこれまで見られない。制御入力を最小化するには、関節角度の目標軌道に対応する手先柔軟性を考慮し、それを制御入力と同時に最適化する必要がある。しかし、上記特許文献１のフィードフォワード制御は、手先柔軟性の目標剛性が予め与えており、それを最適化するアルゴリズムは含まれていない。

そこで、本発明は、関節角度の軌道制御において、トルク指令値と同時に関節剛性指令値の最適化を行い、最小の駆動力指令値でリンクの拮抗駆動を行うことを目的とするものである。

本発明は、基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させるロボット装置の制御方法において、前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算工程と、前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算工程と、前記差分が所定値以下に収束するまで、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行う反復工程と、前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算工程と、を備えたことを特徴とする。

本発明によれば、関節の剛性と関節の軌道とを同時に最適化する反復演算アルゴリズムにより、目標軌道に対して最小の駆動力指令値でアクチュエータを拮抗駆動するフィードフォワード入力を生成することが可能となる。駆動力指令値を最小にすることで、例えばＭｃＫｉｂｂｅｎ型人工筋肉では消費する空気の量を最小にすることができる。これにより、タンク等の搭載型の空気源を用いるロボット装置では稼働時間を延長することが可能となる。

筋の粘弾性モデルを示す図である。 本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第１実施形態における制御部のブロック線図である。 本発明の第１実施形態における目標軌道を示す図である。 本発明の第１実施形態におけるシミュレーション結果を示す図である。 本発明の第２実施形態における制御部のブロック線図である。 本発明の第２実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第３実施形態における制御部のブロック線図である。 本発明の第３実施形態における制御部のフィードフォワード制御のブロック線図である。 本発明の第３実施形態におけるスティフネス楕円を説明する図である。 本発明の第３実施形態における軌道設計を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。

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

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置に適用されるアクチュエータの粘弾性モデルを示す図である。本第１実施形態では、ロボット装置として、空気圧式の人工筋肉アクチュエータを用いたマニピュレータの関節角度と関節の剛性の同時制御を例として説明する。

（１）モデリング
人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。人工筋肉アクチュエータは、図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。ここで、力発生要素の収縮力をｕ、収縮方向を正とする筋の収縮量をｘとする。収縮速度は、以下の式となる。

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

とモデル化され、筋収縮力の弾性力、粘性力は、力発生要素の収縮力ｕに比例するという非線形要素を有する点が特徴的である。

次に、本第１実施形態のロボット装置を図２に示す。図２に示すロボット装置１００は、いわゆる１リンクマニピュレータであり、リンク１０１と、一対のアクチュエータｅ_１，ｆ_１からなる駆動部１２０と、基体であるベース部材１０３及びベース部材１０３から延びる棒状のアーム部材１０４とを備えている。リンク１０１は、アーム部材１０４に関節１０５を介して旋回可能に連結されている。

なお、本第１実施形態では、この基体として、図２に示すように、ベース部材１０３及びアーム部材１０４で構成される場合について説明するが、これに限定するものではない。例えば、基体が、脚式移動ロボットの胴体や作業ロボットの台座部、リンク１０１に連結される別のリンク等であってもよい。

各アクチュエータｅ_１，ｆ_１は、一関節筋アクチュエータであり、一端が基体であるベース部材１０３に接続され、他端がリンク１０１の基端１０１ａに接続されて、駆動力（収縮力）の差によりリンク１０１を揺動させるように拮抗配置されている。つまり、アクチュエータｅ_１，ｆ_１は、リンク１０１を挟んでリンク１０１の両側に対称配置されている。

また、ロボット装置１００は、各アクチュエータｅ_１，ｆ_１の駆動力を各駆動力指令値により設定して、リンク１０１の動作を制御する制御部１５０を備えている。

各アクチュエータｅ_１，ｆ_１は、図１に示す空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは収縮方向にのみ力を発生するため、関節１０５を任意の角度に位置決めするために、図２に示すように拮抗して配置されている。つまり、各アクチュエータｅ_１，ｆ_１は、ベース部材に対して互いに反対方向にリンク１０１を引っ張る駆動力を発生する。各アクチュエータｅ_１，ｆ_１は、人工筋肉からなる一関節筋アクチュエータであるので、収縮することにより駆動力が発生する。したがって、アクチュエータｅ_１，ｆ_１の収縮力が駆動力となる。

拮抗配置されたアクチュエータｅ_１，ｆ_１の力発生要素の駆動力（収縮力）をそれぞれｕ_ｅ１，ｕ_ｆ１、アーム部材１０４に対するリンク１０１の角度、つまり関節１０５の角度をθ、リンク１０１の慣性モーメントをＩとする。また、リンク１０１のモーメントアーム径、つまりリンク１０１の旋回中心点とリンク１０１におけるアクチュエータｅ_１，ｆ_１の接続点との長さをｒとする。運動方程式は、以下の式（２）となる。

式（２）の右辺第一項における収縮力ｕ_ｅ１，ｕ_ｆ１の差が関節１０５に回転トルクを与え、右辺第二、第三項における収縮力ｕ_ｅ１，ｕ_ｆ１の和が関節１０５に対する剛性および粘性を変動させることがわかる。つまり、アクチュエータｅ_１，ｆ_１は、駆動力の差により関節１０５にトルクを付与すると共に、駆動力の和により関節１０５に剛性を付与する。

（２）制御系設計
制御部１５０は、トルク指令値演算処理１５１、変更演算処理１５２、反復処理１５３、駆動力指令値演算処理１５５を実行する。本第１実施形態では、駆動力指令値ｕ_ｅ１，ｕ_ｆ１を算出する駆動力指令値演算処理１５５の実行にフィードフォワード制御入力を与えることにより、関節１０５の角度を目標軌道に追従させる。このとき、収縮力（駆動力）が最小となるフィードフォワード入力を求める。関節１０５の目標軌道をｒ_ａとする。つまり、制御部１５０は、関節１０５の角度θを目標軌道ｒ_ａに追従させるのに必要な各アクチュエータｅ_１，ｆ_１の各駆動力指令値ｕ_ｅ１，ｕ_ｆ１を求める。そして、制御部１５０は、各アクチュエータｅ_１，ｆ_１に発生させる駆動力が駆動力指令値ｕ_ｅ１，ｕ_ｆ１となるように、各アクチュエータｅ_１，ｆ_１を制御する。

（２．１）フィードフォワード制御系設計
本節では、各アクチュエータｅ_１，ｆ_１に与えるフィードフォワード入力の導出方法について述べる。まず、以下のようにモーメントアーム径ｒ倍される駆動力指令値ｕ_ｅ１，ｕ_ｆ１の差をＴ_１、駆動力指令値ｕ_ｅ１，ｕ_ｆ１の和（合計値）を示す関節剛性指令値をＵ_１とする。
（ｕ_ｆ１−ｕ_ｅ１）ｒ＝Ｔ_１ （３）
ｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１ （４）

また、人工筋肉アクチュエータｅ_１，ｆ_１は収縮方向にのみ力を発生するため、以下の条件を同時に満たさなければならない。
ｕ_ｆ１＞０，ｕ_ｅ１＞０ （５）

ここで、式（３）及び式（４）を式（２）に代入すると、

と表される。Ｔ_１が関節１０５にトルクを与え、Ｕ_１が関節１０５に対する剛性および粘性を増減させることがわかる。

そこで、トルク指令値Ｔ１，関節剛性指令値Ｕ１を同時に満たす駆動力指令値を求めるには、式（３）と式（４）をｕ_ｅ１，ｕ_ｆ１について解き、

と決定すればよい。式（７）より、駆動力指令値ｕ_ｆ１，ｕ_ｅ１を最小化して任意のトルク指令値Ｔ_１を与えるためには、関節剛性指令値Ｕ_１を最小化すればよいことがわかる。しかし、式（５）と式（７）より、
Ｔ_１／ｒ＜Ｕ_１，Ｔ_１／ｒ＞−Ｕ_１ （８）
という条件が得られる。これより、人工筋肉アクチュエータｅ_１，ｆ_１に収縮方向のみの駆動力指令値ｕ_ｆ１，ｕ_ｅ１を与えるために、関節剛性指令値Ｕ_１はトルク指令値Ｔ_１を用いて、
｜Ｔ_１｜／ｒ＜Ｕ_１ （９）
という条件を満たす必要があることがわかる。

関節１０５の角度を任意の目標軌道ｒ_ａに追従させるためのトルク指令値Ｔ_１は、目標軌道ｒ_ａの角速度、角加速度をそれぞれ、

あるいは、ｒ_ａ’，ｒ_ａ’’と表す。トルク指令値Ｔ_１は、リンク１０１の逆動力学により、以下の式（１０）と表せる。

式（１０）には、関節剛性指令値Ｕ_１が含まれるが、式（９）からわかるように、関節剛性指令値Ｕ_１はトルク指令値Ｔ_１で制約される指令値である。そこで、本第１実施形態では、式（９）と式（１０）を同時に満たすトルク指令値Ｔ_１，関節剛性指令値Ｕ_１を反復演算アルゴリズムで求める。

図３に本第１実施形態における制御部１５０のブロック線図を示す。Ｉｔｅｒａｔｉｏｎ Ａｌｇｏｒｉｔｈｍで示したブロックで、変更演算処理１５２を行っている。

本第１実施形態では、反復回数をｉとして、ｉ回の反復演算による、トルク指令値Ｔ_１と関節剛性指令値Ｕ_１を、
Ｔ_１［ｉ］，Ｕ_１［ｉ］ （１１）
と表記する。

制御部１５０は、トルク指令値演算処理１５１として、リンク１０１の逆動力学に基づき、目標軌道ｒ_ａ、目標軌道の角速度ｒ_ａ’、目標軌道の角加速度ｒ_ａ’’、関節剛性指令値Ｕ_１［ｉ］を用いて、関節に必要なトルクを示すトルク指令値Ｔ_１［ｉ］を演算する。つまり、制御部１５０は、ｉ回目の反復によるトルク指令値Ｔ_１［ｉ］を、式（１２）を用いて演算する（トルク指令値演算工程）。

次に、制御部１５０は、変更演算処理１５２として、関節剛性指令値Ｕ_１［ｉ］と、トルク指令値Ｔ_１［ｉ］の絶対値をリンク１０１のモーメントアーム径ｒで割った値｜Ｔ_１［ｉ］｜／ｒとの差分を演算する。この差分をＥ_１［ｉ］とすると、制御部１５０は、以下の式（１３）により差分Ｅ_１［ｉ］を演算する。
Ｅ_１［ｉ］＝Ｕ_１［ｉ］−｜Ｔ_１［ｉ］｜／ｒ （１３）

次に、制御部１５０は、変更演算処理１５２として、関節剛性指令値Ｕ_１［ｉ］から、差分Ｅ_１［ｉ］に０よりも大きく１以下の収束係数γを乗じた値を引く演算を行うことで、関節剛性指令値Ｕ_１［ｉ］を変更する（変更演算工程）。つまり、制御部１５０は、反復演算アルゴリズムの収束係数γを０＜γ≦１として、ｉ＋１回目の関節剛性指令値Ｕ_{１［ｉ＋１］}を以下の式（１４）により求める。
Ｕ_{１［ｉ＋１］}＝Ｕ_１［ｉ］−γＥ_１［ｉ］ （１４）

制御部１５０は、反復処理１５３として、式（１２）〜式（１４）の演算を、差分Ｅ_１［ｉ］が所定値以下（例えば０）に収束するまでの所定回数ｍ、反復して行う（反復工程）。

差分Ｅ_１［ｉ］が０に収束しているので、関節剛性指令値Ｕ_１［ｍ］は式（９）を満たす最小の値であり、これを用いるトルク指令値Ｔ_１［ｍ］は最小の値をとる。これより、駆動力指令値ｕ_ｅ１，ｕ_ｆ１を最小とするフィードフォワード入力は、式（７）より、

と導出される。

したがって、制御部１５０は、駆動力指令値演算処理１５５として、反復処理１５３後、関節剛性指令値Ｕ_１［ｍ］及びトルク指令値Ｔ_１［ｍ］を用いて、各駆動力指令値ｕ_ｆ１，ｕ_ｅ１を演算する（駆動力指令値演算工程）。

さらに、関節剛性指令値に下限値を設けるには、剛性下限値をＶ_１として、式（１３）に代わり次式を用いることで可能である。
Ｅ_１［ｉ］＝Ｕ_１［ｉ］−｜Ｔ_１［ｉ］｜／ｒ−Ｖ_１ （１６）

式（１６）により、例えば、目標軌道の到達時間におけるＶ_１を調整すれば、マニピュレータの到達角度における関節剛性の制御が可能となる。

なお、目標軌道ｒ_ａの到達角度をθ_ｄとすると、リンク整定に必要なトルク指令値Ｔ_１ｄは、
Ｕ_１ｋｒ^２θ_ｄ≦Ｔ_１ｄ （１７）
となるが、同時に式（９）を満たす必要がある。そのため、弾性定数ｋ、プーリ径ｒは以下の条件
ｋｒθ_ｄ＜１ （１８）
を満たす必要がある。

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

（２．２）軌道設計
次に、関節１０５の目標軌道ｒ_ａの設計方法を示す。本実施形態では、目標軌道ｒ_ａは図４に示すような、加速区間−等速区間−減速区間を有する軌道を用いる。ここで、ｒ_ａＳ，ｒ_ａＦはそれぞれ、軌道の初期角度、目標角度である。また、ｔ_ａ，ｔ_ｂ，ｔ_ｆｉｎはそれぞれ、等速区間の開始時間、等速区間の終了時間、位置決め終了時間である。本第１実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。

（３）シミュレーション
前節の制御系を用いるシミュレーションを行う。リンクの慣性モーメントをＩ＝８．３×１０^−２ｋｇｍ^２、モーメントアーム径をｒ＝０．１ｍ、弾性、粘性定数をｋ＝２５，ｂ＝３とする。目標軌道ｒ_ａは、初期角度ｒ_ａＳ＝−２０ｄｅｇ、目標角度ｒ_ａＦ＝２０ｄｅｇ、等速区間の開始時間ｔ_ａ＝０．４秒、等速区間の終了時間ｔ_ｂ＝０．６秒、位置決め終了時間ｔ_ｆｉｎ＝１秒とする。関節剛性指令値Ｕ_１は２．１に示した反復演算アルゴリズムにより、式（９）に示した制約を満たすように導出されている。なお、関節剛性指令の下限値はＶ_１＝０．０２とし、γ＝１として反復演算を行っている。

図５（ａ）に関節の角度θと目標軌道ｒ_ａを、それぞれ実線、破線で、図５（ｂ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１の応答をそれぞれ実線、破線で示す。さらに、図５（ｃ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１の差を実線で、和を破線で、Ｔ_１／ｒの応答を一点鎖線で示す。

図５（ａ）より実線で示す関節１０５の角度θは、破線で示す軌道ｒ_ａと重なっており、関節１０５は目標軌道ｒ_ａに追従していることがわかる。図５（ｂ）より、駆動力指令値ｕ_ｅ１，ｕ_ｆ１はフィードフォワード制御系の制御入力（トルク指令値）Ｔ_１が式（７）と式（９）に示した条件で配分されているため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。図５（ｃ）において、実線で示すｕ_ｆ１−ｕ_ｅ１は、一点鎖線で示すＴ_１／ｒと重なっており、式（３）の条件を満たしている。また、破線で示すｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１は、実線で示すＴ_１／ｒの絶対値に対してＶ_１の間隔をもって近接しており、常に最小値を取っていることがわかる。本第１実施形態の提案手法により、目標軌道ｒ_ａへの追従を実現しながら、最小の駆動力指令値ｕ_ｅ１，ｕ_ｆ１での拮抗駆動が可能であることがわかる。駆動力指令値ｕ_ｅ１，ｕ_ｆ１を最小にすることで、例えばＭｃＫｉｂｂｅｎ型人工筋肉では消費する空気の量を最小にすることができる。これにより、タンク等の搭載型の空気源を用いるロボット装置では稼働時間を延長することが可能となる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。本第２実施形態では、制御部の制御動作が上記第１実施形態の制御部の制御動作と異なるものである。なお、ロボット装置本体の構成について、上記第１実施形態で説明した図１のロボット装置本体の構成と同様の部分については、同一符号を付して詳細な説明を省略する。

本第２実施形態では、上記第１実施形態で示したフィードフォワード制御系とフィードバック制御系を併合する２自由度制御系を導出する。フィードバック制御系は、目標軌道と反復演算アルゴリズムで求めた関節剛性指令値に同時に追従可能とするように導出する。フィードフォワード制御系のみでは、リンク１０１の慣性モーメントなどにパラメータ同定誤差が生ずると制御性能が劣化してしまう。しかし、フィードバック制御系との２自由度制御系を構成することにより、モデル誤差が存在しても目標軌道と関節剛性指令値の同時制御が可能であることを示す。

（１）モデリング
本第２実施形態で用いるモデルは上記第１実施形態と同じとする。

（２）制御系設計
フィードフォワード制御系の導出は上記第１実施形態と同様である。本第２実施形態では、上記第１実施形態の反復演算アルゴリズムにより導出される関節トルク指令Ｔ_{ＦＦ［ｍ］}をＴ_ＦＦと記す。

図６に本第２実施形態に係る制御部１５０Ａのブロック線図である。図６では、図３で示したフィードフォワード制御系の破線で囲まれた部分を、Ｋ_ＦＦＷとして示している。

制御部１５０Ａは、フィードフォワード制御系Ｋ_ＦＦＷの他、更に、補正トルク指令値演算処理１５４としてのＰＩＤ制御処理Ｋ_ＰＩＤ１を実行する機能を有している。

制御部１５０Ａは、ＰＩＤ制御処理Ｋ_ＰＩＤ１として、関節１０５の角度θと目標軌道ｒ_ａとの差分を補償する補正トルク指令値としての制御入力トルクＴ_ＦＢを演算する（補正トルク指令値演算工程）。

ここで、式（２）より、
（ｕ_ｆ１−ｕ_ｅ１）ｒ＝Ｔ_ＦＦ＋Ｔ_ＦＢ＝Ｔ′_１ （２０）
を満たすようにｕ_ｅ１，ｕ_ｆ１を決定すれば、関節１０５にフィードフォワード制御トルクＴ_ＦＦとフィードバック制御トルクＴ_ＦＢの和であるＴ′_１を与えることができる。トルク指令値Ｔ′_１は、フィードフォワード制御系Ｋ_ＦＦＷのトルク指令値演算処理１５１（図３参照）で求めたトルク指令値Ｔ_ＦＦにＰＩＤ制御処理Ｋ_ＰＩＤ１で求めた制御入力トルクＴ_ＦＢを加算補正した演算結果である。

したがって、本第２実施形態では、駆動力指令値演算処理１５５を実行する際に、各駆動力指令値の算出に用いるトルク指令値として、トルク指令値Ｔ′_１を用いる。

ところで、上記第１実施形態と同様に、同時に関節１０５の剛性に関する条件
Ｕ_１＝ｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１［ｍ］ （２１）
を満たすように、駆動力指令値ｕ_ｅ１，ｕ_ｆ１を決定する必要がある。

トルク指令値Ｔ′_１と関節剛性指令値Ｕ_１を同時に満たすには、式（２０）と式（４）を駆動力指令値ｕ_ｅ１，ｕ_ｆ１について解き、

と決定すればよい。しかし、人工筋肉アクチュエータｅ_１，ｆ_１は収縮方向にのみ力を発生するため、上記第１実施形態と同様に、Ｔ′_１が、
−Ｕ_１ｒ＜Ｔ′_１＜Ｕ_１ｒ （２３）
という条件を満たす必要がある。これにより、関節１０５の剛性が関節剛性指令値Ｕ_１となると同時に、トルク指令値Ｔ′_１により目標軌道ｒ_ａに追従制御される。式（２３）を実現するには、制御部１５０Ａは、図６のブロック線図に示すように、
｜Ｔ′_１｜＜Ｕ_１ｒ （２４）
と制御入力を制約する制約処理１５６を実行すればよい。もしくは、ＰＩＤ制御処理Ｋ_ＰＩＤ１のゲインを変動させ、式（２３）の範囲に制御入力の大きさを収めるなどの方法がある。

上記第１実施形態において、剛性下限値Ｖ_１を設定することにより、関節剛性を制御することが可能であることを示した。しかし、フィードバック制御系のゲインが高い場合には、それによる関節１０５の剛性が支配的になってしまう。

そこで、制御部１５０Ａは、図６に示すように、接触ゲインＧ_ｔを導入し、接触が発生した場合は、Ｇ_ｔ＝０とすることでフィードバック系を遮断する。これにより、手先が人や物に接触したことを検知するなどの任意の時刻で、速やかにリンク１０１の剛性を関節剛性指令値Ｕ_１へと切り替えることができる。

本第２実施形態では、ＰＩＤ制御処理Ｋ_ＰＩＤ１に用いる伝達関数を、

としている。

（３）シミュレーション
前節で導出した制御系を用いてシミュレーションを行った。リンク１０１のパラメータと目標軌道ｒ_ａは上記第１実施形態と同様とする。本第２実施形態では、モデルの同定誤差に対する制御性能の検証を行うため、リンク１０１の慣性モーメントをＩ′＝１．０５Ｉとし、フィードフォワード制御入力生成時のモデルに対して誤差を有するモデルを用いてシミュレーションを行った。また本第２実施形態では、上記第１実施形態のように関節剛性指令の下限値を小さくすると、フィードバック制御系の制約の範囲が小さくなることで、ワインドアップ現象を発生しやすくなってしまう。そこで、Ｖ_１＝０．１としてフィードフォワード入力を導出している。

図７（ａ）に関節１０５の角度θと目標軌道ｒ_ａを、それぞれ実線、破線で、図７（ｂ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１の応答をそれぞれ実線、破線で示す。さらに、図７（ｃ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１の差を実線で、和を破線で、Ｔ′_１／ｒの応答を一点鎖線で示す。また、図７（ｄ）にＴ_ＦＦ，Ｔ_ＦＢをそれぞれ実線、破線で示す。

図７（ａ）より、実線で示す関節１０５の角度θは破線で示す目標軌道ｒ_ａと重なっており、関節１０５は目標軌道ｒ_ａに追従していることがわかる。フィードフォワード制御系のみでは、モデル誤差により軌道に追従することはできない。しかし、図７（ｄ）の破線で示すように、フィードバック制御入力がモデル誤差を補償しているため、追従することを可能としている。図７（ｂ）より、駆動力指令値ｕ_ｅ１，ｕ_ｆ１は２自由度制御系の制御入力Ｔ′_１が、式（２２）と式（２４）に示した条件で配分されているため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。図７（ｃ）において、実線で示すｕ_ｆ１−ｕｅ１は、一点鎖線で示すＴ′_１／ｒと重なっており、式（２０）の条件を満たしている。また、破線で示すｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１は、上記第１実施形態と同様に、実線で示すＴ′_１／ｒの絶対値に近接しており、式（２４）の制約を満たす範囲で、常に最小値を取っていることがわかる。本第２実施形態で示した２自由度制御系により、モデルに同定誤差などが存在しても、目標軌道ｒ_ａへの追従を実現しながら、最小の収縮力での拮抗駆動が可能であることがわかる。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について詳細に説明する。図８は、本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。本第３実施形態では、人工筋肉アクチュエータを用いた３対６筋を有する２リンクマニピュレータとしてのロボット装置２００の手先剛性の制御を例として説明する。

（１）モデリング
図８に示すロボット装置２００は、プーリ２０３と、プーリ２０３に対して旋回可能に第１関節２１１を介して連結された第１リンク２０１と、第１リンク２０１に対して旋回可能に第２関節２１２を介して連結された第２リンク２０２とを備えている。

第１リンク２０１は、長手部材からなり、その基端２０１ａがプーリ２０３にｘ−ｙ直交座標系の平面（以下、「作業平面」という）内で旋回可能に支持されている。第２リンク２０２は、長手部材からなり、その基端２０２ａが第１リンク２０１の先端２０１ｂに作業平面内で旋回可能に支持されている。

第２リンク２０２の先端（以下、「リンク先端」という）２０２ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、第１リンク２０１は、第１関節２１１と第２関節２１２との間に配置され、第１関節２１１で旋回可能に支持されており、第２リンク２０２は、第２関節２１２で旋回可能に支持されている。

なお、本第２実施形態では、第１リンク２０１に対しては、プーリ２０３が基体となり、第２リンク２０２に対しては、第１リンク２０１が基体となる。このプーリ２０３は、例えばロボット胴体等に設けられている。

ロボット装置２００は、一対の第１アクチュエータｅ_１，ｆ_１と、一対の第２アクチュエータｅ_２，ｆ_２と、一対の第３アクチュエータｅ_３，ｆ_３と、を備えている。各第１アクチュエータｅ_１，ｆ_１は、一端がプーリ２０３に接続され、他端が第１リンク２０１の長手方向中央部に接続され、駆動力の差により第１リンク２０１を旋回させるように拮抗配置されている。

また、各第２アクチュエータｅ_２，ｆ_２は、一端が第１リンク２０１の長手方向中央部に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第２リンク２０２を旋回させるように拮抗配置されている。また、各第３アクチュエータｅ_３，ｆ_３は、一端がプーリ２０３に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第１リンク２０１及び第２リンク２０２を旋回させるように拮抗配置されている。つまり、第１アクチュエータｅ_１，ｆ_１は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第２アクチュエータｅ_２，ｆ_２は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第３アクチュエータｅ_３，ｆ_３は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。

また、ロボット装置２００は、各アクチュエータｅ_１，ｆ_１，ｅ_２，ｆ_２，ｅ_３，ｆ_３の駆動力を各駆動力指令値により設定して、リンク２０１，２０２の動作を制御する制御部２５０を備えている。

第１アクチュエータｅ_１，ｆ_１は、第１リンク２０１を駆動する第１の一関節駆動アクチュエータである。第２アクチュエータｅ_２，ｆ_２は、第２リンク２０２を駆動する第２の一関節駆動アクチュエータである。また、第３アクチュエータｅ_３，ｆ_３は、第１リンク２０１と第２リンク２０２を同時に駆動する二関節同時駆動アクチュエータである。人の上腕部や下肢大腿部には、二関節筋とよばれる二関節同時駆動アクチュエータが存在することが知られている。人の四肢の筋配列は複雑だが、実効筋概念が導入され、３対６筋を有する２リンクモデルが提示されている。

各アクチュエータｅ_１，ｆ_１，ｅ_２，ｆ_２，ｅ_３，ｆ_３は、図１に示す筋の粘弾性特性を有する人工筋肉アクチュエータである。人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。筋は図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。

図８のアクチュエータｅ_１，ｆ_１，ｅ_２，ｆ_２，ｅ_３，ｆ_３について、ｕ_ｅｎ，ｕ_ｆｎ（ｎ＝１，２，３）を力発生要素の駆動力を発生させる駆動力指令値とする。また、ｋ_ｅｎ，ｋ_ｆｎ，ｂ_ｅｎ，ｂ_ｆｎ（ｎ＝１，２，３）を人工筋肉アクチュエータの弾性力定数、粘性力定数とする。第１，第２リンク２０１，２０２の角度、つまり関節の角度をθ_１，θ_２、第１，第２リンク２０１，２０２の慣性モーメントをＩ_１，Ｉ_２、第１，第２リンク２０１，２０２の長さを２×ｌ_１，２×ｌ_２、第１，第２リンク２０１，２０２の質量をｍ_１，ｍ_２とする。

モーメントアーム径、つまり第１リンク２０１の旋回中心点とプーリ２０３におけるアクチュエータｅ_１，ｆ_１の接続点との長さ、及び第２リンク２０２の旋回中心点と第２リンク２０２におけるアクチュエータｅ_２，ｆ_２の接続点との長さをｒとする。

本第３実施形態では、各筋の弾性力定数，粘性力定数をいずれもｋ，ｂとすると、運動方程式は、以下の式（２６）及び式（２７）となる。

となる。

（２）制御系設計
上記第１及び第２実施形態では、収縮力（駆動力）を最小にする関節の軌道制御を行ったが、本第３実施形態においても、２自由度制御系を設計する。

図９に本第３実施形態の制御部２５０のブロック線図を示す。また、図１０に、フィードフォワード制御のブロック線図を示す。Ｋ_ＦＦＷで示すブロックがフィードフォワード制御系である。制御部２５０は、トルク指令値演算処理２５１、変更演算処理２５２、反復処理２５３、補正トルク指令値演算処理２５４及び駆動力指令値演算処理２５５を実行する。本第３実施形態では、制御部２５０は、関節２１１，２１２の角度を目標軌道に追従させるのに必要な各アクチュエータｅ_ｎ，ｆ_ｎの各駆動力指令値ｕ_ｅｎ，ｕ_ｆｎを求める。そして、制御部２５０は、各アクチュエータｅ_ｎ，ｆ_ｎに発生させる駆動力が駆動力指令値ｕ_ｅｎ，ｕ_ｆｎとなるように、各アクチュエータｅ_ｎ，ｆ_ｎを制御する。

（２．１）フィードフォワード制御系
上記第１実施形態と同様に、駆動力指令値の差をＴ_ｎ、和をＵ_ｎとおくと、
（ｕ_ｆｎ−ｕ_ｅｎ）ｒ＝Ｔ_ＦＦｎ＋Ｔ_ＦＢｎ＝Ｔ_ｎ，ｎ＝１，２，３ （２８）
ｕ_ｆｎ＋ｕ_ｅｎ＝Ｕ_ｎ，ｎ＝１，２，３ （２９）
と表せる。ここで、Ｔ_ＦＦｎ，Ｔ_ＦＢｎは、それぞれフィードフォワード制御系、フィードバック制御系により与えられるトルク指令値である。本節では、Ｔ_ＦＢｎ＝０としてフィードフォワード制御入力の導出方法を示す。

式（２６）及び式（２７）をθ_ｖ＝［θ_１ θ_２］^Ｔ，Ｔ_ｖ＝［Ｔ_１＋Ｔ_３ Ｔ_２＋Ｔ_３］^Ｔと定義して行列表記すると、

となる。ここで、Ｋ_ｖ，Ｃ_ｖはそれぞれ筋の粘弾性による剛性行列、減衰行列であり、

である。また、θ_ｃｖは、関節トルクＴ_１，Ｔ_２が作用しないときの筋の弾性力による中立姿勢角度θ_ｃ１，θ_ｃ２からなるベクトルであり、
θ_ｃｖ＝［θ_ｃ１ θ_ｃ２］^Ｔ （３２）
となる。関節角度θ_ｎ（ｎ＝１，２）に対する目標軌道をｒ_ａｎ（ｎ＝１，２）とし、
ｒ_ａｖ＝［ｒ_ａ１ ｒ_ａ２］^Ｔ （３３）
と定義する。目標軌道の角速度、角加速度をそれぞれ、

と表す。ここで、関節角度が目標軌道に追従するために必要なフィードフォワードトルク指令値はＴ_ＦＦｖ＝［Ｔ_ＦＦ１＋Ｔ_ＦＦ３ Ｔ_ＦＦ２＋Ｔ_ＦＦ３］^Ｔと表す。トルク指令値Ｔ_ＦＦｖはリンクの逆動力学により、

と表すことができる。

しかし、二関節同時駆動アクチュエータを有するマニピュレータは、制御される自由度に対して、制御入力の数が冗長である。そのため、逆動力学により各関節２１１，２１２に対するトルク指令値（以下、各関節トルクと略す）は求められるが、二関節駆動アクチュエータによるトルクを含む指令Ｔ_ＦＦｎ（ｎ＝１，２，３）を一意に決定できない。そこで、式（３４）右辺から得られる関節トルク指令をＴ_ＭＦ＝［Ｔ_ＭＦ１ Ｔ_ＭＦ２］^Ｔとおき、Ｔ_ＦＦｎ（ｎ＝１，２，３）の絶対値の最大値
ｍａｘ（｜Ｔ_ＦＦｎ｜，ｎ＝１，２，３） （３５）
が最小となるように、各関節トルク指令Ｔ_ＭＦを分配する。

まず、Ｔ_ＭＦとＴ_ＦＦｎ（ｎ＝１，２，３）の関係は、式（３６）及び式（３７）となる。
Ｔ_ＦＦ１＋Ｔ_ＦＦ３＝Ｔ_ＭＦ１ （３６）
Ｔ_ＦＦ２＋Ｔ_ＦＦ３＝Ｔ_ＭＦ２ （３７）

Ｔ_ＭＦ１，Ｔ_ＭＦ２が同符号の場合、式（３６）及び式（３７）より、Ｔ_ＦＦ３を増減させるとＴ_ＦＦ３の変化量だけＴ_ＦＦ１とＴ_ＦＦ２を増減できることがわかる。Ｔ_ＭＦ１≦Ｔ_ＭＦ２である場合、Ｔ_ＦＦ３を０から徐々に増減させ、Ｔ_ＦＦ１＝Ｔ_ＦＦ３とすればよい。このとき、Ｔ_ＦＦｎ（ｎ＝１，２，３）は、それぞれ、

と求まる。一方、Ｔ_ＭＦ２＞Ｔ_ＭＦ１である場合は、Ｔ_ＦＦ２＝Ｔ_ＦＦ３とすればよく、Ｔ_ＦＦｎ（ｎ＝１，２，３）は

と求まる。

Ｔ_ＭＦ１，Ｔ_ＭＦ２が異符号の場合は、例えばＴ_ＦＦ３の増加によりＴ_ＦＦ１の絶対値は減少するがＴ_ＦＦ２の絶対値は増加してしまう。従って、Ｔ_ＦＦ１，Ｔ_ＦＦ２を、
｜Ｔ_ＦＦ１｜＝｜Ｔ_ＦＦ２｜ （４０）
とすればよい。また、Ｔ_ＭＦ１とＴ_ＭＦ２が異符号で、式（４０）を満たす場合、Ｔ_ＦＦ１とＴ_ＦＦ２は異符号となるため、式（３６）、式（３７）及び式（４０）より、

と求まる。

これを用いて、上記第１実施形態と同様に、収縮方向のみに力を発生するという条件、
｜Ｔ_ＦＦｎ｜＜Ｕ_ｎｒ，ｎ＝１，２，３ （４２）
を成立するトルク指令値Ｔ_ＦＦｎと関節剛性指令値Ｕ_ｎとを反復演算アルゴリズムにより求める。

本第３実施形態では、上記第１実施形態と同様に、反復回数をｉとして、ｉ回の反復演算による、トルク指令値Ｔ_ＦＦｎと関節剛性指令値Ｕ_ｎを
Ｔ_{ＦＦｎ［ｉ］}，Ｕ_ｎ［ｉ］，ｎ＝１，２，３ （４３）
と表記する。また、Ｕ_ｎ［ｉ］が要素に含まれる筋の粘弾性による剛性行列、減衰行列をＫ_ｖ［ｉ］，Ｃ_ｖ［ｉ］と表記する。

制御部２５０は、トルク指令値演算処理２５１として、リンク２０１，２０２の逆動力学により、目標軌道、目標軌道の角速度、目標軌道の角加速度、関節剛性指令値を用いて、関節２１１，２１２に必要なトルクを示すトルク指令値Ｔ_{ＦＦｎ［ｉ］}を演算する。
つまり、制御部２５０は、式（４４）に基づき、ｉ回目の反復によるトルク指令値Ｔ_{ＦＦｎ［ｉ］}を演算する（トルク指令値演算工程）。

ここで、制御部２５０は、式（４４）を、式（３８）〜（４１）に示した方法により分配しＴ_{ＦＦｎ［ｉ］}（ｎ＝１，２，３）を求める。

次に、制御部２５０は、変更演算処理２５２として、関節剛性指令値Ｕ_ｎ［ｉ］と、トルク指令値Ｔ_{ＦＦｎ［ｉ］}の絶対値をリンクのモーメントアーム径ｒで割った値｜Ｔ_{ＦＦｎ［ｉ］}｜／ｒとの差分を演算する。この差分をＥ_ｎ［ｉ］とすると、制御部２５０は、以下の式（４５）により差分Ｅ_１［ｉ］を演算する。

なお、本第３実施形態では、関節剛性指令値Ｕ_ｎに対する剛性下限値をＶ_ｎとし、式（４２）の条件に対する差分Ｅ_ｎ［ｉ］を以下のように定義している。
Ｅ_ｎ［ｉ］＝Ｕ_ｎ［ｉ］−｜Ｔ_{ＦＦｎ［ｉ］}｜／ｒ−Ｖ_ｎ，ｎ＝１，２，３ （４５）

次に、制御部２５０は、変更演算処理２５２として、関節剛性指令値Ｕ_ｎ［ｉ］から、差分Ｅ_ｎ［ｉ］に０よりも大きく１以下の収束係数γを乗じた値を引く演算を行うことで、関節剛性指令値Ｕ_ｎ［ｉ］を変更する（変更演算工程）。つまり、制御部２５０は、反復演算アルゴリズムの収束係数γを０＜γ≦１として、ｉ＋１回目の関節剛性指令値Ｕ_{ｎ［ｉ＋１］}を以下の式（４６）により求める。
Ｕ_{ｎ［ｉ＋１］}＝Ｕ_ｎ［ｉ］−γＥ_ｎ［ｉ］，ｎ＝１，２，３ （４６）

制御部２５０は、反復処理２５３として、式（４４）〜式（４６）の演算を、差分Ｅ_ｎ［ｉ］が所定値以下（例えば０）に収束するまで所定回数ｍ、反復して行う（反復工程）。

図１０に示すＤｉｓｔｒｉｂｕｔｉｏｎ Ａｌｇｏｒｉｔｈｍのブロックでは、式（３８）〜（４１）に示した分配アルゴリズムを実行し、Ｉｔｅｒａｔｉｏｎ Ａｌｇｏｒｉｔｈｍのブロックでは、式（４４）〜式（４６）の反復演算アルゴリズムを実行する。

次に、フィードフォワード制御のみを行う場合（つまりＴ_ＦＢｎ＝０の場合）についての駆動力指令値演算処理２５５（図９）について説明する。なお、トルク指令値はＴ_ｎ＝Ｔ_ＦＦＷｎである。

制御部２５０は、反復処理２５３の終了後、駆動力指令値演算処理２５５として、関節剛性指令値Ｕ_ｎ［ｍ］及びトルク指令値Ｔ_ｎ［ｍ］を用いて、各駆動力指令値を演算する各駆動力指令値ｕ_ｅｎ，ｕ_ｆｎを演算する（駆動力指令値演算工程）。これにより、関節２１１，２１２にはトルク指令値Ｔ_ｎに対応するトルクが印加され、関節２１１，２１２の角度θ_１，θ_２が目標軌道に追従する。

（２．２）２自由度制御系
フィードフォワード制御と共にフィードバック制御も行う場合は、制御部２５０は、図９に示す、補正トルク指令値演算処理２５４として、ＰＩＤ制御処理Ｋ_ＰＩＤ１〜Ｋ_ＰＩＤ３を実行する（補正トルク指令値演算工程）。

そして、制御部２５０は、トルク指令値Ｔ_ｎとして、フィードフォワード入力Ｔ_ＦＦｎに制御入力トルクＴ_ＦＢｎを加算補正した結果を用いる。その際、制御部２５０は、上記第２実施形態と同様、制約処理２５６として式（３０）で制約し、収縮力ｕ_ｆｎ，ｕ_ｅｎを求める。

以下、具体的に説明すると、図９に示すフィードバック制御系であるＰＩＤ制御処理Ｋ_ＰＩＤ１，Ｋ_ＰＩＤ２では、関節角度θ_１，θ_２と目標軌道ｒ_ａ１，ｒ_ａ２との差分を補償するための制御入力トルクＴ_ＦＢ１，Ｔ_ＦＢ２を演算する。さらに、ＰＩＤ制御処理Ｋ_ＰＩＤ３では、関節角度θ_１＋θ_２と目標角度ｒ_ａ１＋ｒ_ａ２との差分を補償するための制御入力トルクＴ_ＦＢ３を演算する。

そして、制御入力Ｔ_ｎ（ｎ＝１，２，３）に対して上記第２実施形態と同様に、
｜Ｔ_ｎ｜＜Ｕ_ｎｒ，ｎ＝１，２，３ （４７）
と制約する。駆動力指令値ｕ_ｆｎ，ｕ_ｅｎ（ｎ＝１，２，３）は、

となる。

（２．３）手先剛性制御
２リンクマニピュレータでは、手先が外界と直に接触するので、手先の剛性を制御することが重要である。手先の剛性は、図１１に示すようなスティフネス楕円によって表される。この楕円は、各方向に対しての剛性の分布を示し、手先と楕円の距離が離れるほど剛性が高いことを表している。上記第１実施形態の１リンクマニュピレータでは、関節の剛性と手先の剛性は同一であった。本第３実施形態の３対６筋を有する２リンクマニュピレータでは、関節剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を制御することにより手先の剛性が求まる。例えば、関節剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を
Ｕ_１＝Ｕ_２＝Ｕ_３ （４９）
と制御すると、スティフネス楕円の長軸は第１関節と手先を結ぶ方向を向くことが知られている。本第３実施形態では、剛性は収縮力が最小値となるように反復演算されるため、駆動中の剛性を任意に指定することはできない。しかし、手先の目標位置で手先剛性を制御することは可能である。本第３実施形態では、図１２に示すように、関節剛性指令値の下限値を可変させ、手先の目標位置で、式（４９）を満たすように剛性の制御を行う。

（２．４）軌道設計
本第３実施形態では、目標軌道を手先が図８のｙ軸上を正方向に駆動するように設定する。さらに上記第１実施形態と同様に加速区間−等速区間−減速区間を有するものとする。関節角度に対する目標軌道ｒ_ａ１，ｒ_ａ２は、手先軌道から逆運動学により求める。本第３実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。

（３）シミュレーション
前節の制御系を用いたシミュレーションを行う。第１リンク２０１と第２リンク２０２の物理パラメータは同一とする。リンク長さを０．２ｍ、リンクの慣性モーメントをＩ_１＝Ｉ_２＝１．３×１０^−３ｋｇｍ^２、モーメントアーム径を０．０５ｍ、弾性、粘性定数をｋ＝１２，ｂ＝０．００３とする。目標軌道は、等速区間の開始時間ｔ_ａ＝０．２７４７秒、等速区間の終了時間ｔ_ｂ＝０．４７４６秒、位置決め終了時間ｔ_ｆｉｎ＝０．７５秒とする。また、関節トルクが作用しないときの弾性力による中立姿勢角度θ_ｃ１，θ_ｃ２は、手先軌道の中間地点における関節角度とし、θ_ｃ１＝２９．７ｄｅｇ、θ_ｃ２＝１２０．７ｄｅｇとする。さらに、本第１実施形態では、上記第２実施形態と同様に、２自由度制御系のモデルの同定誤差に対する検証を行う。そのため、フィードフォワード入力の生成では慣性モーメントをＩ_１，Ｉ_２とするが、シミュレーションではリンクの慣性モーメントをＩ_１′＝１．１Ｉ_１，Ｉ２′＝１．０５Ｉ_２とする。

図１３（ａ）及び図１３（ｂ）に第１、第２リンクの関節角度θ_１，θ_２を実線で、目標軌道ｒ_ａ１，ｒ_ａ２を破線で示す。図１４（ａ）〜図１４（ｃ）に駆動力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３の応答を実線で、駆動力指令値ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３の応答を破線で示す。さらに、図１５（ａ）〜図１５（ｃ）に駆動力指令値ｕ_ｅｎ，ｕ_ｆｎ（ｎ＝１，２，３）の差を実線で、和を破線で、Ｔ_ｎ／ｒ（ｎ＝１，２，３）の応答を一点鎖線で示す。また、図１６にＴ_ＦＢ１，Ｔ_ＦＢ２，Ｔ_ＦＢ３をそれぞれ実線、破線、一点鎖線で示す。

図１３（ａ）及び図１３（ｂ）より実線で示す関節角度は破線で示す軌道と重なっており、リンクは目標軌道に追従していることがわかる。上記第２実施形態と同様、図１６に示すように、フィードバック制御入力がモデル誤差を補償しているためである。図１４（ａ）より、駆動力指令値ｕ_ｅ１，ｕ_ｆ１は２自由度制御系の制御入力Ｔ_１を、式（４７）と式（４８）に示した条件で配分されているため、人工筋肉の収縮力は正の値を取るという特性を満たしていることがわかる。図１４（ｂ）及び図１４（ｃ）より、駆動力指令値ｕ_ｅ２，ｕ_ｆ２，ｕ_ｅ３，ｕ_ｆ３も同様に正の値を取っていることがわかる。

図１５（ａ）において、実線で示すｕ_ｆ１−ｕ_ｅ１は、一点鎖線で示すＴ_１／ｒと重なっており、式（２８）の条件を満たしている。同様に、図１５（ｂ）及び図１５（ｃ）において、実線で示すＵ_２，Ｕ_３は、一点鎖線で示すＴ_２／ｒ，Ｔ_３／ｒと重なっている。また図１５（ａ）〜図１５（ｃ）において、破線で示すｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１，ｕ_ｆ２＋ｕ_ｅ２＝Ｕ_２，ｕ_ｆ３＋ｕ_ｅ３＝Ｕ_３は、図１２に設定した０〜０．６２５秒のＶ_ｎ（ｎ＝１，２，３）が０に近い区間では、実線で示すＴ′_１／ｒの絶対値に近接している。これから、上記第１及び第２実施形態と同様に、式（４７）の制約を満たす範囲で、常に最小値を取っていることがわかる。さらに、Ｖ_ｎが増加する０．６５秒以降では、各拮抗対の剛性はＴ′_１／ｒ（ｎ＝１，２，３）の絶対値に対して大きな値へと移行している。これより、手先のスティフネス楕円は目標軌道において、式（４９）の条件を満たすように制御されていることがわかる。本第３実施形態の提案手法により、モデルに同定誤差などが存在する場合でも、最小の収縮力で目標軌道への追従を実現しながら、目標位置では３対６筋の弾性により手先の剛性を制御することが可能であることがわかる。

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

１００…ロボット装置、１０１…リンク、１０５…関節、１５０…制御部、１５１…トルク指令値演算処理、１５２…変更演算処理、１５３…反復処理、１５５…駆動力指令値演算処理

Claims (12)

1. 基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させるロボット装置の制御方法において、
   前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算工程と、
   前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算工程と、
   前記差分が所定値以下に収束するまで、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行う反復工程と、
   前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算工程と、を備えたことを特徴とするロボット装置の制御方法。
2. 前記関節の角度と前記目標軌道との差分を補償する補正トルク指令値を演算する補正トルク指令値演算工程を備え、
   前記駆動力指令値演算工程では、前記各駆動力指令値の演算に用いる前記トルク指令値として、前記トルク指令値演算工程で求めた前記トルク指令値に前記補正トルク指令値演算工程で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とする請求項１に記載のロボット装置の制御方法。
3. 前記反復工程では、前記関節剛性指令値が、前記トルク指令値の絶対値を前記モーメントアーム径で割った値よりも大きい値となる関係を満たすように、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行うことを特徴とする請求項１又は２に記載のロボット装置の制御方法。
4. 前記反復工程により、前記関節剛性指令値及び前記トルク指令値を最小の値とすることを特徴とする請求項１乃至３のいずれか１項に記載のロボット装置の制御方法。
5. 前記リンクが第１リンクであり、
   前記関節が第１関節であり、
   前記ロボット装置が、前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置の制御方法。
6. 前記ロボット装置が、前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
   前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項５に記載のロボット装置の制御方法。
7. 基体に関節を介して旋回可能に連結されたリンクと、
   前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、
   前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させる制御部と、を備え、
   前記制御部は、
   前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算処理と、
   前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算処理と、
   前記差分が所定値以下に収束するまで、前記トルク指令値演算処理及び前記変更演算処理の演算を反復して行う反復処理と、
   前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算処理と、を実行することを特徴とするロボット装置。
8. 前記制御部は、前記関節の角度と前記目標軌道との差分を補償する補正トルク指令値を演算する補正トルク指令値演算処理を実行し、前記駆動力指令値演算処理では、前記各駆動力指令値の演算に用いる前記トルク指令値として、前記トルク指令値演算処理で求めた前記トルク指令値に前記補正トルク指令値演算処理で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とする請求項７に記載のロボット装置。
9. 前記制御部は、前記反復処理において、前記関節剛性指令値が、前記トルク指令値の絶対値を前記モーメントアーム径で割った値よりも大きい値となる関係を満たすように、前記トルク指令値演算処理及び前記変更演算処理の演算を反復して行うことを特徴とする請求項７又は８に記載のロボット装置。
10. 前記制御部は、前記反復処理において、前記関節剛性指令値及び前記トルク指令値を最小の値とすることを特徴とする請求項７乃至９のいずれか１項に記載のロボット装置。
11. 前記リンクが第１リンクであり、
    前記関節が第１関節であり、
    前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項７乃至１０のいずれか１項に記載のロボット装置。
12. 前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
    前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項１１に記載のロボット装置。

Images