(57)【特許請求の範囲】
【請求項１】
基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させるロボット装置の制御方法において、
前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算工程と、
前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算工程と、
前記差分が所定値以下に収束するまで、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行う反復工程と、
前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算工程と、を備えたことを特徴とするロボット装置の制御方法。
【請求項２】
前記関節の角度と前記目標軌道との差分を補償する補正トルク指令値を演算する補正トルク指令値演算工程を備え、
前記駆動力指令値演算工程では、前記各駆動力指令値の演算に用いる前記トルク指令値として、前記トルク指令値演算工程で求めた前記トルク指令値に前記補正トルク指令値演算工程で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とする請求項１に記載のロボット装置の制御方法。
【請求項３】
前記反復工程では、前記関節剛性指令値が、前記トルク指令値の絶対値を前記モーメントアーム径で割った値よりも大きい値となる関係を満たすように、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行うことを特徴とする請求項１又は２に記載のロボット装置の制御方法。
【請求項４】
前記反復工程により、前記関節剛性指令値及び前記トルク指令値を最小の値とすることを特徴とする請求項１乃至３のいずれか１項に記載のロボット装置の制御方法。
【請求項５】
前記リンクが第１リンクであり、
前記関節が第１関節であり、
前記ロボット装置が、前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置の制御方法。
【請求項６】
前記ロボット装置が、前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項５に記載のロボット装置の制御方法。
【請求項７】
基体に関節を介して旋回可能に連結されたリンクと、
前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、
前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させる制御部と、を備え、
前記制御部は、
前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算処理と、
前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算処理と、
前記差分が所定値以下に収束するまで、前記トルク指令値演算処理及び前記変更演算処理の演算を反復して行う反復処理と、
前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算処理と、を実行することを特徴とするロボット装置。
【請求項８】
前記制御部は、前記関節の角度と前記目標軌道との差分を補償する補正トルク指令値を演算する補正トルク指令値演算処理を実行し、前記駆動力指令値演算処理では、前記各駆動力指令値の演算に用いる前記トルク指令値として、前記トルク指令値演算処理で求めた前記トルク指令値に前記補正トルク指令値演算処理で求めた前記補正トルク指令値を加算補正した結果を用いることを特徴とする請求項７に記載のロボット装置。
【請求項９】
前記制御部は、前記反復処理において、前記関節剛性指令値が、前記トルク指令値の絶対値を前記モーメントアーム径で割った値よりも大きい値となる関係を満たすように、前記トルク指令値演算処理及び前記変更演算処理の演算を反復して行うことを特徴とする請求項７又は８に記載のロボット装置。
【請求項１０】
前記制御部は、前記反復処理において、前記関節剛性指令値及び前記トルク指令値を最小の値とすることを特徴とする請求項７乃至９のいずれか１項に記載のロボット装置。
【請求項１１】
前記リンクが第１リンクであり、
前記関節が第１関節であり、
前記第１リンクに対して第２関節を介して旋回可能に連結された第２リンクを更に有していることを特徴とする請求項７乃至１０のいずれか１項に記載のロボット装置。
【請求項１２】
前記第１リンクに対して前記第２リンクを互いに反対方向に駆動する一対の一関節駆動型のアクチュエータと、
前記第１リンクと前記第２リンクとを同時に互いに反対方向に駆動する一対の二関節駆動型のアクチュエータと、を更に有することを特徴とする請求項１１に記載のロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、手先が対象物に柔軟に接触できるロボット装置の制御方法及びロボット装置に関するものである。好適には、作業ロボットや脚式移動ロボットに適用される。
【背景技術】
【０００２】
マニピュレータの制御方法において、手先が対象物に柔軟に接触できることが重要になってきている。これを産業ロボットに応用すれば、ロボット装置と人間との共同作業が実現でき、手先の柔軟性の方向を制御することにより部品の嵌め合い作業等が容易になる。また、これを脚式移動ロボットに応用すれば、地面に柔らかく接地することで胴体に対する衝撃を和らげることができ、段差を吸収することで不整地を安定に歩行することを可能となる。
【０００３】
手先の柔軟性の制御を実現するために、手先に力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉の中でも、特にＭｃＫｉｂｂｅｎ型人工筋肉に代表される空気圧ゴム人工筋肉は、粘弾性特性が筋に類似している。そこで、マニピュレータに配置されている人工筋肉アクチュエータの柔らかさを制御することで、任意の手先の柔軟性で対象物に接触することができる。しかし、人工筋肉アクチュエータは、粘弾性特性に非線系性を有している、収縮方向のみに力を発生するため拮抗配置して制御を行う必要がある、などの理由で制御性に難があることが知られている。
【０００４】
これに対し、筋の粘弾性特性を含むマニピュレータのモデルと、修正値計算部を用いて関節角度と手先の柔軟性を同時制御するためのフィードフォワード入力を生成する技術が開示されている（特許文献１参照）。特許文献１では、モデルを用いて制御入力が与えられた時の関節角度と人工筋肉アクチュエータの粘弾性係数を出力し、関節角度を目標値と比較する。そして、関節角度と目標値との誤差を修正値計算部に逆伝播させ、フィードフォワード入力を修正する。修正されたフィードフォワード入力を再びモデルに与える操作を繰り返し、徐々にフィードフォワード入力を求めている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３４３６３２０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記特許文献１では、フィードフォワード制御により関節角度と手先の柔軟性の同時制御を行っている。しかし、人工筋肉アクチュエータを最小の制御入力で拮抗駆動するための研究例はこれまで見られない。制御入力を最小化するには、関節角度の目標軌道に対応する手先柔軟性を考慮し、それを制御入力と同時に最適化する必要がある。しかし、上記特許文献１のフィードフォワード制御は、手先柔軟性の目標剛性が予め与えており、それを最適化するアルゴリズムは含まれていない。
【０００７】
そこで、本発明は、関節角度の軌道制御において、トルク指令値と同時に関節剛性指令値の最適化を行い、最小の駆動力指令値でリンクの拮抗駆動を行うことを目的とするものである。
【課題を解決するための手段】
【０００８】
本発明は、基体に関節を介して旋回可能に連結されたリンクと、前記基体に対して互いに反対方向に前記リンクを引っ張る駆動力を発生して、駆動力の差により前記関節にトルクを付与すると共に、駆動力の和により前記関節に剛性を付与する一対のアクチュエータと、を備え、前記各アクチュエータの各駆動力指令値を求めて、前記各アクチュエータに発生させる駆動力が前記各駆動力指令値となるように、前記各アクチュエータを制御することにより、前記関節の関節角度を目標軌道に追従させるロボット装置の制御方法において、前記リンクの逆動力学により、前記目標軌道、前記目標軌道の角速度、前記目標軌道の角加速度、前記各駆動力指令値の合計値を示す関節剛性指令値を用いて、前記関節に必要なトルクを示すトルク指令値を演算するトルク指令値演算工程と、前記関節剛性指令値と、前記トルク指令値の絶対値を前記リンクのモーメントアーム径で割った値との差分を演算し、前記関節剛性指令値から、前記差分に０よりも大きく１以下の係数を乗じた値を引く演算を行うことで、前記関節剛性指令値を変更する変更演算工程と、前記差分が所定値以下に収束するまで、前記トルク指令値演算工程及び前記変更演算工程の演算を反復して行う反復工程と、前記差分が所定値以下に収束した場合、前記関節剛性指令値及び前記トルク指令値を用いて、前記各駆動力指令値を演算する駆動力指令値演算工程と、を備えたことを特徴とする。
【発明の効果】
【０００９】
本発明によれば、関節の剛性と関節の軌道とを同時に最適化する反復演算アルゴリズムにより、目標軌道に対して最小の駆動力指令値でアクチュエータを拮抗駆動するフィードフォワード入力を生成することが可能となる。駆動力指令値を最小にすることで、例えばＭｃＫｉｂｂｅｎ型人工筋肉では消費する空気の量を最小にすることができる。これにより、タンク等の搭載型の空気源を用いるロボット装置では稼働時間を延長することが可能となる。
【図面の簡単な説明】
【００１０】
【図１】筋の粘弾性モデルを示す図である。
【図２】本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。
【図３】本発明の第１実施形態における制御部のブロック線図である。
【図４】本発明の第１実施形態における目標軌道を示す図である。
【図５】本発明の第１実施形態におけるシミュレーション結果を示す図である。
【図６】本発明の第２実施形態における制御部のブロック線図である。
【図７】本発明の第２実施形態におけるシミュレーション結果を示す図である。
【図８】本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。
【図９】本発明の第３実施形態における制御部のブロック線図である。
【図１０】本発明の第３実施形態における制御部のフィードフォワード制御のブロック線図である。
【図１１】本発明の第３実施形態におけるスティフネス楕円を説明する図である。
【図１２】本発明の第３実施形態における軌道設計を示す図である。
【図１３】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１４】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１５】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【図１６】本発明の第３実施形態におけるシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１１】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１２】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置に適用されるアクチュエータの粘弾性モデルを示す図である。本第１実施形態では、ロボット装置として、空気圧式の人工筋肉アクチュエータを用いたマニピュレータの関節角度と関節の剛性の同時制御を例として説明する。
【００１３】
（１）モデリング
人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。人工筋肉アクチュエータは、図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。ここで、力発生要素の収縮力をｕ、収縮方向を正とする筋の収縮量をｘとする。収縮速度は、以下の式となる。
【００１４】
【数１】
0005930754.tif 000002
【００１５】
また、ｋを弾性力定数、ｂを粘性力定数、Ｆを筋収縮力とする。このとき、筋の粘弾性特性は、
【００１６】
【数２】
0005930754.tif 000003
とモデル化され、筋収縮力の弾性力、粘性力は、力発生要素の収縮力ｕに比例するという非線形要素を有する点が特徴的である。
【００１７】
次に、本第１実施形態のロボット装置を図２に示す。図２に示すロボット装置１００は、いわゆる１リンクマニピュレータであり、リンク１０１と、一対のアクチュエータｅ１，ｆ１からなる駆動部１２０と、基体であるベース部材１０３及びベース部材１０３から延びる棒状のアーム部材１０４とを備えている。リンク１０１は、アーム部材１０４に関節１０５を介して旋回可能に連結されている。
【００１８】
なお、本第１実施形態では、この基体として、図２に示すように、ベース部材１０３及びアーム部材１０４で構成される場合について説明するが、これに限定するものではない。例えば、基体が、脚式移動ロボットの胴体や作業ロボットの台座部、リンク１０１に連結される別のリンク等であってもよい。
【００１９】
各アクチュエータｅ１，ｆ１は、一関節筋アクチュエータであり、一端が基体であるベース部材１０３に接続され、他端がリンク１０１の基端１０１ａに接続されて、駆動力（収縮力）の差によりリンク１０１を揺動させるように拮抗配置されている。つまり、アクチュエータｅ１，ｆ１は、リンク１０１を挟んでリンク１０１の両側に対称配置されている。
【００２０】
また、ロボット装置１００は、各アクチュエータｅ１，ｆ１の駆動力を各駆動力指令値により設定して、リンク１０１の動作を制御する制御部１５０を備えている。
【００２１】
各アクチュエータｅ１，ｆ１は、図１に示す空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは収縮方向にのみ力を発生するため、関節１０５を任意の角度に位置決めするために、図２に示すように拮抗して配置されている。つまり、各アクチュエータｅ１，ｆ１は、ベース部材に対して互いに反対方向にリンク１０１を引っ張る駆動力を発生する。各アクチュエータｅ１，ｆ１は、人工筋肉からなる一関節筋アクチュエータであるので、収縮することにより駆動力が発生する。したがって、アクチュエータｅ１，ｆ１の収縮力が駆動力となる。
【００２２】
拮抗配置されたアクチュエータｅ１，ｆ１の力発生要素の駆動力（収縮力）をそれぞれｕｅ１，ｕｆ１、アーム部材１０４に対するリンク１０１の角度、つまり関節１０５の角度をθ、リンク１０１の慣性モーメントをＩとする。また、リンク１０１のモーメントアーム径、つまりリンク１０１の旋回中心点とリンク１０１におけるアクチュエータｅ１，ｆ１の接続点との長さをｒとする。運動方程式は、以下の式（２）となる。
【００２３】
【数３】
0005930754.tif 000004
【００２４】
式（２）の右辺第一項における収縮力ｕｅ１，ｕｆ１の差が関節１０５に回転トルクを与え、右辺第二、第三項における収縮力ｕｅ１，ｕｆ１の和が関節１０５に対する剛性および粘性を変動させることがわかる。つまり、アクチュエータｅ１，ｆ１は、駆動力の差により関節１０５にトルクを付与すると共に、駆動力の和により関節１０５に剛性を付与する。
【００２５】
（２）制御系設計
制御部１５０は、トルク指令値演算処理１５１、変更演算処理１５２、反復処理１５３、駆動力指令値演算処理１５５を実行する。本第１実施形態では、駆動力指令値ｕｅ１，ｕｆ１を算出する駆動力指令値演算処理１５５の実行にフィードフォワード制御入力を与えることにより、関節１０５の角度を目標軌道に追従させる。このとき、収縮力（駆動力）が最小となるフィードフォワード入力を求める。関節１０５の目標軌道をｒａとする。つまり、制御部１５０は、関節１０５の角度θを目標軌道ｒａに追従させるのに必要な各アクチュエータｅ１，ｆ１の各駆動力指令値ｕｅ１，ｕｆ１を求める。そして、制御部１５０は、各アクチュエータｅ１，ｆ１に発生させる駆動力が駆動力指令値ｕｅ１，ｕｆ１となるように、各アクチュエータｅ１，ｆ１を制御する。
【００２６】
（２．１）フィードフォワード制御系設計
本節では、各アクチュエータｅ１，ｆ１に与えるフィードフォワード入力の導出方法について述べる。まず、以下のようにモーメントアーム径ｒ倍される駆動力指令値ｕｅ１，ｕｆ１の差をＴ１、駆動力指令値ｕｅ１，ｕｆ１の和（合計値）を示す関節剛性指令値をＵ１とする。
（ｕｆ１−ｕｅ１）ｒ＝Ｔ１          （３）
ｕｆ１＋ｕｅ１＝Ｕ１                （４）
【００２７】
また、人工筋肉アクチュエータｅ１，ｆ１は収縮方向にのみ力を発生するため、以下の条件を同時に満たさなければならない。
ｕｆ１＞０，ｕｅ１＞０          （５）
【００２８】
ここで、式（３）及び式（４）を式（２）に代入すると、
【００２９】
【数４】
0005930754.tif 000005
と表される。Ｔ１が関節１０５にトルクを与え、Ｕ１が関節１０５に対する剛性および粘性を増減させることがわかる。
【００３０】
そこで、トルク指令値Ｔ１，関節剛性指令値Ｕ１を同時に満たす駆動力指令値を求めるには、式（３）と式（４）をｕｅ１，ｕｆ１について解き、
【００３１】
【数５】
0005930754.tif 000006
と決定すればよい。式（７）より、駆動力指令値ｕｆ１，ｕｅ１を最小化して任意のトルク指令値Ｔ１を与えるためには、関節剛性指令値Ｕ１を最小化すればよいことがわかる。しかし、式（５）と式（７）より、
Ｔ１／ｒ＜Ｕ１，Ｔ１／ｒ＞−Ｕ１          （８）
という条件が得られる。これより、人工筋肉アクチュエータｅ１，ｆ１に収縮方向のみの駆動力指令値ｕｆ１，ｕｅ１を与えるために、関節剛性指令値Ｕ１はトルク指令値Ｔ１を用いて、
｜Ｔ１｜／ｒ＜Ｕ１          （９）
という条件を満たす必要があることがわかる。
【００３２】
関節１０５の角度を任意の目標軌道ｒａに追従させるためのトルク指令値Ｔ１は、目標軌道ｒａの角速度、角加速度をそれぞれ、
【００３３】
【数６】
0005930754.tif 000007
あるいは、ｒａ’，ｒａ’’と表す。トルク指令値Ｔ１は、リンク１０１の逆動力学により、以下の式（１０）と表せる。
【００３４】
【数７】
0005930754.tif 000008
【００３５】
式（１０）には、関節剛性指令値Ｕ１が含まれるが、式（９）からわかるように、関節剛性指令値Ｕ１はトルク指令値Ｔ１で制約される指令値である。そこで、本第１実施形態では、式（９）と式（１０）を同時に満たすトルク指令値Ｔ１，関節剛性指令値Ｕ１を反復演算アルゴリズムで求める。
【００３６】
図３に本第１実施形態における制御部１５０のブロック線図を示す。Ｉｔｅｒａｔｉｏｎ  Ａｌｇｏｒｉｔｈｍで示したブロックで、変更演算処理１５２を行っている。
【００３７】
本第１実施形態では、反復回数をｉとして、ｉ回の反復演算による、トルク指令値Ｔ１と関節剛性指令値Ｕ１を、
Ｔ１［ｉ］，Ｕ１［ｉ］          （１１）
と表記する。
【００３８】
制御部１５０は、トルク指令値演算処理１５１として、リンク１０１の逆動力学に基づき、目標軌道ｒａ、目標軌道の角速度ｒａ’、目標軌道の角加速度ｒａ’’、関節剛性指令値Ｕ１［ｉ］を用いて、関節に必要なトルクを示すトルク指令値Ｔ１［ｉ］を演算する。つまり、制御部１５０は、ｉ回目の反復によるトルク指令値Ｔ１［ｉ］を、式（１２）を用いて演算する（トルク指令値演算工程）。
【００３９】
【数８】
0005930754.tif 000009
【００４０】
次に、制御部１５０は、変更演算処理１５２として、関節剛性指令値Ｕ１［ｉ］と、トルク指令値Ｔ１［ｉ］の絶対値をリンク１０１のモーメントアーム径ｒで割った値｜Ｔ１［ｉ］｜／ｒとの差分を演算する。この差分をＥ１［ｉ］とすると、制御部１５０は、以下の式（１３）により差分Ｅ１［ｉ］を演算する。
Ｅ１［ｉ］＝Ｕ１［ｉ］−｜Ｔ１［ｉ］｜／ｒ          （１３）
【００４１】
次に、制御部１５０は、変更演算処理１５２として、関節剛性指令値Ｕ１［ｉ］から、差分Ｅ１［ｉ］に０よりも大きく１以下の収束係数γを乗じた値を引く演算を行うことで、関節剛性指令値Ｕ１［ｉ］を変更する（変更演算工程）。つまり、制御部１５０は、反復演算アルゴリズムの収束係数γを０＜γ≦１として、ｉ＋１回目の関節剛性指令値Ｕ１［ｉ＋１］を以下の式（１４）により求める。
Ｕ１［ｉ＋１］＝Ｕ１［ｉ］−γＥ１［ｉ］          （１４）
【００４２】
制御部１５０は、反復処理１５３として、式（１２）〜式（１４）の演算を、差分Ｅ１［ｉ］が所定値以下（例えば０）に収束するまでの所定回数ｍ、反復して行う（反復工程）。
【００４３】
差分Ｅ１［ｉ］が０に収束しているので、関節剛性指令値Ｕ１［ｍ］は式（９）を満たす最小の値であり、これを用いるトルク指令値Ｔ１［ｍ］は最小の値をとる。これより、駆動力指令値ｕｅ１，ｕｆ１を最小とするフィードフォワード入力は、式（７）より、
【００４４】
【数９】
0005930754.tif 000010
と導出される。
【００４５】
したがって、制御部１５０は、駆動力指令値演算処理１５５として、反復処理１５３後、関節剛性指令値Ｕ１［ｍ］及びトルク指令値Ｔ１［ｍ］を用いて、各駆動力指令値ｕｆ１，ｕｅ１を演算する（駆動力指令値演算工程）。
【００４６】
さらに、関節剛性指令値に下限値を設けるには、剛性下限値をＶ１として、式（１３）に代わり次式を用いることで可能である。
Ｅ１［ｉ］＝Ｕ１［ｉ］−｜Ｔ１［ｉ］｜／ｒ−Ｖ１          （１６）
【００４７】
式（１６）により、例えば、目標軌道の到達時間におけるＶ１を調整すれば、マニピュレータの到達角度における関節剛性の制御が可能となる。
【００４８】
なお、目標軌道ｒａの到達角度をθｄとすると、リンク整定に必要なトルク指令値Ｔ１ｄは、
Ｕ１ｋｒ２θｄ≦Ｔ１ｄ          （１７）
となるが、同時に式（９）を満たす必要がある。そのため、弾性定数ｋ、プーリ径ｒは以下の条件
ｋｒθｄ＜１          （１８）
を満たす必要がある。
【００４９】
以上の導出は、筋の弾性力がθ＝０ｄｅｇからの角度に比例して発生することを仮定していた。任意の角度を筋の弾性力発生の基準となる角度（以下、中立角度と記す）としてフィードフォワード制御入力を生成するためには、θｃを中立角度とすると、式（１２）に代わり、以下の式（１９）とすることで求まる。
【００５０】
【数１０】
0005930754.tif 000011
【００５１】
（２．２）軌道設計
次に、関節１０５の目標軌道ｒａの設計方法を示す。本実施形態では、目標軌道ｒａは図４に示すような、加速区間−等速区間−減速区間を有する軌道を用いる。ここで、ｒａＳ，ｒａＦはそれぞれ、軌道の初期角度、目標角度である。また、ｔａ，ｔｂ，ｔｆｉｎはそれぞれ、等速区間の開始時間、等速区間の終了時間、位置決め終了時間である。本第１実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。
【００５２】
（３）シミュレーション
前節の制御系を用いるシミュレーションを行う。リンクの慣性モーメントをＩ＝８．３×１０−２ｋｇｍ２、モーメントアーム径をｒ＝０．１ｍ、弾性、粘性定数をｋ＝２５，ｂ＝３とする。目標軌道ｒａは、初期角度ｒａＳ＝−２０ｄｅｇ、目標角度ｒａＦ＝２０ｄｅｇ、等速区間の開始時間ｔａ＝０．４秒、等速区間の終了時間ｔｂ＝０．６秒、位置決め終了時間ｔｆｉｎ＝１秒とする。関節剛性指令値Ｕ１は２．１に示した反復演算アルゴリズムにより、式（９）に示した制約を満たすように導出されている。なお、関節剛性指令の下限値はＶ１＝０．０２とし、γ＝１として反復演算を行っている。
【００５３】
図５（ａ）に関節の角度θと目標軌道ｒａを、それぞれ実線、破線で、図５（ｂ）に駆動力指令値ｕｅ１，ｕｆ１の応答をそれぞれ実線、破線で示す。さらに、図５（ｃ）に駆動力指令値ｕｅ１，ｕｆ１の差を実線で、和を破線で、Ｔ１／ｒの応答を一点鎖線で示す。
【００５４】
図５（ａ）より実線で示す関節１０５の角度θは、破線で示す軌道ｒａと重なっており、関節１０５は目標軌道ｒａに追従していることがわかる。図５（ｂ）より、駆動力指令値ｕｅ１，ｕｆ１はフィードフォワード制御系の制御入力（トルク指令値）Ｔ１が式（７）と式（９）に示した条件で配分されているため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。図５（ｃ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ１／ｒと重なっており、式（３）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は、実線で示すＴ１／ｒの絶対値に対してＶ１の間隔をもって近接しており、常に最小値を取っていることがわかる。本第１実施形態の提案手法により、目標軌道ｒａへの追従を実現しながら、最小の駆動力指令値ｕｅ１，ｕｆ１での拮抗駆動が可能であることがわかる。駆動力指令値ｕｅ１，ｕｆ１を最小にすることで、例えばＭｃＫｉｂｂｅｎ型人工筋肉では消費する空気の量を最小にすることができる。これにより、タンク等の搭載型の空気源を用いるロボット装置では稼働時間を延長することが可能となる。
【００５５】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。本第２実施形態では、制御部の制御動作が上記第１実施形態の制御部の制御動作と異なるものである。なお、ロボット装置本体の構成について、上記第１実施形態で説明した図１のロボット装置本体の構成と同様の部分については、同一符号を付して詳細な説明を省略する。
【００５６】
本第２実施形態では、上記第１実施形態で示したフィードフォワード制御系とフィードバック制御系を併合する２自由度制御系を導出する。フィードバック制御系は、目標軌道と反復演算アルゴリズムで求めた関節剛性指令値に同時に追従可能とするように導出する。フィードフォワード制御系のみでは、リンク１０１の慣性モーメントなどにパラメータ同定誤差が生ずると制御性能が劣化してしまう。しかし、フィードバック制御系との２自由度制御系を構成することにより、モデル誤差が存在しても目標軌道と関節剛性指令値の同時制御が可能であることを示す。
【００５７】
（１）モデリング
本第２実施形態で用いるモデルは上記第１実施形態と同じとする。
【００５８】
（２）制御系設計
フィードフォワード制御系の導出は上記第１実施形態と同様である。本第２実施形態では、上記第１実施形態の反復演算アルゴリズムにより導出される関節トルク指令ＴＦＦ［ｍ］をＴＦＦと記す。
【００５９】
図６に本第２実施形態に係る制御部１５０Ａのブロック線図である。図６では、図３で示したフィードフォワード制御系の破線で囲まれた部分を、ＫＦＦＷとして示している。
【００６０】
制御部１５０Ａは、フィードフォワード制御系ＫＦＦＷの他、更に、補正トルク指令値演算処理１５４としてのＰＩＤ制御処理ＫＰＩＤ１を実行する機能を有している。
【００６１】
制御部１５０Ａは、ＰＩＤ制御処理ＫＰＩＤ１として、関節１０５の角度θと目標軌道ｒａとの差分を補償する補正トルク指令値としての制御入力トルクＴＦＢを演算する（補正トルク指令値演算工程）。
【００６２】
ここで、式（２）より、
（ｕｆ１−ｕｅ１）ｒ＝ＴＦＦ＋ＴＦＢ＝Ｔ′１          （２０）
を満たすようにｕｅ１，ｕｆ１を決定すれば、関節１０５にフィードフォワード制御トルクＴＦＦとフィードバック制御トルクＴＦＢの和であるＴ′１を与えることができる。トルク指令値Ｔ′１は、フィードフォワード制御系ＫＦＦＷのトルク指令値演算処理１５１（図３参照）で求めたトルク指令値ＴＦＦにＰＩＤ制御処理ＫＰＩＤ１で求めた制御入力トルクＴＦＢを加算補正した演算結果である。
【００６３】
したがって、本第２実施形態では、駆動力指令値演算処理１５５を実行する際に、各駆動力指令値の算出に用いるトルク指令値として、トルク指令値Ｔ′１を用いる。
【００６４】
ところで、上記第１実施形態と同様に、同時に関節１０５の剛性に関する条件
Ｕ１＝ｕｆ１＋ｕｅ１＝Ｕ１［ｍ］          （２１）
を満たすように、駆動力指令値ｕｅ１，ｕｆ１を決定する必要がある。
【００６５】
トルク指令値Ｔ′１と関節剛性指令値Ｕ１を同時に満たすには、式（２０）と式（４）を駆動力指令値ｕｅ１，ｕｆ１について解き、
【００６６】
【数１１】
0005930754.tif 000012
と決定すればよい。しかし、人工筋肉アクチュエータｅ１，ｆ１は収縮方向にのみ力を発生するため、上記第１実施形態と同様に、Ｔ′１が、
−Ｕ１ｒ＜Ｔ′１＜Ｕ１ｒ          （２３）
という条件を満たす必要がある。これにより、関節１０５の剛性が関節剛性指令値Ｕ１となると同時に、トルク指令値Ｔ′１により目標軌道ｒａに追従制御される。式（２３）を実現するには、制御部１５０Ａは、図６のブロック線図に示すように、
｜Ｔ′１｜＜Ｕ１ｒ          （２４）
と制御入力を制約する制約処理１５６を実行すればよい。もしくは、ＰＩＤ制御処理ＫＰＩＤ１のゲインを変動させ、式（２３）の範囲に制御入力の大きさを収めるなどの方法がある。
【００６７】
上記第１実施形態において、剛性下限値Ｖ１を設定することにより、関節剛性を制御することが可能であることを示した。しかし、フィードバック制御系のゲインが高い場合には、それによる関節１０５の剛性が支配的になってしまう。
【００６８】
そこで、制御部１５０Ａは、図６に示すように、接触ゲインＧｔを導入し、接触が発生した場合は、Ｇｔ＝０とすることでフィードバック系を遮断する。これにより、手先が人や物に接触したことを検知するなどの任意の時刻で、速やかにリンク１０１の剛性を関節剛性指令値Ｕ１へと切り替えることができる。
【００６９】
本第２実施形態では、ＰＩＤ制御処理ＫＰＩＤ１に用いる伝達関数を、
【００７０】
【数１２】
0005930754.tif 000013
としている。
【００７１】
（３）シミュレーション
前節で導出した制御系を用いてシミュレーションを行った。リンク１０１のパラメータと目標軌道ｒａは上記第１実施形態と同様とする。本第２実施形態では、モデルの同定誤差に対する制御性能の検証を行うため、リンク１０１の慣性モーメントをＩ′＝１．０５Ｉとし、フィードフォワード制御入力生成時のモデルに対して誤差を有するモデルを用いてシミュレーションを行った。また本第２実施形態では、上記第１実施形態のように関節剛性指令の下限値を小さくすると、フィードバック制御系の制約の範囲が小さくなることで、ワインドアップ現象を発生しやすくなってしまう。そこで、Ｖ１＝０．１としてフィードフォワード入力を導出している。
【００７２】
図７（ａ）に関節１０５の角度θと目標軌道ｒａを、それぞれ実線、破線で、図７（ｂ）に駆動力指令値ｕｅ１，ｕｆ１の応答をそれぞれ実線、破線で示す。さらに、図７（ｃ）に駆動力指令値ｕｅ１，ｕｆ１の差を実線で、和を破線で、Ｔ′１／ｒの応答を一点鎖線で示す。また、図７（ｄ）にＴＦＦ，ＴＦＢをそれぞれ実線、破線で示す。
【００７３】
図７（ａ）より、実線で示す関節１０５の角度θは破線で示す目標軌道ｒａと重なっており、関節１０５は目標軌道ｒａに追従していることがわかる。フィードフォワード制御系のみでは、モデル誤差により軌道に追従することはできない。しかし、図７（ｄ）の破線で示すように、フィードバック制御入力がモデル誤差を補償しているため、追従することを可能としている。図７（ｂ）より、駆動力指令値ｕｅ１，ｕｆ１は２自由度制御系の制御入力Ｔ′１が、式（２２）と式（２４）に示した条件で配分されているため、人工筋肉の収縮力は正の値のみを取るという特性を満たしていることがわかる。図７（ｃ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ′１／ｒと重なっており、式（２０）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１は、上記第１実施形態と同様に、実線で示すＴ′１／ｒの絶対値に近接しており、式（２４）の制約を満たす範囲で、常に最小値を取っていることがわかる。本第２実施形態で示した２自由度制御系により、モデルに同定誤差などが存在しても、目標軌道ｒａへの追従を実現しながら、最小の収縮力での拮抗駆動が可能であることがわかる。
【００７４】
［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について詳細に説明する。図８は、本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。本第３実施形態では、人工筋肉アクチュエータを用いた３対６筋を有する２リンクマニピュレータとしてのロボット装置２００の手先剛性の制御を例として説明する。
【００７５】
（１）モデリング
図８に示すロボット装置２００は、プーリ２０３と、プーリ２０３に対して旋回可能に第１関節２１１を介して連結された第１リンク２０１と、第１リンク２０１に対して旋回可能に第２関節２１２を介して連結された第２リンク２０２とを備えている。
【００７６】
第１リンク２０１は、長手部材からなり、その基端２０１ａがプーリ２０３にｘ−ｙ直交座標系の平面（以下、「作業平面」という）内で旋回可能に支持されている。第２リンク２０２は、長手部材からなり、その基端２０２ａが第１リンク２０１の先端２０１ｂに作業平面内で旋回可能に支持されている。
【００７７】
第２リンク２０２の先端（以下、「リンク先端」という）２０２ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、第１リンク２０１は、第１関節２１１と第２関節２１２との間に配置され、第１関節２１１で旋回可能に支持されており、第２リンク２０２は、第２関節２１２で旋回可能に支持されている。
【００７８】
なお、本第２実施形態では、第１リンク２０１に対しては、プーリ２０３が基体となり、第２リンク２０２に対しては、第１リンク２０１が基体となる。このプーリ２０３は、例えばロボット胴体等に設けられている。
【００７９】
ロボット装置２００は、一対の第１アクチュエータｅ１，ｆ１と、一対の第２アクチュエータｅ２，ｆ２と、一対の第３アクチュエータｅ３，ｆ３と、を備えている。各第１アクチュエータｅ１，ｆ１は、一端がプーリ２０３に接続され、他端が第１リンク２０１の長手方向中央部に接続され、駆動力の差により第１リンク２０１を旋回させるように拮抗配置されている。
【００８０】
また、各第２アクチュエータｅ２，ｆ２は、一端が第１リンク２０１の長手方向中央部に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第２リンク２０２を旋回させるように拮抗配置されている。また、各第３アクチュエータｅ３，ｆ３は、一端がプーリ２０３に接続され、他端が第２リンク２０２の基端２０２ａに接続され、駆動力の差により第１リンク２０１及び第２リンク２０２を旋回させるように拮抗配置されている。つまり、第１アクチュエータｅ１，ｆ１は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第２アクチュエータｅ２，ｆ２は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。また、第３アクチュエータｅ３，ｆ３は、第１リンク２０１を挟んで第１リンク２０１の両側に対称配置されている。
【００８１】
また、ロボット装置２００は、各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３の駆動力を各駆動力指令値により設定して、リンク２０１，２０２の動作を制御する制御部２５０を備えている。
【００８２】
第１アクチュエータｅ１，ｆ１は、第１リンク２０１を駆動する第１の一関節駆動アクチュエータである。第２アクチュエータｅ２，ｆ２は、第２リンク２０２を駆動する第２の一関節駆動アクチュエータである。また、第３アクチュエータｅ３，ｆ３は、第１リンク２０１と第２リンク２０２を同時に駆動する二関節同時駆動アクチュエータである。人の上腕部や下肢大腿部には、二関節筋とよばれる二関節同時駆動アクチュエータが存在することが知られている。人の四肢の筋配列は複雑だが、実効筋概念が導入され、３対６筋を有する２リンクモデルが提示されている。
【００８３】
各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３は、図１に示す筋の粘弾性特性を有する人工筋肉アクチュエータである。人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。筋は図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。
【００８４】
図８のアクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３について、ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）を力発生要素の駆動力を発生させる駆動力指令値とする。また、ｋｅｎ，ｋｆｎ，ｂｅｎ，ｂｆｎ（ｎ＝１，２，３）を人工筋肉アクチュエータの弾性力定数、粘性力定数とする。第１，第２リンク２０１，２０２の角度、つまり関節の角度をθ１，θ２、第１，第２リンク２０１，２０２の慣性モーメントをＩ１，Ｉ２、第１，第２リンク２０１，２０２の長さを２×ｌ１，２×ｌ２、第１，第２リンク２０１，２０２の質量をｍ１，ｍ２とする。
【００８５】
モーメントアーム径、つまり第１リンク２０１の旋回中心点とプーリ２０３におけるアクチュエータｅ１，ｆ１の接続点との長さ、及び第２リンク２０２の旋回中心点と第２リンク２０２におけるアクチュエータｅ２，ｆ２の接続点との長さをｒとする。
【００８６】
本第３実施形態では、各筋の弾性力定数，粘性力定数をいずれもｋ，ｂとすると、運動方程式は、以下の式（２６）及び式（２７）となる。
【００８７】
【数１３】
0005930754.tif 000014
【００８８】
【数１４】
0005930754.tif 000015
となる。
【００８９】
（２）制御系設計
上記第１及び第２実施形態では、収縮力（駆動力）を最小にする関節の軌道制御を行ったが、本第３実施形態においても、２自由度制御系を設計する。
【００９０】
図９に本第３実施形態の制御部２５０のブロック線図を示す。また、図１０に、フィードフォワード制御のブロック線図を示す。ＫＦＦＷで示すブロックがフィードフォワード制御系である。制御部２５０は、トルク指令値演算処理２５１、変更演算処理２５２、反復処理２５３、補正トルク指令値演算処理２５４及び駆動力指令値演算処理２５５を実行する。本第３実施形態では、制御部２５０は、関節２１１，２１２の角度を目標軌道に追従させるのに必要な各アクチュエータｅｎ，ｆｎの各駆動力指令値ｕｅｎ，ｕｆｎを求める。そして、制御部２５０は、各アクチュエータｅｎ，ｆｎに発生させる駆動力が駆動力指令値ｕｅｎ，ｕｆｎとなるように、各アクチュエータｅｎ，ｆｎを制御する。
【００９１】
（２．１）フィードフォワード制御系
上記第１実施形態と同様に、駆動力指令値の差をＴｎ、和をＵｎとおくと、
（ｕｆｎ−ｕｅｎ）ｒ＝ＴＦＦｎ＋ＴＦＢｎ＝Ｔｎ，ｎ＝１，２，３        （２８）
ｕｆｎ＋ｕｅｎ＝Ｕｎ，ｎ＝１，２，３          （２９）
と表せる。ここで、ＴＦＦｎ，ＴＦＢｎは、それぞれフィードフォワード制御系、フィードバック制御系により与えられるトルク指令値である。本節では、ＴＦＢｎ＝０としてフィードフォワード制御入力の導出方法を示す。
【００９２】
式（２６）及び式（２７）をθｖ＝［θ１  θ２］Ｔ，Ｔｖ＝［Ｔ１＋Ｔ３  Ｔ２＋Ｔ３］Ｔと定義して行列表記すると、
【００９３】
【数１５】
0005930754.tif 000016
となる。ここで、Ｋｖ，Ｃｖはそれぞれ筋の粘弾性による剛性行列、減衰行列であり、
【００９４】
【数１６】
0005930754.tif 000017
である。また、θｃｖは、関節トルクＴ１，Ｔ２が作用しないときの筋の弾性力による中立姿勢角度θｃ１，θｃ２からなるベクトルであり、
θｃｖ＝［θｃ１  θｃ２］Ｔ          （３２）
となる。関節角度θｎ（ｎ＝１，２）に対する目標軌道をｒａｎ（ｎ＝１，２）とし、
ｒａｖ＝［ｒａ１  ｒａ２］Ｔ          （３３）
と定義する。目標軌道の角速度、角加速度をそれぞれ、
【００９５】
【数１７】
0005930754.tif 000018
と表す。ここで、関節角度が目標軌道に追従するために必要なフィードフォワードトルク指令値はＴＦＦｖ＝［ＴＦＦ１＋ＴＦＦ３  ＴＦＦ２＋ＴＦＦ３］Ｔと表す。トルク指令値ＴＦＦｖはリンクの逆動力学により、
【００９６】
【数１８】
0005930754.tif 000019
と表すことができる。
【００９７】
しかし、二関節同時駆動アクチュエータを有するマニピュレータは、制御される自由度に対して、制御入力の数が冗長である。そのため、逆動力学により各関節２１１，２１２に対するトルク指令値（以下、各関節トルクと略す）は求められるが、二関節駆動アクチュエータによるトルクを含む指令ＴＦＦｎ（ｎ＝１，２，３）を一意に決定できない。そこで、式（３４）右辺から得られる関節トルク指令をＴＭＦ＝［ＴＭＦ１  ＴＭＦ２］Ｔとおき、ＴＦＦｎ（ｎ＝１，２，３）の絶対値の最大値
ｍａｘ（｜ＴＦＦｎ｜，ｎ＝１，２，３）          （３５）
が最小となるように、各関節トルク指令ＴＭＦを分配する。
【００９８】
まず、ＴＭＦとＴＦＦｎ（ｎ＝１，２，３）の関係は、式（３６）及び式（３７）となる。
ＴＦＦ１＋ＴＦＦ３＝ＴＭＦ１          （３６）
ＴＦＦ２＋ＴＦＦ３＝ＴＭＦ２          （３７）
【００９９】
ＴＭＦ１，ＴＭＦ２が同符号の場合、式（３６）及び式（３７）より、ＴＦＦ３を増減させるとＴＦＦ３の変化量だけＴＦＦ１とＴＦＦ２を増減できることがわかる。ＴＭＦ１≦ＴＭＦ２である場合、ＴＦＦ３を０から徐々に増減させ、ＴＦＦ１＝ＴＦＦ３とすればよい。このとき、ＴＦＦｎ（ｎ＝１，２，３）は、それぞれ、
【０１００】
【数１９】
0005930754.tif 000020
と求まる。一方、ＴＭＦ２＞ＴＭＦ１である場合は、ＴＦＦ２＝ＴＦＦ３とすればよく、ＴＦＦｎ（ｎ＝１，２，３）は
【０１０１】
【数２０】
0005930754.tif 000021
と求まる。
【０１０２】
ＴＭＦ１，ＴＭＦ２が異符号の場合は、例えばＴＦＦ３の増加によりＴＦＦ１の絶対値は減少するがＴＦＦ２の絶対値は増加してしまう。従って、ＴＦＦ１，ＴＦＦ２を、
｜ＴＦＦ１｜＝｜ＴＦＦ２｜          （４０）
とすればよい。また、ＴＭＦ１とＴＭＦ２が異符号で、式（４０）を満たす場合、ＴＦＦ１とＴＦＦ２は異符号となるため、式（３６）、式（３７）及び式（４０）より、
【０１０３】
【数２１】
0005930754.tif 000022
と求まる。
【０１０４】
これを用いて、上記第１実施形態と同様に、収縮方向のみに力を発生するという条件、
｜ＴＦＦｎ｜＜Ｕｎｒ，ｎ＝１，２，３          （４２）
を成立するトルク指令値ＴＦＦｎと関節剛性指令値Ｕｎとを反復演算アルゴリズムにより求める。
【０１０５】
本第３実施形態では、上記第１実施形態と同様に、反復回数をｉとして、ｉ回の反復演算による、トルク指令値ＴＦＦｎと関節剛性指令値Ｕｎを
ＴＦＦｎ［ｉ］，Ｕｎ［ｉ］，ｎ＝１，２，３          （４３）
と表記する。また、Ｕｎ［ｉ］が要素に含まれる筋の粘弾性による剛性行列、減衰行列をＫｖ［ｉ］，Ｃｖ［ｉ］と表記する。
【０１０６】
制御部２５０は、トルク指令値演算処理２５１として、リンク２０１，２０２の逆動力学により、目標軌道、目標軌道の角速度、目標軌道の角加速度、関節剛性指令値を用いて、関節２１１，２１２に必要なトルクを示すトルク指令値ＴＦＦｎ［ｉ］を演算する。
つまり、制御部２５０は、式（４４）に基づき、ｉ回目の反復によるトルク指令値ＴＦＦｎ［ｉ］を演算する（トルク指令値演算工程）。
【０１０７】
【数２２】
0005930754.tif 000023
【０１０８】
ここで、制御部２５０は、式（４４）を、式（３８）〜（４１）に示した方法により分配しＴＦＦｎ［ｉ］（ｎ＝１，２，３）を求める。
【０１０９】
次に、制御部２５０は、変更演算処理２５２として、関節剛性指令値Ｕｎ［ｉ］と、トルク指令値ＴＦＦｎ［ｉ］の絶対値をリンクのモーメントアーム径ｒで割った値｜ＴＦＦｎ［ｉ］｜／ｒとの差分を演算する。この差分をＥｎ［ｉ］とすると、制御部２５０は、以下の式（４５）により差分Ｅ１［ｉ］を演算する。
【０１１０】
なお、本第３実施形態では、関節剛性指令値Ｕｎに対する剛性下限値をＶｎとし、式（４２）の条件に対する差分Ｅｎ［ｉ］を以下のように定義している。
Ｅｎ［ｉ］＝Ｕｎ［ｉ］−｜ＴＦＦｎ［ｉ］｜／ｒ−Ｖｎ，ｎ＝１，２，３          （４５）
【０１１１】
次に、制御部２５０は、変更演算処理２５２として、関節剛性指令値Ｕｎ［ｉ］から、差分Ｅｎ［ｉ］に０よりも大きく１以下の収束係数γを乗じた値を引く演算を行うことで、関節剛性指令値Ｕｎ［ｉ］を変更する（変更演算工程）。つまり、制御部２５０は、反復演算アルゴリズムの収束係数γを０＜γ≦１として、ｉ＋１回目の関節剛性指令値Ｕｎ［ｉ＋１］を以下の式（４６）により求める。
Ｕｎ［ｉ＋１］＝Ｕｎ［ｉ］−γＥｎ［ｉ］，ｎ＝１，２，３        （４６）
【０１１２】
制御部２５０は、反復処理２５３として、式（４４）〜式（４６）の演算を、差分Ｅｎ［ｉ］が所定値以下（例えば０）に収束するまで所定回数ｍ、反復して行う（反復工程）。
【０１１３】
図１０に示すＤｉｓｔｒｉｂｕｔｉｏｎ  Ａｌｇｏｒｉｔｈｍのブロックでは、式（３８）〜（４１）に示した分配アルゴリズムを実行し、Ｉｔｅｒａｔｉｏｎ  Ａｌｇｏｒｉｔｈｍのブロックでは、式（４４）〜式（４６）の反復演算アルゴリズムを実行する。
【０１１４】
次に、フィードフォワード制御のみを行う場合（つまりＴＦＢｎ＝０の場合）についての駆動力指令値演算処理２５５（図９）について説明する。なお、トルク指令値はＴｎ＝ＴＦＦＷｎである。
【０１１５】
制御部２５０は、反復処理２５３の終了後、駆動力指令値演算処理２５５として、関節剛性指令値Ｕｎ［ｍ］及びトルク指令値Ｔｎ［ｍ］を用いて、各駆動力指令値を演算する各駆動力指令値ｕｅｎ，ｕｆｎを演算する（駆動力指令値演算工程）。これにより、関節２１１，２１２にはトルク指令値Ｔｎに対応するトルクが印加され、関節２１１，２１２の角度θ１，θ２が目標軌道に追従する。
【０１１６】
（２．２）２自由度制御系
フィードフォワード制御と共にフィードバック制御も行う場合は、制御部２５０は、図９に示す、補正トルク指令値演算処理２５４として、ＰＩＤ制御処理ＫＰＩＤ１〜ＫＰＩＤ３を実行する（補正トルク指令値演算工程）。
【０１１７】
そして、制御部２５０は、トルク指令値Ｔｎとして、フィードフォワード入力ＴＦＦｎに制御入力トルクＴＦＢｎを加算補正した結果を用いる。その際、制御部２５０は、上記第２実施形態と同様、制約処理２５６として式（３０）で制約し、収縮力ｕｆｎ，ｕｅｎを求める。
【０１１８】
以下、具体的に説明すると、図９に示すフィードバック制御系であるＰＩＤ制御処理ＫＰＩＤ１，ＫＰＩＤ２では、関節角度θ１，θ２と目標軌道ｒａ１，ｒａ２との差分を補償するための制御入力トルクＴＦＢ１，ＴＦＢ２を演算する。さらに、ＰＩＤ制御処理ＫＰＩＤ３では、関節角度θ１＋θ２と目標角度ｒａ１＋ｒａ２との差分を補償するための制御入力トルクＴＦＢ３を演算する。
【０１１９】
そして、制御入力Ｔｎ（ｎ＝１，２，３）に対して上記第２実施形態と同様に、
｜Ｔｎ｜＜Ｕｎｒ，ｎ＝１，２，３          （４７）
と制約する。駆動力指令値ｕｆｎ，ｕｅｎ（ｎ＝１，２，３）は、
【０１２０】
【数２３】
0005930754.tif 000024
となる。
【０１２１】
（２．３）手先剛性制御
２リンクマニピュレータでは、手先が外界と直に接触するので、手先の剛性を制御することが重要である。手先の剛性は、図１１に示すようなスティフネス楕円によって表される。この楕円は、各方向に対しての剛性の分布を示し、手先と楕円の距離が離れるほど剛性が高いことを表している。上記第１実施形態の１リンクマニュピレータでは、関節の剛性と手先の剛性は同一であった。本第３実施形態の３対６筋を有する２リンクマニュピレータでは、関節剛性指令値Ｕ１，Ｕ２，Ｕ３を制御することにより手先の剛性が求まる。例えば、関節剛性指令値Ｕ１，Ｕ２，Ｕ３を
Ｕ１＝Ｕ２＝Ｕ３          （４９）
と制御すると、スティフネス楕円の長軸は第１関節と手先を結ぶ方向を向くことが知られている。本第３実施形態では、剛性は収縮力が最小値となるように反復演算されるため、駆動中の剛性を任意に指定することはできない。しかし、手先の目標位置で手先剛性を制御することは可能である。本第３実施形態では、図１２に示すように、関節剛性指令値の下限値を可変させ、手先の目標位置で、式（４９）を満たすように剛性の制御を行う。
【０１２２】
（２．４）軌道設計
本第３実施形態では、目標軌道を手先が図８のｙ軸上を正方向に駆動するように設定する。さらに上記第１実施形態と同様に加速区間−等速区間−減速区間を有するものとする。関節角度に対する目標軌道ｒａ１，ｒａ２は、手先軌道から逆運動学により求める。本第３実施形態では、加速区間−等速区間−減速区間を有する軌道を用いたが、例えばミニマムジャーク軌道のような等速区間を伴わない軌道であっても構わない。
【０１２３】
（３）シミュレーション
前節の制御系を用いたシミュレーションを行う。第１リンク２０１と第２リンク２０２の物理パラメータは同一とする。リンク長さを０．２ｍ、リンクの慣性モーメントをＩ１＝Ｉ２＝１．３×１０−３ｋｇｍ２、モーメントアーム径を０．０５ｍ、弾性、粘性定数をｋ＝１２，ｂ＝０．００３とする。目標軌道は、等速区間の開始時間ｔａ＝０．２７４７秒、等速区間の終了時間ｔｂ＝０．４７４６秒、位置決め終了時間ｔｆｉｎ＝０．７５秒とする。また、関節トルクが作用しないときの弾性力による中立姿勢角度θｃ１，θｃ２は、手先軌道の中間地点における関節角度とし、θｃ１＝２９．７ｄｅｇ、θｃ２＝１２０．７ｄｅｇとする。さらに、本第１実施形態では、上記第２実施形態と同様に、２自由度制御系のモデルの同定誤差に対する検証を行う。そのため、フィードフォワード入力の生成では慣性モーメントをＩ１，Ｉ２とするが、シミュレーションではリンクの慣性モーメントをＩ１′＝１．１Ｉ１，Ｉ２′＝１．０５Ｉ２とする。
【０１２４】
図１３（ａ）及び図１３（ｂ）に第１、第２リンクの関節角度θ１，θ２を実線で、目標軌道ｒａ１，ｒａ２を破線で示す。図１４（ａ）〜図１４（ｃ）に駆動力指令値ｕｆ１，ｕｆ２，ｕｆ３の応答を実線で、駆動力指令値ｕｅ１，ｕｅ２，ｕｅ３の応答を破線で示す。さらに、図１５（ａ）〜図１５（ｃ）に駆動力指令値ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）の差を実線で、和を破線で、Ｔｎ／ｒ（ｎ＝１，２，３）の応答を一点鎖線で示す。また、図１６にＴＦＢ１，ＴＦＢ２，ＴＦＢ３をそれぞれ実線、破線、一点鎖線で示す。
【０１２５】
図１３（ａ）及び図１３（ｂ）より実線で示す関節角度は破線で示す軌道と重なっており、リンクは目標軌道に追従していることがわかる。上記第２実施形態と同様、図１６に示すように、フィードバック制御入力がモデル誤差を補償しているためである。図１４（ａ）より、駆動力指令値ｕｅ１，ｕｆ１は２自由度制御系の制御入力Ｔ１を、式（４７）と式（４８）に示した条件で配分されているため、人工筋肉の収縮力は正の値を取るという特性を満たしていることがわかる。図１４（ｂ）及び図１４（ｃ）より、駆動力指令値ｕｅ２，ｕｆ２，ｕｅ３，ｕｆ３も同様に正の値を取っていることがわかる。
【０１２６】
図１５（ａ）において、実線で示すｕｆ１−ｕｅ１は、一点鎖線で示すＴ１／ｒと重なっており、式（２８）の条件を満たしている。同様に、図１５（ｂ）及び図１５（ｃ）において、実線で示すＵ２，Ｕ３は、一点鎖線で示すＴ２／ｒ，Ｔ３／ｒと重なっている。また図１５（ａ）〜図１５（ｃ）において、破線で示すｕｆ１＋ｕｅ１＝Ｕ１，ｕｆ２＋ｕｅ２＝Ｕ２，ｕｆ３＋ｕｅ３＝Ｕ３は、図１２に設定した０〜０．６２５秒のＶｎ（ｎ＝１，２，３）が０に近い区間では、実線で示すＴ′１／ｒの絶対値に近接している。これから、上記第１及び第２実施形態と同様に、式（４７）の制約を満たす範囲で、常に最小値を取っていることがわかる。さらに、Ｖｎが増加する０．６５秒以降では、各拮抗対の剛性はＴ′１／ｒ（ｎ＝１，２，３）の絶対値に対して大きな値へと移行している。これより、手先のスティフネス楕円は目標軌道において、式（４９）の条件を満たすように制御されていることがわかる。本第３実施形態の提案手法により、モデルに同定誤差などが存在する場合でも、最小の収縮力で目標軌道への追従を実現しながら、目標位置では３対６筋の弾性により手先の剛性を制御することが可能であることがわかる。
【０１２７】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。
【符号の説明】
【０１２８】
１００…ロボット装置、１０１…リンク、１０５…関節、１５０…制御部、１５１…トルク指令値演算処理、１５２…変更演算処理、１５３…反復処理、１５５…駆動力指令値演算処理
【図１】
0005930754.tif 000025
【図２】
0005930754.tif 000026
【図３】
0005930754.tif 000027
【図４】
0005930754.tif 000028
【図５】
0005930754.tif 000029
【図６】
0005930754.tif 000030
【図７】
0005930754.tif 000031
【図８】
0005930754.tif 000032
【図９】
0005930754.tif 000033
【図１０】
0005930754.tif 000034
【図１１】
0005930754.tif 000035
【図１２】
0005930754.tif 000036
【図１３】
0005930754.tif 000037
【図１４】
0005930754.tif 000038
【図１５】
0005930754.tif 000039
【図１６】
0005930754.tif 000040