(57)【特許請求の範囲】
【請求項１】
基端が固定部材に揺動可能に支持されたリンクと、前記固定部材に対して前記リンクを揺動させる駆動部と、を有するリンク機構を備え、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御するロボット装置の制御方法において、
前記固定部材に対する前記リンクの揺動角度と目標揺動角度との偏差を算出する偏差算出ステップと、
前記偏差を補償するトルク指令値Ｔ１を生成するトルク指令生成ステップと、
前記トルク指令値Ｔ１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記第１及び第２の駆動力指令値の和を示す剛性指令値Ｕ１との乗算値Ｕ１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ１を補正するトルク指令補正ステップと、
前記トルク指令補正ステップにより補正されたトルク指令値Ｔ１を用いて、前記各駆動力指令値を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式により求める駆動力指令値演算ステップと、
を備えたことを特徴とするロボット装置の制御方法。
【請求項２】
前記ロボット装置が、前記リンクに外力が作用したことにより検知信号を発生するセンサを有しており、
前記偏差算出ステップでは、前記センサから検知信号が出力されている期間には０以上１以下の範囲内の値であり、前記センサからの検知信号の出力が停止されている期間には１の値である係数値Ｇｔを前記偏差に乗じた、新たな偏差を算出することを特徴とする請求項１に記載のロボット装置の制御方法。
【請求項３】
前記偏差算出ステップでは、前記センサから検知信号が出力されている期間において、前記係数値Ｇｔを０に設定することを特徴とする請求項２に記載のロボット装置の制御方法。
【請求項４】
前記偏差算出ステップでは、前記センサから検知信号が出力されている期間において、前記係数値Ｇｔを、時間が経過するに連れて初期値から大きくなるように設定することを特徴とする請求項２に記載のロボット装置の制御方法。
【請求項５】
前記トルク指令生成ステップでは、前記センサから検知信号が出力されている期間は、前記偏差算出ステップにより算出された偏差に対してＰＩＤ制御演算を行い、この演算結果に対して前記係数値Ｇｔを乗じた値に、前記センサから検知信号が出力される直前のＰＩＤ制御演算の演算結果に（１−Ｇｔ）の値を乗じた値を加算して、トルク指令値Ｔを生成する、ことを特徴とする請求項２乃至４のいずれか１項に記載のロボット装置の制御方法。
【請求項６】
前記固定部材が４足歩行ロボットの胴体であり、前記リンク機構が前記胴体に接続された４つの支持脚のうちのいずれかの支持脚であり、
前記リンクの軌道データに基づいて前記目標揺動角度を設定する目標設定ステップを備え、
前記偏差算出ステップでは、前記センサから検知信号が出力されている期間においては、前記リンクの脚相に応じて、前記係数値Ｇｔを０又は１に決定し、
前記目標設定ステップでは、前記係数値Ｇｔが０から１に変更されたときに、前記軌道データを前記偏差に基づいて再計算することを特徴とする請求項２乃至５のいずれか１項に記載のロボット装置の制御方法。
【請求項７】
前記制約関数は、前記トルク指令生成ステップで生成したトルク指令値Ｔ１の絶対値が前記乗算値Ｕ１×ｒ以下の場合は、前記トルク指令生成ステップで生成したトルク指令値Ｔ１のままとし、前記トルク指令生成ステップで生成したトルク指令値Ｔ１の絶対値が前記乗算値Ｕ１×ｒを上回った場合は、絶対値が前記乗算値Ｕ１×ｒに制限された新たなトルク指令値Ｔ１を生成する飽和関数であることを特徴とする請求項１乃至６のいずれか１項に記載のロボット装置の制御方法。
【請求項８】
前記制約関数は、±Ｕ１×ｒを漸近線とする双曲線正接関数であることを特徴とする請求項１乃至６のいずれか１項に記載のロボット装置の制御方法。
【請求項９】
前記駆動部が、一端が前記固定部材に接続され、他端が前記リンクに接続され、収縮により駆動力を発生させて前記リンクを揺動させる一対の一関節筋アクチュエータであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置の制御方法。
【請求項１０】
前記駆動部が、前記関節に設けられた回転モータであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置の制御方法。
【請求項１１】
基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構を備え、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御するロボット装置の制御方法において、
前記固定部材に対する前記第１のリンクの第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出ステップと、
前記第１の偏差を補償する第１のトルク指令値Ｔ１を生成する第１のトルク指令生成ステップと、
前記第１のトルク指令値Ｔ１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ１を補正する第１のトルク指令補正ステップと、
前記第１のトルク指令補正ステップにより補正された第１のトルク指令値Ｔ１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式により求める第１の駆動力指令値演算ステップと、
前記第１のリンクに対する前記第２のリンクの第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出ステップと、
前記第２の偏差を補償する第２のトルク指令値Ｔ２を生成する第２のトルク指令生成ステップと、
前記第２のトルク指令値Ｔ２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ２を補正する第２のトルク指令補正ステップと、
前記第２のトルク指令補正ステップにより補正された第２のトルク指令値Ｔ２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式により求める第２の駆動力指令値演算ステップと、
を備えたことを特徴とするロボット装置の制御方法。
【請求項１２】
前記第１のリンク及び前記第２のリンクが揺動する平面内に直交座標系を定義し、前記第２のリンクの先端における剛性の大きさを、前記直交座標系に前記第２のリンクの先端を中心とするスティフネス楕円で表し、前記スティフネス楕円の長軸及び短軸のうちの一方の軸が前記直交座標系における基準軸に対して平行となるように、前記一対の第３のアクチュエータの駆動力指令値の和を示す第３の剛性指令値Ｕ３を生成する剛性指令値生成ステップと、
前記第３の剛性指令値Ｕ３に基づいて前記各第３のアクチュエータの駆動力指令値を求める第３の駆動力指令値演算ステップと、を備えたことを特徴とする請求項１１に記載のロボット装置の制御方法。
【請求項１３】
基端が固定部材に揺動可能に支持されたリンク、前記固定部材に対して前記リンクを揺動させる駆動部を有するリンク機構と、
前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御する制御装置と、を備え、
前記制御装置は、
前記固定部材に対する前記リンクの揺動角度を検知する揺動角度検知センサと、
前記揺動角度と目標揺動角度との偏差を算出する偏差算出部と、
前記偏差を補償するトルク指令値Ｔ１を生成するトルク指令生成部と、
前記トルク指令値Ｔ１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記各駆動力指令値の和を示す剛性指令値Ｕ１との乗算値Ｕ１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ１を補正するトルク指令補正部と、
前記トルク指令補正部で補正したトルク指令値Ｔ１を用いて、前記各駆動力指令値を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式により求める駆動力指令値演算部と、を有する特徴とするロボット装置。
【請求項１４】
基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構と、
前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御する制御装置と、を備え、
前記制御装置は、
前記固定部材に対する前記第１のリンクの第１の揺動角度を検知する第１の揺動角度検知センサと、
前記第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出部と、
前記第１の偏差を補償する第１のトルク指令値Ｔ１を生成する第１のトルク指令生成部と、
前記第１のトルク指令値Ｔ１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ１を補正する第１のトルク指令補正部と、
前記第１のトルク指令補正部で補正した第１のトルク指令値Ｔ１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式により求める第１の駆動力指令値演算部と、
前記第２のリンクの前記第１のリンクに対する第２の揺動角度を検知する第２の揺動角度検知センサと、
前記第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出部と、
前記第２の偏差を補償する第２のトルク指令値Ｔ２を生成する第２のトルク指令生成部と、
前記第２のトルク指令値Ｔ２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ２を補正する第２のトルク指令補正部と、
前記第２のトルク指令補正部で補正した第２のトルク指令値Ｔ２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式により求める第２の駆動力指令値演算部と、を有することを特徴とするロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、リンクの角度制御及び剛性制御を行うロボット装置の制御方法及びロボット装置に関するものである。好適には、作業ロボットや脚式移動ロボットに適用される。
【背景技術】
【０００２】
マニピュレータの制御方法において、リンクの先端に設けたエンドエフェクタ（例えばハンド）が対象物に柔軟に接触できることが重要になってきている。これを産業ロボットに応用すれば、ロボット装置と人間との共同作業が実現でき、ハンドの柔軟性の方向を制御することにより、部品の嵌め合い作業等が容易になる。また、これを脚式移動ロボットに応用すれば、地面に柔らかく接地することで胴体に対する衝撃を和らげることができ、段差を吸収することで不整地を安定して歩行することが可能となる。
【０００３】
ハンドの柔軟性の制御を実現するために、ハンドに力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉の中でも、特にＭｃＫｉｂｂｅｎ型人工筋肉に代表される空気圧ゴム人工筋肉は、粘弾性特性が筋に類似している。そしてマニピュレータに配置されている人工筋肉アクチュエータの柔らかさを制御することで、任意の手先の柔軟性で対象物に接触することができる。しかし、人工筋肉アクチュエータは、粘弾性特性に非線系性を有している、収縮方向のみに力を発生するため拮抗配置して制御を行う必要がある、などの理由で制御性に難があることが知られている。
【０００４】
これに対し、圧縮流体が供給されるゴム人工筋肉アクチュエータをリンクに拮抗配置し、関節の角度検出を行い、フィードバック制御することによって関節におけるリンクの揺動角度を制御する技術が開示されている（特許文献１参照）。
【０００５】
また、筋の粘弾性特性を含むマニピュレータのモデルと、修正値計算部を用いて関節角度と手先の柔軟性を同時制御するためのフィードフォワード入力を生成する技術が開示されている（特許文献２参照）。特許文献２では、モデルを用いて制御入力が与えられた時の関節角度と人工筋肉アクチュエータの粘弾性係数を出力し、それを目標値とを比較する。そして、目標値との誤差を修正値計算部に逆伝播させ、フィードフォワード入力を修正する。修正されたフィードフォワード入力を再びモデルに与える操作を繰り返し、徐々にフィードフォワード入力を求めている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開昭６３−６８３８２号公報
【特許文献２】特許第３４３６３２０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記特許文献１では、リンクの揺動角度を検知して目標揺動角度となるようにフィードバック制御を行っているが、関節におけるリンクの揺動角度のみを制御しており、関節におけるリンクの柔軟さ（剛性）を変動させる制御は実現していない。
【０００８】
更に、上記特許文献２では、フィードフォワード制御によりリンクの揺動角度と関節におけるリンクの柔軟性の同時制御を行っている。フィードフォワード制御入力は、目標値との誤差を繰り返し修正することによって求められているため、その生成に要する計算量は非常に大きい。そのため、リンクの揺動角度やリンクとリンクを接続する関節の柔軟性の目標値を変更する度に、再び繰り返し演算をする必要があり、目標値を変更した際に、演算に時間を要するという問題がある。
【０００９】
なお、これらの問題は、リンクを駆動する駆動部として人工筋肉アクチュエータを用いた場合に限らず、駆動部として回転モータを用いて制御する場合についても同様である。
【００１０】
そこで、本発明は、フィードバック制御により、リンクの角度制御と関節の剛性制御を行うロボット装置の制御方法及びロボット装置を提供することを目的とするものである。
【課題を解決するための手段】
【００１１】
本発明のロボット装置の制御方法は、基端が固定部材に揺動可能に支持されたリンクと、前記固定部材に対して前記リンクを揺動させる駆動部と、を有するリンク機構を備え、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御するロボット装置の制御方法において、前記固定部材に対する前記リンクの揺動角度と目標揺動角度との偏差を算出する偏差算出ステップと、前記偏差を補償するトルク指令値Ｔ１を生成するトルク指令生成ステップと、前記トルク指令値Ｔ１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記第１及び第２の駆動力指令値の和を示す剛性指令値Ｕ１との乗算値Ｕ１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ１を補正するトルク指令補正ステップと、前記トルク指令補正ステップにより補正されたトルク指令値Ｔ１を用いて、前記各駆動力指令値を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式により求める駆動力指令値演算ステップと、を備えたことを特徴とする。
【００１２】
また、本発明のロボット装置の制御方法は、基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構を備え、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御するロボット装置の制御方法において、前記固定部材に対する前記第１のリンクの第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出ステップと、前記第１の偏差を補償する第１のトルク指令値Ｔ１を生成する第１のトルク指令生成ステップと、前記第１のトルク指令値Ｔ１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ１を補正する第１のトルク指令補正ステップと、前記第１のトルク指令補正ステップにより補正された第１のトルク指令値Ｔ１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式により求める第１の駆動力指令値演算ステップと、前記第１のリンクに対する前記第２のリンクの第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出ステップと、前記第２の偏差を補償する第２のトルク指令値Ｔ２を生成する第２のトルク指令生成ステップと、前記第２のトルク指令値Ｔ２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ２を補正する第２のトルク指令補正ステップと、前記第２のトルク指令補正ステップにより補正された第２のトルク指令値Ｔ２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式により求める第２の駆動力指令値演算ステップと、を備えたことを特徴とする。
【００１３】
また、本発明のロボット装置は、基端が固定部材に揺動可能に支持されたリンク、前記固定部材に対して前記リンクを揺動させる駆動部を有するリンク機構と、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御する制御装置と、を備え、前記制御装置は、前記固定部材に対する前記リンクの揺動角度を検知する揺動角度検知センサと、前記揺動角度と目標揺動角度との偏差を算出する偏差算出部と、前記偏差を補償するトルク指令値Ｔ１を生成するトルク指令生成部と、前記トルク指令値Ｔ１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記各駆動力指令値の和を示す剛性指令値Ｕ１との乗算値Ｕ１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ１を補正するトルク指令補正部と、前記トルク指令補正部で補正したトルク指令値Ｔ１を用いて、前記各駆動力指令値を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式により求める駆動力指令値演算部と、を有する特徴とする。
【００１４】
また、本発明のロボット装置は、基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構と、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御する制御装置と、を備え、前記制御装置は、前記固定部材に対する前記第１のリンクの第１の揺動角度を検知する第１の揺動角度検知センサと、前記第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出部と、前記第１の偏差を補償する第１のトルク指令値Ｔ１を生成する第１のトルク指令生成部と、前記第１のトルク指令値Ｔ１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ１を補正する第１のトルク指令補正部と、前記第１のトルク指令補正部で補正した第１のトルク指令値Ｔ１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式により求める第１の駆動力指令値演算部と、前記第２のリンクの前記第１のリンクに対する第２の揺動角度を検知する第２の揺動角度検知センサと、前記第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出部と、前記第２の偏差を補償する第２のトルク指令値Ｔ２を生成する第２のトルク指令生成部と、前記第２のトルク指令値Ｔ２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ２を補正する第２のトルク指令補正部と、前記第２のトルク指令補正部で補正した第２のトルク指令値Ｔ２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式により求める第２の駆動力指令値演算部と、を有することを特徴とする。
【発明の効果】
【００１５】
本発明によれば、簡便なフィードバック制御系により、リンクの角度制御と関節におけるリンクの剛性制御の同時制御を実現することができる。これにより、リンクの揺動角度と剛性の目標値が変更されても、繰り返し演算を行う必要がないので、迅速にリンクの揺動角度及び関節の剛性を目標値に調整することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の第１実施形態に係るロボット装置のアクチュエータの粘弾性モデルを示す図である。
【図２】本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。
【図３】本発明の第１実施形態に係るロボット装置の制御装置を示すブロック線図である。
【図４】本発明の第１実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）はリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は収縮力指令値に対する応答を示す図である。（ｃ）は収縮力指令値の差及び和に対する応答と、トルク指令値による応答を示す図である。
【図５】本発明の第１実施形態に係るロボット装置における剛性指令値に対する応答のシミュレーション結果を示す図である。
【図６】比較例のロボット装置におけるシミュレーション結果を示す図であり、（ａ）はリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は収縮力指令値に対する応答を示す図である。（ｃ）は収縮力指令値の差及び和に対する応答と、トルク指令値による応答を示す図である。
【図７】本発明の第２実施形態に係るロボット装置の概略構成を示す説明図である。
【図８】本発明の第２実施形態に係るロボット装置の第２のリンクの先端におけるスティフネス楕円を説明するための図である。
【図９】本発明の第２実施形態に係るロボット装置の制御装置を示すブロック線図である。
【図１０】本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１のリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は第２のリンクの揺動角度と目標揺動角度の変化を示す図である。
【図１１】本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１の剛性指令値に対する応答を示す図、（ｂ）は第２の剛性指令値に対する応答を示す図、（ｃ）は第３の剛性指令値に対する応答を示す図である。
【図１２】本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１の収縮力指令値の差及び和に対する応答と、第１のトルク指令値による応答を示す図である。（ｂ）は第２の収縮力指令値の差及び和に対する応答と、第２のトルク指令値による応答を示す図である。（ｃ）は第３の収縮力指令値の差及び和に対する応答を示す図である。
【図１３】本発明の第２実施形態に係るロボット装置における第２のリンクの先端の軌跡と目標軌道のシミュレーション結果を示す図である。
【図１４】本発明の第３実施形態に係るロボット装置の制御装置を示すブロック線図である。
【図１５】本発明の第３実施形態の制御装置の各ゲイン調整器の係数値の変化を示す図である。
【図１６】本発明の第３実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は外力が作用したときの第１のリンクの揺動角度を示す図であり、（ｂ）は外力が作用したときの第２のリンクの揺動角度を示す図である。
【図１７】本発明の第３実施形態に係るロボット装置における第３のアクチュエータに対する収縮力指令値の差及び和に対する応答のシミュレーション結果を示す図である。
【図１８】本発明の第３実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）はゲイン調整器の係数値を変化させた場合の第２のリンクの先端の軌跡と目標軌道を示す図である。（ｂ）はゲイン調整器の係数値を０とした場合の第２のリンクの先端の軌跡と目標軌道を示す図である。
【図１９】本発明の第４実施形態に係るロボット装置の制御装置の要部を示すブロック線図である。
【図２０】本発明の第５実施形態におけるロボット装置のシミュレーション結果を示すスティック線図である。
【図２１】本発明の第６実施形態に係るロボット装置の要部を示すブロック線図である。
【図２２】本発明の第７実施形態に係るロボット装置における制約関数を説明するための図である。
【図２３】本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は目標角度と各制約関数を用いた応答の角度とを比較した図、（ｂ）は各制約関数で決定される収縮力指令値を示す図である。
【図２４】本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は第１のリンクの目標角度と各制約関数の応答である実際の角度とを比較した図、（ｂ）は第２のリンクの目標角度と各制約関数の応答である実際の角度とを比較した図である。
【図２５】本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は各制約関数を用いた第１の収縮力指令値の応答を示す図、（ｂ）は各制約関数を用いた第２の収縮力指令値の応答を示す図である。
【図２６】本発明の第８実施形態に係るロボット装置としての４足歩行ロボットのリンク配置と筋配置および座標系を示す側面図である。
【図２７】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は右後脚のスティック線図、足先軌道及びスティフネス楕円を示す図である。（ｂ）は右前脚のスティック線図、足先軌道及びスティフネス楕円を示す図である。
【図２８】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は各支持脚の歩容位相ダイアグラム、（ｂ）は各支持脚における支持脚相を説明するための図である。
【図２９】本発明の第８実施形態に係るロボット装置の制御装置の要部を示すブロック線図である。
【図３０】本発明の第８実施形態に係るロボット装置の外観を示す斜視図である。
【図３１】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は左後脚第１リンク角度の応答を示す図、（ｂ）は左後脚第２リンク角度の応答を示す図である。（ｃ）は右後脚第１リンク角度の応答を示す図、（ｄ）は第２リンク角度の応答を示す図である。
【図３２】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は平地歩行時の左後脚足球中心の軌跡を示す図、（ｂ）は障害物乗り上げ時の左後脚足球中心の軌跡を示す図である。
【図３３】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の歩行制御による応答を示す図、（ｂ）は関節軌道の追従のみによる歩行制御による応答を示す図である。
【図３４】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は胴体重心のロール角度、（ｂ）は進行方向の変位、（ｃ）は速度の応答を示す。
【図３５】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の制御系による歩行による応答を示す図、（ｂ）は関節軌道の追従のみによる歩行による応答を示す図である。
【図３６】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の歩行制御によるロボット装置の姿勢推移を示す図、（ｂ）は関節軌道の追従のみによるロボット装置の姿勢推移を示す図である。
【図３７】本発明の第８実施形態に係るロボット装置におけるシミュレーション結果であり、胴体重心の加速度を示す図である。
【発明を実施するための形態】
【００１７】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１８】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置に適用されるアクチュエータの粘弾性モデルを示す図である。本第１実施形態では、ロボット装置として、空気圧式の人工筋肉アクチュエータを用いたマニピュレータの関節角度と関節の剛性の同時制御を例として説明する。
【００１９】
（１）モデリング
人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。人工筋肉アクチュエータは、図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。ここで、ｕを力発生要素の収縮力、ｘを収縮方向を正とする筋の収縮量とする。収縮速度は、以下の式となる。
【００２０】
【数１】
0005854695.tif 000002
【００２１】
また、ｋ弾性力定数、ｂを粘性力定数、Ｆを筋収縮力とする。このとき、筋の粘弾性特性は、
【００２２】
【数２】
0005854695.tif 000003
とモデル化され、筋収縮力の弾性力、粘性力は、力発生要素の収縮力ｕに比例するという非線形要素を有する点が特徴的である。
【００２３】
次に、本第１実施形態のロボット装置を図２に示す。図２に示すロボット装置１００は、いわゆる１リンクマニピュレータであり、リンク１０１と、一対のアクチュエータｅ１，ｆ１からなる駆動部１２０と、固定部材であるベース部材１０３及びベース部材１０３から延びる棒状のアーム部材１０４とを備えている。
【００２４】
リンク１０１は、長手部材からなり、その基端１０１ａが固定部材であるアーム部材１０４の長手方向の先端に揺動可能に支持されている。このリンク１０１の先端１０１ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、リンク１０１は、関節で揺動可能に支持されている。
【００２５】
各アクチュエータｅ１，ｆ１は、一関節筋アクチュエータであり、一端が固定部材であるベース部材１０３に接続され、他端がリンク１０１の基端１０１ａに接続されて、駆動力（収縮力）の差によりリンク１０１を揺動させるように拮抗配置されている。つまり、アクチュエータｅ１，ｆ１は、リンク１０１を挟んでリンク１０１の両側に対称配置されている。
【００２６】
また、ロボット装置１００は、各アクチュエータｅ１，ｆ１の駆動力を各駆動力指令値により設定して、リンク１０１の動作を制御する制御装置１５０を備えている。
【００２７】
各アクチュエータｅ１，ｆ１は、図１に示す空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは収縮方向にのみ力を発生するため、関節を任意の角度に位置決めするために、図２に示すように拮抗して配置されている。つまり、各アクチュエータｅ１，ｆ１は、人工筋肉からなる一関節筋アクチュエータであるので、収縮することにより駆動力が発生する。したがって、アクチュエータｅ１，ｆ１の収縮力が駆動力となる。
【００２８】
拮抗配置されたアクチュエータｅ１，ｆ１の力発生要素の駆動力（収縮力）をそれぞれｕｅ１，ｕｆ１、アーム部材１０４に対するリンク１０１の揺動角度をθ、リンクの慣性モーメントをＩとする。また、モーメントアームの長さ、つまりリンク１０１の揺動中心点とリンク１０１におけるアクチュエータｅ１，ｆ１の接続点との長さをｒとする。運動方程式は、以下の式（２）となる。
【００２９】
【数３】
0005854695.tif 000004
【００３０】
式（２）の右辺第１項における駆動力ｕｅ１，ｕｆ１の差が関節に回転トルクを与え、右辺第２項、第３項における駆動力ｕｅ１，ｕｆ１の和が関節におけるリンクの剛性、粘性を変動させることがわかる。
【００３１】
（２）制御系設計
本第１実施形態では、アーム部材１０４に対するリンク１０１の揺動角度を目標揺動角度に位置決めし、同時にアーム部材１０４とリンク１０１との関節におけるリンク１０１の剛性を制御することを目的としている。
【００３２】
図３に本第１実施形態における制御装置１５０のブロック線図を示す。制御装置１５０は、リンク１０１を第１の揺動方向（図１中、矢印と反対方向）に揺動させる駆動力をアクチュエータｅ１に発生させる第１の駆動力指令値ｕｅ１を生成する。また、制御装置１５０は、リンク１０１を第１の揺動方向とは反対の第２の揺動方向（図１中、矢印の方向）に揺動させる駆動力をアクチュエータｆ１に発生させる第２の駆動力指令値ｕｆ１を生成する。そして、制御装置１５０は、第１の駆動力指令値ｕｅ１と第２の駆動力指令値ｕｆ１との差に基づき、一対のアクチュエータｅ１，ｆ１を動作させてリンク１０１の揺動動作を制御する。これと共に、制御装置１５０は、第１の駆動力指令値ｕｅ１と第２の駆動力指令値ｕｆ１との和に基づき一対のアクチュエータｅ１，ｆ１を動作させてアーム部材１０４とリンク１０１との関節の剛性を制御する。
【００３３】
ここで、固定部材であるアーム部材１０４に対するリンク１０１の目標揺動角度をｒａ、アーム部材１０４とリンク１０１との関節におけるリンク１０１の目標剛性をｒｓとする。なお、図３中、符号Ｐで示すブロックは、アクチュエータｅ１，ｆ１及びリンク１０１からなるリンク機構を示している。以下、制御装置１５０の構成について詳細に説明する。
【００３４】
制御装置１５０は、アーム部材１０４に対するリンク１０１の揺動角度θ１を検知する揺動角度検知センサ１と、リンク１０１に外力が作用したときに外力が作用したことを示す検知信号を出力するタッチセンサ２とを備えている。
【００３５】
また、制御装置１５０は、偏差算出部である減算器３及びゲイン調整器４と、トルク指令生成部であるＰＩＤ制御器５と、剛性指令値Ｕ１にリンク１０１の揺動中心点を中心とするモーメントアームの長さｒを乗算する乗算部としての乗算器６とを備えている。
【００３６】
また、制御装置１５０は、ＰＩＤ制御器５が出力するトルク指令値Ｔ１を補正するトルク指令補正部である飽和回路７を備えている。また、制御装置１５０は、トルク指令値Ｔ１及び剛性指令値Ｕ１から各アクチュエータｅ１，ｆ１への駆動力指令値ｕｅ１，ｕｆ１を演算により求める駆動力指令値演算部８を備えている。
【００３７】
以下、各部について詳細に説明する。まず、揺動角度検知センサ１は、ポテンショメータ又はエンコーダ等のセンサであり、図１において、図示は省略しているが、アーム部材１０４とリンク１０１との関節に設けられており、アーム部材１０４の延びる方向を基準とする関節の角度を検知する。
【００３８】
タッチセンサ２は、不図示のエンドエフェクタ（例えばハンド）又はリンク１０１に設けられ、エンドエフェクタ又はリンク１０１に接触物が接触したときに、検知信号を出力する。つまり、リンク１０１に外力が作用するとは、エンドエフェクタ又はリンク１０１に接触物が接触することを意味する。
【００３９】
減算器３は、揺動角度θ１と目標揺動角度ｒａとの偏差（差分）を示す値（ｒａ−θ１）を算出する。
【００４０】
ゲイン調整器４は、減算器３が出力した偏差を示す値（ｒａ−θ１）に接触ゲインである係数値Ｇｔを乗じて出力する。係数値Ｇｔは、０以上１以下の範囲内の値であり、タッチセンサ２から検知信号がない場合（センサ２からの検知信号の出力が停止されている期間）には、係数値Ｇｔ＝１として、入力した偏差を示す値（ｒａ−θ１）を出力する。タッチセンサ２から検知信号が出力されている期間は、偏差を示す値（ｒａ−θ１）に０以上１以下の範囲内の値に設定された係数値Ｇｔ（本実施形態では、Ｇｔ＝０）を乗じて、新たな偏差を示す値（つまり、ｒａ−θ１＝０）を出力する。
【００４１】
ＰＩＤ制御器５は、偏差を示す値（ｒａ−θ１）に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ１を生成する。即ち、このフィードバック制御系であるＰＩＤ制御器５は、リンク１０１の揺動角度θ１と目標揺動角度ｒａとの偏差を補償するための（つまり、揺動角度θ１を目標揺動角度ｒａに近づけるための）トルク指令値Ｔ１を出力する。
【００４２】
本第１実施形態では、ＰＩＤ制御器５の伝達関数は、
【００４３】
【数４】
0005854695.tif 000005
としている。
【００４４】
ここで、式（２）より、
【００４５】
【数５】
0005854695.tif 000006
を満たすように駆動力指令値ｕｅ１，ｕｆ１を決定すれば、トルク指令値Ｔ１のトルクでリンク１０１を揺動させることができる。
【００４６】
しかし、以下の式（５）に示す同時に関節におけるリンク１０１の剛性に関する条件を満たさなければならない。
【００４７】
【数６】
0005854695.tif 000007
【００４８】
ここで、２つの駆動力指令値ｕｅ１，ｕｆ１の和を示す剛性指令値Ｕ１を定義すると、剛性指令値Ｕ１は、以下の式（６）となる。
【００４９】
【数７】
0005854695.tif 000008
【００５０】
従って、剛性指令値Ｕ１は、目標剛性ｒｓを用いた以下の式（７）となる。なお、弾性力定数ｋ及びモーメントアームの長さｒは定数であるので、目標剛性ｒｓと剛性指令値Ｕ１は比例関係にあり同等である。
【００５１】
【数８】
0005854695.tif 000009
【００５２】
この式（７）を満たすように、駆動力指令値ｕｅ１，ｕｆ１を決定する必要がある。そこで、トルク指令値Ｔ１と剛性指令値Ｕ１とを同時に満たすには、式（４）と式（６）をｕｅ１，ｕｆ１について解き、以下の式（８）とすればよい。
【００５３】
【数９】
0005854695.tif 000010
【００５４】
しかし、人工筋肉アクチュエータは収縮方向にのみ力を発生する。そのため、以下の式（９）の条件を同時に満たさなければならない。
【００５５】
【数１０】
0005854695.tif 000011
【００５６】
式（８）と式（９）より以下の式（１０）の条件が得られる。
【００５７】
【数１１】
0005854695.tif 000012
【００５８】
つまりトルク指令値Ｔ１は、以下の式（１１）の条件となる。
【００５９】
【数１２】
0005854695.tif 000013
【００６０】
従って、式（１１）の条件を満たしていれば、関節におけるリンク１０１の剛性が目標剛性ｒｓとなると同時に、トルク指令値Ｔ１によりリンク１０１の揺動角度が目標角度ｒａに位置決めされる。
【００６１】
そこで、本第１実施形態では、乗算器６は、モーメントアームの長さｒと剛性指令値Ｕ１との乗算値Ｕ１×ｒを算出し、飽和回路７は、式（１１）を実現するために、制約関数に従って、トルク指令値Ｔ１を、式（１２）の値に制約する。
【００６２】
【数１３】
0005854695.tif 000014
【００６３】
即ち、飽和回路７は、トルク指令値Ｔ１の絶対値を、モーメントアームの長さｒと剛性指令値Ｕ１との乗算値Ｕ１×ｒ以下に制約する制約関数に従って、トルク指令値Ｔ１を補正する。より具体的には、飽和回路７は、制約関数として飽和関数に従い、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１をそのまま出力する。また、飽和回路７は、飽和関数に従い、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒを上回った場合は、絶対値が乗算値Ｕ１×ｒに制限された新たなトルク指令値Ｔ１を生成して出力する。
【００６４】
なお、トルク指令補正部は、上述した飽和回路７である以外に、ＰＩＤ制御器５のゲインを変動させ、式（１１）の範囲にトルク指令値Ｔ１（制御入力）の大きさを収めるように機能する回路であってもよい。
【００６５】
以下、タッチセンサ２が検知信号を出力していない場合（タッチセンサ２がＯＦＦ）について、図３を参照しながら制御装置１５０の動作を説明する。まず、揺動角度検知センサ１が、リンク１０１の揺動角度を検知する（揺動角度検知ステップ）。
【００６６】
そして、減算器３が、リンク１０１の揺動角度θ１と目標揺動角度ｒａとの偏差を示す値（ｒａ−θ１）を算出する（偏差算出ステップ）。
【００６７】
タッチセンサ２がＯＦＦであるので、ゲイン調整器４は、乗算する係数値Ｇｔが１であり、偏差を示す値（ｒａ−θ１）をそのまま出力する。
【００６８】
ＰＩＤ制御器５は、ゲイン調整器４から取得した偏差を示す値（ｒａ−θ１）に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ１を生成する（トルク指令生成ステップ）。
【００６９】
乗算器６は、取得した剛性指令値Ｕ１とモーメントアームの長さｒとの乗算値Ｕ１×ｒを算出する（乗算ステップ）。モーメントアームの長さｒは、予め設定（不図示の記憶装置に記憶）された定数であり、乗算器６が剛性指令値Ｕ１を取得したときに、演算により乗算値Ｕ１×ｒを求める。なお、予め乗算値Ｕ１×ｒの計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておき、乗算器６が剛性指令値Ｕ１を取得したときにテーブルデータを参照して乗算値Ｕ１×ｒを求めてもよい。
【００７０】
飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１をそのまま出力する。また、飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒを上回った場合は、絶対値が乗算値Ｕ１×ｒ以下に制限された新たなトルク指令値Ｔ１を生成して出力する（トルク指令補正ステップ）。
【００７１】
具体的には、飽和回路７は、絶対値が乗算値Ｕ１×ｒとする新たなトルク指令値Ｔ１を出力する。例えば、ＰＩＤ制御器５で生成したトルク指令値Ｔ１が、Ｕ１×ｒを上回る場合は、新たなトルク指令値Ｔ１をＵ１×ｒとする。また、ＰＩＤ制御器５で生成したトルク指令値Ｔ１が、−Ｕ１×ｒを下回る場合は、新たなトルク指令値Ｔ１を−Ｕ１×ｒとする。これにより、トルク指令値Ｔ１が式（１１）に制限されるので、最終的に求められる各駆動力指令値ｕｅ１，ｕｆ１が、負の値となることはない。
【００７２】
そして、駆動力指令値演算部８は、飽和回路７により出力されたトルク指令値Ｔ１を用いて、各駆動力指令値ｕｅ１，ｕｆ１を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式に基づいて計算する（駆動力指令値演算ステップ）。なお、剛性指令値Ｕ１、トルク指令値Ｔ１及びモーメントアームの長さｒに対応した（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部８は、剛性指令値Ｕ１、トルク指令値Ｔ１を取得したときに、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。そして、各アクチュエータｅ１，ｆ１は、各駆動力指令値ｕｅ１，ｕｆ１に従って空気圧が調整され、各アクチュエータｅ１，ｆ１の駆動力が各駆動力指令値ｕｅ１，ｕｆ１に設定される。
【００７３】
以上の動作により、リンク１０１の揺動角度θ１が目標揺動角度ｒａに調整され、且つ関節におけるリンク１０１の剛性が目標剛性ｒｓ（＝Ｕ１×ｋ×ｒ２）に調整されように、フィードバック制御により、各駆動力指令値ｕｅ１，ｕｆ１が設定される。
【００７４】
したがって、フィードフォワード制御よりも簡便なフィードバック制御により、リンク１０１の角度制御と剛性制御の同時制御を実現することができる。これにより、リンク１０１の目標揺動角度ｒａと目標剛性ｒｓ（つまり、剛性指令値Ｕ１）が変更されても、フィードフォワード制御のように繰り返し演算を行う必要がないので、迅速にリンク１０１の揺動角度及び剛性を目標値に調整することができる。
【００７５】
ところで、フィードバック制御系のゲインが高い場合には、それによる関節の剛性が支配的になってしまう。つまり、リンク１０１に外力が作用した場合、リンク１０１が外力により目標揺動角度ｒａの位置から揺動しようとするので、仮に係数値Ｇｔ＝１としておくと、揺動角度θ１と目標揺動角度ｒａとの偏差を示す値（ｒａ−θ１）が０ではなくなる。
【００７６】
その結果、ＰＩＤ制御器５が出力するトルク指令値Ｔ１によりリンク１０１を目標揺動角度ｒａの定位置に戻すように各アクチュエータｅ１，ｆ１の駆動力が設定され、リンク１０１が定位置に位置決めされる。従って、リンク１０１に外力が作用した時、リンク１０１が移動しないように各アクチュエータｅ１，ｆ１の駆動力が設定されるので、リンク１０１の剛性が目標剛性ｒｓよりも高くなり、リンク１０１の柔軟性が損なわれてしまうこととなる。
【００７７】
そこで、ゲイン調整器４は、タッチセンサ２がリンク１０１に外力が作用したことを検知して検知信号を出力している期間は、偏差を示す値（ｒａ−θ１）に係数値Ｇｔ＝０を乗じて、実際に偏差があったとしても偏差なしを示すｒａ−θ１＝０を出力する。つまり、ゲイン調整器４は、新たな偏差を示す値（ｒａ−θ１＝０）を算出して出力する。
【００７８】
これにより、ＰＩＤ制御器５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ１を出力することとなり、リンク１０１の剛性は、目標剛性ｒｓに維持されることとなる。したがって、リンク１０１に外力が作用しても、速やかに係数値Ｇｔ＝０となり、フィードバック制御系が遮断され、リンク１０１の柔軟性が維持される。
【００７９】
なお、タッチセンサ２がＯＦＦに切り換わったときは、ゲイン調整器４における係数値がＧｔ＝１に切り換わり、実際の偏差（ｒａ−θ１）に基づいてフィードバック制御を行うこととなる。
【００８０】
（３）シミュレーション
図３の制御装置１５０を用いてシミュレーションを行った。ここで、リンク１０１の慣性モーメントをＩ＝８．３×１０−２［ｋｇｍ２］、モーメントアームの長さをｒ＝０．１［ｍ］、弾性力定数をｋ＝３、粘性力定数をｂ＝３とする。目標揺動角度ｒａはランプ状とし、初期角度４５［ｄｅｇ］から２秒間で目標揺動角度ｒａ＝６５［ｄｅｇ］に到達する位置決めを行う。
【００８１】
このとき、リンク１０１の関節の目標剛性をｒｓ＝Ｕ１×ｋ×ｒ２＝０．４×ｋ×ｒ２［Ｎｍ／ｒａｄ］と制御する。ここで、弾性力定数ｋ，モーメントアームの長さｒは定数であるので、剛性指令値Ｕ１をＵ１＝０．４として、実際の関節の剛性が０．４となるように制御すればよい。
【００８２】
図４（ａ）にリンク１０１の揺動角度θ１を実線で示し、目標揺動角度ｒａを破線で示す。また、図４（ｂ）に駆動力指令値ｕｅ１，ｕｆ１に対する各アクチュエータの実際の駆動力（収縮力、つまり応答）をそれぞれ実線、破線で示す。更に、図４（ｃ）に駆動力指令値ｕｅ１，ｕｆ１に対するアクチュエータの実際の駆動力の差を実線で、和を破線で、Ｔ１／ｒの応答を一点鎖線で示す。
【００８３】
図４（ａ）より、リンク１０１の揺動角度θ１は目標揺動角度ｒａに位置決めされていることがわかる。図４（ｂ）より、駆動力指令値ｕｅ１，ｕｆ１に対する応答はフィードバック制御系のトルク指令値Ｔ１を、式（７）と式（１０）に示した条件で配分されているが、人工筋肉の駆動力は正の値のみを取るという特性を満たしていることがわかる。図４（ｃ）において、実線で示すｕｆ１−ｕｅ１の応答は、一点鎖線で示すＴ１／ｒの応答と重なっており、式（３）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１の応答は常に０．４となっており、関節の剛性が目標剛性に制御されていることがわかる。
【００８４】
次に、リンク１０１の先端が外力により押されることを想定するシミュレーションを行う。１０秒から１１秒にかけて関節角度を正方向に回転させるトルク外乱をリンク１０１の関節に与えている。タッチセンサ２において接触が発生している間（１０秒から１１秒）、ゲイン調整器４の係数値Ｇｔを０とする。図５に関節におけるリンク１０１の目標剛性ｒｓを変化させ、剛性指令値Ｕ１を０．３，１．０，１０とした場合の応答をそれぞれ実線、破線、点線で示す。
【００８５】
実線で示す剛性指令値Ｕ１＝０．３の応答より、関節の剛性が、剛性指令値Ｕ１＝１．０，１０とした場合の剛性よりも低く制御されているため、外力により大きくリンク１０１の揺動角度が変位していることがわかる。リンク１０１への接触物の接触が発生した際に、接触力と同一方向に関節が変位していることから、接触物の接触に対して柔らかい制御が実現していることを示している。そして、目標剛性ｒｓ（つまり、剛性指令値Ｕ１）を高めるに連れて関節の剛性が高まり、接触に対するリンクの変位量は減少している。
【００８６】
本第１実施形態により、リンク１０１の目標揺動角度ｒａへの位置決めを実現しながら、自在に関節におけるリンク１０１の剛性を変化させることが可能であることがわかる。
【００８７】
比較として、式（７）を用いて駆動力指令値ｕｅ１，ｕｆ１を決定するが、式（１０）の条件を考慮せずに制御を行った場合の応答を、図６（ａ）〜図６（ｃ）に示す。式（１０）の条件なしでは、各アクチュエータｅ１，ｆ１が収縮方向にのみ力を発生する、という制約が考慮されていない。そのため、図６（ｂ）に示すように駆動力指令値は負の値を取ってしまう。各アクチュエータｅ１，ｆ１は膨張方向には力を発生しないため、負の値の場合は０としてシミュレーションを行った。
【００８８】
その結果、図６（ａ）に示すように、リンク１０１は目標揺動角度に制御されているが、図６（ｃ）に示すように、０〜０．３秒と２〜２．３秒では剛性指令値Ｕ１に対する応答は目標値の０．４とならず、関節の剛性が目標剛性に制御されていないことがわかる。
【００８９】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について詳細に説明する。図７は、本発明の第２実施形態に係るロボット装置の概略構成を示す説明図である。本第２実施形態では、人工筋肉アクチュエータを用いた３対６筋を有するリンク機構としての２リンクマニピュレータを備えたロボット装置の手先剛性の制御を例として説明する。
【００９０】
（１）モデリング
本実施の形態で扱う３対６筋を有するリンク機構としての２リンクマニピュレータを備えたロボット装置を図７に示す。ロボット装置２００は、２リンクマニピュレータＰを備えている。２リンクマニピュレータＰは、第１のリンク２０１と、第２のリンク２０２と、固定部材であるプーリ２０３とを有している。
【００９１】
第１のリンク２０１は、長手部材からなり、その基端２０１ａがプーリ２０３にｘ−ｙ直交座標系の平面（以下、「作業平面」という）内で揺動可能に支持されている。第２のリンク２０２は、長手部材からなり、その基端２０２ａが第１のリンク２０１の先端２０１ｂに作業平面内で揺動可能に支持されている。
【００９２】
この第２のリンク２０２の先端（以下、「リンク先端」という）２０２ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、第１のリンク２０１は、第１の関節と第２の関節との間に配置され、第１の関節で揺動可能に支持されており、第２のリンク２０２は、第２の関節で揺動可能に支持されている。
【００９３】
また、２リンクマニピュレータＰは、一対の第１のアクチュエータｅ１，ｆ１と、一対の第２のアクチュエータｅ２，ｆ２と、一対の第３のアクチュエータｅ３，ｆ３と、を備えている。各第１のアクチュエータｅ１，ｆ１は、一端がプーリ２０３に接続され、他端が第１のリンク２０１の長手方向中央部に接続され、駆動力の差により第１のリンク２０１を揺動させるように拮抗配置されている。
【００９４】
また、各第２のアクチュエータｅ２，ｆ２は、一端が第１のリンク２０１の長手方向中央部に接続され、他端が第２のリンク２０２の基端２０２ａに接続され、駆動力の差により第２のリンク２０２を揺動させるように拮抗配置されている。また、各第３のアクチュエータｅ３，ｆ３は、一端がプーリ２０３に接続され、他端が第２のリンク２０２の基端２０２ａに接続され、駆動力の差により第１のリンク２０１及び第２のリンク２０２を揺動させるように拮抗配置されている。つまり、第１のアクチュエータｅ１，ｆ１は、第１のリンク２０１を挟んで第１のリンク２０１の両側に対称配置されている。また、第２のアクチュエータｅ２，ｆ２は、第１のリンク２０１を挟んで第１のリンク２０１の両側に対称配置されている。また、第３のアクチュエータｅ３，ｆ３は、第１のリンク２０１を挟んで第１のリンク２０１の両側に対称配置されている。
【００９５】
また、ロボット装置２００は、各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３の駆動力を各駆動力指令値により設定して、リンク２０１，２０２の動作を制御する制御装置２５０を備えている。
【００９６】
第１のアクチュエータｅ１，ｆ１は、第１のリンク２０１を駆動する第１の一関節駆動アクチュエータである。第２のアクチュエータｅ２，ｆ２は、第２のリンク２０２を駆動する第２の一関節駆動アクチュエータである。また、第３のアクチュエータｅ３，ｆ３は、第１のリンク２０１と第２のリンク２０２を同時に駆動する二関節同時駆動アクチュエータである。人の上腕上腕部や下肢大腿部には、二関節筋とよばれる二関節同時駆動アクチュエータが存在することが知られている。人の四肢の筋配列は複雑だが、実効筋概念が導入され、３対６筋を有する２リンクモデルが提示されている。
【００９７】
各アクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３は、図１に示す筋の粘弾性特性を有する空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは、筋の粘弾性と呼ばれる特性と類似する特性を有するアクチュエータである。筋は図１に示すように、力発生要素と弾性要素と粘性要素を用いてモデル化される。
【００９８】
図７のアクチュエータｅ１，ｆ１，ｅ２，ｆ２，ｅ３，ｆ３について、ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）を力発生要素の駆動力を発生させる駆動力指令値とする。また、ｋｅｎ，ｋｆｎ，ｂｅｎ，ｂｆｎ（ｎ＝１，２，３）を人工筋肉アクチュエータの弾性力定数、粘性力定数とする。第１，第２のリンク２０１，２０２の揺動角度をθ１，θ２、第１，第２のリンク２０１，２０２の慣性モーメントをＩ１，Ｉ２、第１，第２のリンク２０１，２０２の長さを２×ｌ１，２×ｌ２、第１，第２のリンク２０１，２０２の質量をｍ１，ｍ２とする。
【００９９】
ここで、第１の揺動角度θ１は、基準軸であるｘ軸を基準とする第１のリンク２０１の揺動角度であり、第２の揺動角度θ２は、第１のリンク２０１の長手方向に延びる軸線を基準とする第２のリンク２０２の揺動角度である。
【０１００】
モーメントアームの長さ、つまり第１のリンク２０１の揺動中心点とプーリ２０３におけるアクチュエータｅ１，ｆ１の接続点との長さ、及び第２のリンク２０２の揺動中心点と第２のリンク２０２におけるアクチュエータｅ２，ｆ２の接続点との長さをｒとする。
【０１０１】
本第２実施形態では、各筋の弾性力定数，粘性力定数をいずれもｋ，ｂとすると、２リンクマニピュレータの運動方程式は、以下の式（１３）及び式（１４）となる。
【０１０２】
【数１４】
0005854695.tif 000015
【０１０３】
【数１５】
0005854695.tif 000016
【０１０４】
（２）制御系設計
２リンクマニピュレータであるロボット装置２００では、手先（ハンド）が外界と直に接触するので、手先剛性、即ちリンク先端２０２ｂの剛性を制御することが重要である。手先剛性は、図８に示すような剛性の大きさを示すスティフネス特性を示す楕円（スティフネス楕円）によって表される。
【０１０５】
本第２実施形態では、作業平面内にｘ−ｙ軸直交座標系を定義し、リンク先端２０２ｂにおけるスティフネス特性がｘ−ｙ軸直交座標系にリンク先端２０２ｂを中心とするスティフネス楕円で表される。このスティフネス楕円は、各方向に対する剛性の分布を示し、リンク先端２０２ｂとスティフネス楕円の距離が離れるほど剛性が高いことを表している。
【０１０６】
図９に本第２実施形態における制御装置２５０のブロック線図を示す。ここで、固定部材であるプーリ２０３に対する第１のリンク２０１の第１の目標揺動角度をｒａ１、第１のリンク２０１に対する第２のリンク２０２の第２の目標揺動角度をｒａ２とする。また、プーリ２０３と第１のリンク２０１との第１の関節における第１のリンク２０１の目標剛性をｒｓ１、第１のリンク２０１と第２のリンク２０２との第２の関節における第２のリンク２０２の目標剛性をｒｓ２とする。
【０１０７】
制御装置２５０は、プーリ２０３に対する第１のリンク２０１の第１の揺動角度θ１を検知する第１の揺動角度検知センサ５１と、第１のリンク２０１に対する第２のリンク２０２の第２の揺動角度θ２を検知する第２の揺動角度検知センサ６１とを備えている。また、制御装置２５０は、第２のリンク２０２に外力が作用したときに外力が作用したことを示す検知信号を出力するタッチセンサ５２を備えている。
【０１０８】
また、制御装置２５０は、第１の偏差算出部である減算器５３及びゲイン調整器５４と、第１のトルク指令生成部であるＰＩＤ制御器５５と、を備えている。また、制御装置２５０は、第１の剛性指令値Ｕ１に第１のリンク２０１の揺動中心点を中心とするモーメントアームの長さｒ１を乗算する第１の乗算部としての乗算器５６を備えている。なお、図９では、モーメントアームの長さをｒ１＝ｒとしている。
【０１０９】
また、制御装置２５０は、ＰＩＤ制御器５５が出力するトルク指令値Ｔ１を補正する第１のトルク指令補正部である飽和回路５７を備えている。また、制御装置２５０は、トルク指令値Ｔ１及び剛性指令値Ｕ１から各アクチュエータｅ１，ｆ１への駆動力指令値ｕｅ１，ｕｆ１を演算により求める第１の駆動力指令値演算部５８を備えている。
【０１１０】
同様に、制御装置２５０は、第２の偏差算出部である減算器６３及びゲイン調整器６４と、第２のトルク指令生成部であるＰＩＤ制御器６５と、を備えている。また、制御装置２５０は、第２の剛性指令値Ｕ２に第２のリンク２０２の揺動中心点を中心とするモーメントアームの長さｒ２を乗算する第２の乗算部としての乗算器６６を備えている。なお、図９では、モーメントアームの長さをｒ２＝ｒとしている。
【０１１１】
また、制御装置２５０は、ＰＩＤ制御器６５が出力するトルク指令値Ｔ２を補正する第２のトルク指令補正部である飽和回路６７を備えている。また、制御装置２５０は、トルク指令値Ｔ２及び剛性指令値Ｕ２から各アクチュエータｅ２，ｆ２への駆動力指令値ｕｅ２，ｕｆ２を演算により求める第２の駆動力指令値演算部６８を備えている。
【０１１２】
以下、各部について詳細に説明する。まず、各揺動角度検知センサ５１，６１は、ポテンショメータ又はエンコーダ等のセンサであり、図７において、図示は省略しているが、各関節に設けられている。
【０１１３】
タッチセンサ５２は、不図示のエンドエフェクタ（例えばハンド）又は第２のリンク２０２に設けられ、エンドエフェクタ又は第２のリンク２０２に接触物が接触したときに、検知信号を出力する。つまり、第２のリンク２０２に外力が作用するとは、エンドエフェクタ又は第２のリンク２０２に接触物が接触することを意味する。
【０１１４】
減算器５３は、第１の揺動角度θ１と第１の目標揺動角度ｒａ１との第１の偏差（差分）を示す値（ｒａ１−θ１）を算出する。同様に、減算器６３は、第２の揺動角度θ２と第２の目標揺動角度ｒａ２との第２の偏差（差分）を示す値（ｒａ２−θ２）を算出する。
【０１１５】
ゲイン調整器５４は、減算器５３が出力した第１の偏差を示す値（ｒａ１−θ１）に接触ゲインである第１の係数値Ｇｔ１を乗じて出力する。同様に、ゲイン調整器６４は、減算器６３が出力した第２の偏差を示す値（ｒａ２−θ２）に接触ゲインである第２の係数値Ｇｔ２を乗じて出力する。各係数値Ｇｔ１，Ｇｔ２は、０以上１以下の範囲内の値であり、ゲイン調整器５４，６４は、タッチセンサ５２から検知信号がない場合には、係数値Ｇｔ１，Ｇｔ２＝１として、入力した偏差を示す値（ｒａ１−θ１），（ｒａ２−θ２）を出力する。
【０１１６】
また、ゲイン調整器５４，６４は、タッチセンサ５２から検知信号が出力されている期間は、偏差を示す値（ｒａ１−θ１），（ｒａ２−θ２）に０以上１以下の範囲内の値の係数値、本実施形態では係数値Ｇｔ１，Ｇｔ２＝０を乗じる。これにより、ゲイン調整器５４，６４は、新たな偏差を示す値（ｒａ１−θ１＝０，ｒａ２−θ２＝０）を出力する。
【０１１７】
ＰＩＤ制御器５５は、第１の偏差を示す値（ｒａ１−θ１）に対してＰＩＤ制御演算を行い、第１のリンク２０１を揺動させる第１のトルク指令値Ｔ１を生成する。即ち、フィードバック制御系であるＰＩＤ制御器５５は、第１のリンク２０１の第１の揺動角度θ１と第１の目標揺動角度ｒａ１との第１の偏差を補償するための（つまり、揺動角度θ１を目標揺動角度ｒａ１に近づけるための）第１のトルク指令値Ｔ１を出力する。
【０１１８】
同様に、ＰＩＤ制御器６５は、第２の偏差を示す値（ｒａ２−θ２）に対してＰＩＤ制御演算を行い、第２のリンク２０２を揺動させる第２のトルク指令値Ｔ２を生成する。即ち、フィードバック制御系であるＰＩＤ制御器６５は、第２のリンク２０２の第２の揺動角度θ２と第２の目標揺動角度ｒａ２との第２の偏差を補償するための（つまり、揺動角度θ２を目標揺動角度ｒａ２に近づけるための）第２のトルク指令値Ｔ２を出力する。
【０１１９】
第１のアクチュエータｅ１，ｆ１及び第２のアクチュエータｅ２，ｆ２の制御方法は、上記第１実施形態と同様である。第ｎのアクチュエータｅｎ，ｆｎに対する駆動力指令値ｕｅｎ，ｕｆｎ、第ｎのトルク指令値Ｔｎ、第ｎの剛性指令値Ｕｎ（ただし、ｎ＝１，２）は、以下の式（１５）及び式（１６）の関係にある。
【０１２０】
【数１６】
0005854695.tif 000017
【０１２１】
【数１７】
0005854695.tif 000018
【０１２２】
そして、トルク指令値Ｔ１と剛性指令値Ｕ１とを同時に満たすには、駆動力指令値ｕｆ１＝（Ｕ１＋Ｔ１／ｒ１）／２，駆動力指令値ｕｅ１＝（Ｕ１−Ｔ１／ｒ１）／２を満たせばよい。また、トルク指令値Ｔ２と剛性指令値Ｕ２とを同時に満たすには、駆動力指令値ｕｆ２＝（Ｕ２＋Ｔ２／ｒ２）／２，駆動力指令値ｕｅ２＝（Ｕ２−Ｔ２／ｒ２）／２を満たせばよい。
【０１２３】
そして、各駆動力指令値は、以下の式（１７）の条件を満たす必要がある。
【０１２４】
【数１８】
0005854695.tif 000019
【０１２５】
つまりトルク指令値Ｔｎは、以下の式（１８）の条件となる。
【０１２６】
【数１９】
0005854695.tif 000020
【０１２７】
従って、式（１８）の条件を満たしていれば、関節におけるリンク２０１，２０２の剛性が目標剛性ｒｓ１，ｒｓ２となると同時に、トルク指令値Ｔ１，Ｔ２によりリンク２０１，２０２の揺動角度が目標角度ｒａ１，ｒａ２に位置決めされる。
【０１２８】
本第２実施形態では、乗算器５６は、モーメントアームの長さｒ１と第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１を算出し、飽和回路５７は、式（１８）を実現するために、トルク指令値Ｔ１を、以下の式（１９）の値に制約する。同様に、乗算器６６は、モーメントアームの長さｒ２と第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２を算出し、飽和回路６７は、式（１８）を実現するために、トルク指令値Ｔ２を、以下の式（１９）の値に制約する。
【０１２９】
【数２０】
0005854695.tif 000021
【０１３０】
即ち、飽和回路５７は、第１のトルク指令値Ｔ１の絶対値を、モーメントアームの長さｒ１と第１の剛性指令値Ｕ１との第１の乗算値Ｕ１×ｒ１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ１を補正する。より具体的には、飽和回路５７は、制約関数として飽和関数に従い、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１の絶対値が第１の乗算値Ｕ１×ｒ１以下の場合は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１をそのまま出力する。また、飽和回路５７は、飽和関数に従い、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１の絶対値が第１の乗算値Ｕ１×ｒ１を上回った場合は、絶対値が第１の乗算値Ｕ１×ｒ１以下に制限された新たな第１のトルク指令値Ｔ１を生成して出力する。
【０１３１】
同様に、飽和回路６７は、第２のトルク指令値Ｔ２の絶対値を、モーメントアームの長さｒ２と第２の剛性指令値Ｕ２との第２の乗算値Ｕ２×ｒ２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ２を補正する。より具体的には、飽和回路６７は、制約関数として飽和関数に従い、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２の絶対値が第２の乗算値Ｕ２×ｒ２以下の場合は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２をそのまま出力する。また、飽和回路６７は、飽和関数に従い、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２の絶対値が第２の乗算値Ｕ２×ｒ２を上回った場合は、絶対値が第２の乗算値Ｕ２×ｒ２以下に制限された新たな第２のトルク指令値Ｔ２を生成して出力する。
【０１３２】
ここで、第３のアクチュエータｅ３，ｆ３に対する駆動力指令値ｕｅ３，ｕｆ３に差を与えることによって、第１，第２のアクチュエータと同様に関節にトルクを与えることは可能である。しかし、一つのリンクを複数の位置フィードバック制御系が制御することは冗長であり、フィードバック制御系の設計が複雑になってしまう。
【０１３３】
そこで、本第２実施形態では第３のアクチュエータｅ３，ｆ３は各関節に剛性を与えるために用いる。そこで、駆動力指令値ｕｅ３，ｕｆ３の差を０、第３の剛性指令値をＵ３とすると、以下の式（２０）の関係となる。
【０１３４】
【数２１】
0005854695.tif 000022
【０１３５】
これを解くと、駆動力指令値ｕｅ３，ｕｆ３は、
【０１３６】
【数２２】
0005854695.tif 000023
となる。
【０１３７】
そこで、本第２実施形態では、制御装置２５０は、各第３のアクチュエータｅ３，ｆ３の駆動力指令値ｕｅ３，ｕｆ３を、式（２１）に示すように、第３の剛性指令値Ｕ３の１／２に設定する第３の駆動力指令値演算部７８を備えている。
【０１３８】
上記第１実施形態の１リンクマニュピレータでは、関節の剛性と手先の剛性は同一であった。本第２実施形態の３対６筋を有する２リンクマニュピレータでは、剛性指令値Ｕ１，Ｕ２，Ｕ３を調整することにより手先の剛性が求まる。
【０１３９】
例えば、剛性指令値Ｕ１，Ｕ２，Ｕ３を
【０１４０】
【数２３】
0005854695.tif 000024
と調整すると、スティフネス楕円の長軸は第１関節と手先を結ぶ方向を向くことが知られている。
【０１４１】
以下、スティフネス楕円の長軸及び短軸のうちの一方の軸がｘ−ｙ軸直交座標系における基準軸であるｘ軸と平行になる剛性指令値Ｕ１，Ｕ２，Ｕ３を求める。
【０１４２】
ｘ−ｙ軸に平行な微小な外力ΔＦｘ，ΔＦｙによるリンク先端２０２ｂの微小な変位をΔｘｔ，Δｙｔとする。また、微小な外力ΔＦｘ，ΔＦｙによる、各リンク２０１，２０２の微小な回転角度をΔθ１，Δθ２とする。微小な回転が起こると、筋の粘弾性を有するアクチュエータは、筋の弾性力によりリンクに微小なトルクΔＴｐ１，ΔＴｐ２を発生させ、以下のような式（２３）及び式（２４）で表せる。
【０１４３】
【数２４】
0005854695.tif 000025
【０１４４】
これを行列で表記し、微小な角度とトルクの関係を剛性行列ＫＳで表すと、以下の式（２５）となる。
【０１４５】
【数２５】
0005854695.tif 000026
【０１４６】
ここで、ヤコビ行列Ｊを導入するが、ヤコビ行列Ｊは、以下の式（２６）で表される。
【０１４７】
【数２６】
0005854695.tif 000027
【０１４８】
これより、微小手先変位と微小外力との関係をコンプライアンス行列ＪＳで表すと、以下の式（２７）となる。
【０１４９】
【数２７】
0005854695.tif 000028
【０１５０】
コンプライアンス行列の逆行列が剛性を表すので、スティフネス楕円がｘ−ｙ軸に平行であるという条件は、
【０１５１】
【数２８】
0005854695.tif 000029
となる。この方程式を剛性指令値Ｕ３について解くと、
【０１５２】
【数２９】
0005854695.tif 000030
となる。
【０１５３】
以下、タッチセンサ５２が検知信号を出力していない場合（タッチセンサ５２がＯＦＦ）について、図９を参照しながら制御装置２５０の動作を説明する。まず、第１の揺動角度検知センサ５１が、第１のリンク２０１の第１の揺動角度θ１を検知する（第１の揺動角度検知ステップ）。同様に、第２の揺動角度検知センサ６１が、第２のリンク２０２の第２の揺動角度θ２を検知する（第２の揺動角度検知ステップ）。
【０１５４】
そして、減算器５３が、第１のリンク２０１の第１の揺動角度θ１と第１の目標揺動角度ｒａ１との第１の偏差を示す値（ｒａ１−θ１）を算出する（第１の偏差算出ステップ）。同様に、減算器６３が、第２のリンク２０２の第２の揺動角度θ２と第２の目標揺動角度ｒａ２との第２の偏差を示す値（ｒａ２−θ２）を算出する（第２の偏差算出ステップ）。
【０１５５】
タッチセンサ５２がＯＦＦであるので、各ゲイン調整器５４，６４は、乗算する各係数値Ｇｔ１，Ｇｔ２が１であり、各偏差を示す値（ｒａ１−θ１），（ｒａ２−θ２）をそのまま出力する。
【０１５６】
ＰＩＤ制御器５５は、ゲイン調整器５４から取得した偏差を示す値（ｒａ１−θ１）に対してＰＩＤ制御演算を行い、第１のリンク２０１を揺動させる第１のトルク指令値Ｔ１を生成する（第１のトルク指令生成ステップ）。同様に、ＰＩＤ制御器６５は、ゲイン調整器６４から取得した偏差を示す値（ｒａ２−θ２）に対してＰＩＤ制御演算を行い、第２のリンク２０２を揺動させる第２のトルク指令値Ｔ２を生成する（第２のトルク指令生成ステップ）。
【０１５７】
乗算器５６は、取得した第１の剛性指令値Ｕ１と第１のリンク２０１のモーメントアームの長さｒ１との第１の乗算値Ｕ１×ｒ１を算出する（第１の乗算ステップ）。同様に、乗算器６６は、取得した第２の剛性指令値Ｕ２と第２のリンク２０２のモーメントアームの長さｒ２との第２の乗算値Ｕ２×ｒ２を算出する（第２の乗算ステップ）。
【０１５８】
ここで、モーメントアームの長さｒ１，ｒ２は、予め設定（不図示の記憶装置に記憶）された定数である。乗算器５６が剛性指令値Ｕ１を取得したときに、演算により乗算値Ｕ１×ｒ１を求め、乗算器６６が剛性指令値Ｕ２を取得したときに、演算により乗算値Ｕ２×ｒ２を求める。なお、予め乗算値Ｕ１×ｒ１の計算結果、乗算値Ｕ２×ｒ２の計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておいてもよい。そして、乗算器５６が剛性指令値Ｕ１を取得したときにテーブルデータを参照して乗算値Ｕ１×ｒ１を求め、乗算器６６が剛性指令値Ｕ２を取得したときにテーブルデータを参照して乗算値Ｕ２×ｒ２を求めてもよい。
【０１５９】
飽和回路５７は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１の絶対値が第１の乗算値Ｕ１×ｒ１以下の場合は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１をそのまま出力する。飽和回路５７は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１の絶対値が第１の乗算値Ｕ１×ｒ１を上回った場合は、絶対値が第１の乗算値Ｕ１×ｒ１以下に制限された新たな第１のトルク指令値Ｔ１を生成して出力する（第１のトルク指令補正ステップ）。なお、図９では、モーメントアームの長さをｒ１＝ｒとしている。
【０１６０】
具体的には、飽和回路５７は、絶対値が乗算値Ｕ１×ｒ１とする新たな第１のトルク指令値Ｔ１を出力する。例えば、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１が、Ｕ１×ｒ１を上回る場合は、新たな第１のトルク指令値Ｔ１をＵ１×ｒ１とする。また、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ１が、−Ｕ１×ｒ１を下回る場合は、新たなトルク指令値Ｔ１を−Ｕ１×ｒ１とする。これにより、第１のトルク指令値Ｔ１が式（１８）に制限されるので、最終的に求められる各駆動力指令値ｕｅ１，ｕｆ１が、負の値となることはない。
【０１６１】
同様に、飽和回路６７は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２の絶対値が第２の乗算値Ｕ２×ｒ２以下の場合は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２をそのまま出力する。飽和回路６７は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ２の絶対値が第２の乗算値Ｕ２×ｒ２を上回った場合は、絶対値が第２の乗算値Ｕ２×ｒ２以下に制限された新たな第２のトルク指令値Ｔ２を生成して出力する（第２のトルク指令補正ステップ）。なお、図９では、モーメントアームの長さをｒ２＝ｒとしている。
【０１６２】
そして第１の駆動力指令値演算部５８は、飽和回路５７により出力された第１のトルク指令値Ｔ１を用いて、各駆動力指令値ｕｅ１，ｕｆ１を（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式に基づいて計算する（第１の駆動力指令値演算ステップ）。同様に、第２の駆動力指令値演算部６８は、飽和回路６７により出力された第２のトルク指令値Ｔ２を用いて、各駆動力指令値ｕｅ２，ｕｆ２を（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式に基づいて計算する（第２の駆動力指令値演算ステップ）。なお、剛性指令値Ｕ１、トルク指令値Ｔ１及びモーメントアームの長さｒ１に対応した（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。また、剛性指令値Ｕ２、トルク指令値Ｔ２及びモーメントアームの長さｒ２に対応した（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部５８は、剛性指令値Ｕ１、トルク指令値Ｔ１を取得したときに、（Ｕ１＋Ｔ１／ｒ１）／２，（Ｕ１−Ｔ１／ｒ１）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。また、駆動力指令値演算部６８は、剛性指令値Ｕ２、トルク指令値Ｔ２を取得したときに、（Ｕ２＋Ｔ２／ｒ２）／２，（Ｕ２−Ｔ２／ｒ２）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。
【０１６３】
以上のフィードバック制御により、各駆動力指令値ｕｅ１，ｕｆ１が設定されることで、リンク２０１の揺動角度θ１が目標揺動角度ｒａ１に調整され、且つ第１の関節におけるリンク２０１の剛性が目標剛性ｒｓ１（＝Ｕ１×ｋ×ｒ１２）に調整される。同様に、フィードバック制御により、各駆動力指令値ｕｅ２，ｕｆ２が設定されることで、リンク２０２の揺動角度θ２が目標揺動角度ｒａ２に調整され、且つ第２の関節におけるリンク２０２の剛性が目標剛性ｒｓ２（＝Ｕ２×ｋ×ｒ２２）に調整される。
【０１６４】
したがって、フィードフォワード制御よりも簡便なフィードバック制御により、リンク２０１，２０２の角度制御と剛性制御の同時制御を実現することができる。これにより、リンク２０１，２０２の目標揺動角度と目標剛性（つまり、剛性指令値Ｕ１，Ｕ２）が変更されても、フィードフォワード制御のように繰り返し演算を行う必要がないので、迅速にリンクの揺動角度及び関節の剛性を目標値に調整することができる。
【０１６５】
また、ゲイン調整器５４は、タッチセンサ５２が第２のリンク２０２に外力が作用したことを検知して検知信号を出力している期間は、第１の偏差を示す値（ｒａ１−θ１）に第１の係数値Ｇｔ１＝０を乗じて出力する。つまり、ゲイン調整器５４は、実際に偏差があったとしても偏差なしを示す新たな第１の偏差を示す値（ｒａ１−θ１＝０）を算出して出力する。
【０１６６】
同様に、ゲイン調整器６４は、タッチセンサ５２が第２のリンク２０２に外力が作用したことを検知して検知信号を出力している期間は、第２の偏差を示す値（ｒａ２−θ２）に第２の係数値Ｇｔ２＝０を乗じて出力する。つまり、ゲイン調整器６４は、実際に偏差があったとしても偏差なしを示す新たな第２の偏差を示す値（ｒａ２−θ２＝０）を算出して出力する。
【０１６７】
これにより、各ＰＩＤ制御器５５，６５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ１，Ｔ２を出力することとなる。したがって、リンク２０１，２０２の剛性は、目標剛性に維持され、リンク２０１，２０２に外力が作用しても、速やかに係数値Ｇｔ１，Ｇｔ２＝０となり、フィードバック制御系が遮断され、リンク２０１，２０２の柔軟性が維持される。
【０１６８】
なお、タッチセンサ５２がＯＦＦに切り換わったときは、各ゲイン調整器５４，６４における係数値がＧｔ１，Ｇｔ２＝１に切り換わり、実際の偏差（ｒａ１−θ１），（ｒａ２−θ２）に基づいてフィードバック制御を行うこととなる。
【０１６９】
（３）シミュレーション
図９に示した制御装置２５０を用いてシミュレーションを行った。第１のリンク２０１と第２のリンク２０２の物理パラメータは同一とする。リンク２０１，２０２の長さを０．５［ｍ］、リンク２０１，２０２の慣性モーメントを８．３×１０−２［ｋｇｍ２］、モーメントアームの長さｒ（＝ｒ１＝ｒ２）を０．１［ｍ］、弾性力定数をｋ＝３、粘性力定数をｂ＝３とする。
【０１７０】
第１，第２の目標揺動角度はランプ状とし、第１のリンク２０１は初期角度４５［ｄｅｇ］から２秒間で目標揺動角度ｒａ１＝３５［ｄｅｇ］に到達する位置決めを行う。また、第２のリンク２０２は初期角度９０［ｄｅｇ］から２秒間で目標揺動角度ｒａ２＝８５［ｄｅｇ］に到達する位置決めを行う。
【０１７１】
リンクが位置決めされる目標角度において、リンク先端２０２ｂにおけるスティフネス楕円がｘ−ｙ軸と水平となるように、各アクチュエータの剛性指令値Ｕ１，Ｕ２，Ｕ３を制御する。一関節駆動アクチュエータの剛性指令値Ｕ１，Ｕ２は任意に設定し、Ｕ１＝１０，Ｕ２＝５に設定した。このとき、二関節駆動アクチュエータの剛性指令値Ｕ３は、式（２９）に基づいて、剛性指令値Ｕ３＝１．９０７となるように制御すればよい。
【０１７２】
図１０（ａ），図１０（ｂ）に、第１，第２のリンク２０１，２０２の揺動角度θ１，θ２を実線で、目標揺動角度ｒａ１，ｒａ２を破線で示す。また、図１１（ａ）〜図１１（ｃ）に、駆動力指令値ｕｆ１，ｕｆ２，ｕｆ３の応答を実線で、駆動力指令値ｕｅ１，ｕｅ２，ｕｅ３の応答を破線で示す。さらに、図１２（ａ）〜図１２（ｃ）に、駆動力指令値ｕｅｎ，ｕｆｎ（ｎ＝１，２，３）の応答の差を実線で、応答の和を破線で、Ｔｎ／ｒ（ｎ＝１，２）の応答を一点鎖線で示す。
【０１７３】
まず、図１０（ａ）及び図１０（ｂ）よりリンク２０１，２０２は目標揺動角度に位置決めされていることがわかる。次に、図１１（ａ）より、駆動力指令値ｕｅ１，ｕｆ１は、トルク指令値Ｔ１を｜Ｔ１｜≦Ｕ１×ｒに制限した状態で（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式を用いて配分されている。従って、第１のアクチュエータｅ１，ｆ１の駆動力は正の値を取るという特性を満たしていることがわかる。
【０１７４】
また、図１１（ｂ）より、第２のアクチュエータｅ２，ｆ２の駆動力も同様に正の値を取っていることがわかる。また、図１１（ｃ）より、第３のアクチュエータｅ３，ｆ３の駆動力は一定値のＵ３／２である。
【０１７５】
また、図１２（ａ）において、実線で示すｕｆ１−ｕｅ１の応答は、一点鎖線で示すＴ１／ｒの応答と重なっており、式（１５）の条件を満たしている。また、破線で示すｕｆ１＋ｕｅ１＝Ｕ１の応答は常に設定値である１０となっている。
【０１７６】
同様に、図１２（ｂ）において、実線で示すｕｆ２−ｕｅ２の応答は、一点鎖線で示すＴ２／ｒの応答と重なっており、式（１５）の条件を満たしている。また、破線で示すｕｆ２＋ｕｅ２＝Ｕ２の応答は常に設定値である５となっている。
【０１７７】
更に、図１２（ｃ）において、実線で示すｕｆ３−ｕｅ３の応答は０であり、式（２０）の条件を満たしている。また、破線で示すｕｆ３＋ｕｅ３＝Ｕ３の応答は常に１．９０７となっている。各アクチュエータの拮抗対による剛性が目標値に制御されていることから、リンク先端２０２ｂのスティフネス楕円は目標角度において、ｘ−ｙ軸と平行になるように制御されていることがわかる。
【０１７８】
次に、リンク先端２０２ｂが外力により押されることを想定するシミュレーションを行う。各リンク２０１，２０２が目標角度に静定後、リンク先端２０２ｂにｙ軸負方向の外力を与える。接触が発生している間、係数値Ｇｔ１，Ｇｔ２を０としている。
【０１７９】
図１３にリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。位置決めにおいては、目標軌道に追従している。接触の発生により、係数値Ｇｔ１，Ｇｔ２を０とすると、マニピュレータは人工筋肉アクチュエータの弾性力によって手先の剛性が制御される。本第２実施形態では、手先剛性はｘ−ｙ軸方向に平行になるように剛性指令値Ｕ１，Ｕ２，Ｕ３を設定し、設定値を満たすように３対６筋の駆動力を制御している。そのため、ｙ軸負方向の外乱に対して、手先はｙ軸に平行な方向に柔軟性を発揮していることがわかる。
【０１８０】
［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について説明する。図１４は、本発明の第３実施形態に係るロボット装置の制御装置を示すブロック線図である。なお、本第３実施形態において、上記第２実施形態と同様の構成については、同一符号を付して説明を省略する。
【０１８１】
上記第２実施形態では、関節が目標角度に到達した後、接触ゲインである係数値Ｇｔ１，Ｇｔ２を０としてフィードバック制御系を遮断し、筋の弾性による手先剛性の制御を実現した。しかし、目標軌道に追従しながらリンクに柔軟性を持たせるものではない。また、目標角度においてのみスティフネス楕円がｘ−ｙ軸に平行になる制御を行っていた。
【０１８２】
（１）制御系設計
本第３実施形態では、リンク先端２０２ｂを目標軌道に追従させながら剛性の制御を同時に行う。また、軌道追従中であっても、スティフネス楕円を常にｘ−ｙ軸に平行になるように制御する。
【０１８３】
図１４に示す制御装置２５０Ａは、演算部７０を備えている。演算部７０は、第１，第２のリンク２０１，２０２に対する目標揺動角度（目標軌道）ｒａ１，ｒａ２に応じて常にスティフネス楕円の軸がｘ−ｙ軸と平行になるように第３の剛性指令値Ｕ３を以下の式（３０）に示す演算式により演算する。
【０１８４】
【数３０】
0005854695.tif 000031
【０１８５】
つまり、演算部７０は、図８に示したｘ−ｙ軸直交座標系で、リンク先端２０２ｂにおけるスティフネス楕円の長軸及び短軸のうちの一方の軸が、直交座標系の基準軸（例えばｘ軸）と平行となるように、剛性指令値Ｕ３を演算式により求める。この式（３０）に示す演算式は、第１の剛性指令値Ｕ１、第２の剛性指令値Ｕ２、第１の目標揺動角度ｒａ１、及び第２の目標揺動角度ｒａ２を変数として、第３の剛性指令値Ｕ３を求める演算式である。
【０１８６】
そして、各パラメータＵ１，Ｕ２，ｒａ１，ｒａ２を入力した演算部７０は、式（３０）の演算式に基づき、第３の剛性指令値Ｕ３を演算により生成する（剛性指令値生成ステップ）。そして、第３の駆動力指令値演算部７８は、各駆動力指令値ｕｅ３，ｕｆ３を、第３の剛性指令値Ｕ３に基づいて設定する（第３の駆動力指令値演算ステップ）。本実施形態では、第３の駆動力指令値演算部７８は、各駆動力指令値ｕｅ３，ｕｆ３を、第３の剛性指令値Ｕ３に１／２を乗じて得られる値に設定する。これにより、各リンク２０１，２０２を各目標揺動角度ｒａ１，ｒａ２に基づいて揺動させるときに、スティフネス楕円の軸がｘ−ｙ軸に平行となる状態を保つことができる。
【０１８７】
更に、本第３実施形態ではゲイン調整器５４，６４の係数値Ｇｔ１，Ｇｔ２を、タッチセンサ５２から検知信号が出力されている期間は、以下の式（３１）に示すように、０から１の間で変化させた任意の値とする。
【０１８８】
【数３１】
0005854695.tif 000032
【０１８９】
係数値Ｇｔ１，Ｇｔ２に微小な値をとれば、スティフネス楕円の軸は微小にｘ−ｙ軸から傾くものの、接触物の接触中に各リンク２０１，２０２の目標軌道への追従が可能となる。
【０１９０】
特に、第１の係数値Ｇｔ１及び第２の係数値Ｇｔ２は、タッチセンサ５２から検知信号が出力されている期間中、その期間における初期の時点の初期値から時間が経過するに連れて大きくなるように設定されるのがよい。
【０１９１】
例えば、図１５に示すように、タッチセンサ５２から検知信号が出力されている期間を時刻４秒から時刻８秒の期間とする。その期間における時刻４秒の時点から時間が経過するに連れて、係数値Ｇｔ１，Ｇｔ２が大きくなるように設定される。図１５に示す例では、各係数値Ｇｔ１，Ｇｔ２が段階的に大きくなるように設定される。
【０１９２】
本第３実施形態では、係数値Ｇｔ１，Ｇｔ２は、タッチセンサ５２から検知信号が出力されている期間中においても、０とはならないように設定されており、所定の時間が経過したら、１となるように設定される。これにより、接触物が第２のリンク２０２又はエンドエフェクタに接触した瞬間は、係数値Ｇｔ１，Ｇｔ２が１よりも低く設定されて、各リンク２０１，２０２の柔軟性を保つようにしている。そして、係数値Ｇｔ１，Ｇｔ２は０ではないので、スティフネス楕円の軸は微小にｘ−ｙ軸から傾くものの、接触物の接触中に各リンク２０１，２０２の目標軌道への追従が可能となる。
【０１９３】
次いで、所定の時間が経過したら、接触中であっても係数値Ｇｔ１，Ｇｔ２を１とし、各リンク２０１，２０２を目標揺動角度及び目標剛性に調整される。従って、各リンク２０１，２０２は、接触が生じた瞬間は柔軟性を保ち、その後は迅速に目標の位置に位置決めされるので、制御性が向上する。
【０１９４】
（２）シミュレーション
図１４に示した制御装置２５０Ａを用いてシミュレーションを行った。リンク２０１，２０２の物理パラメータは上記第２実施形態と同様である。目標軌道（目標揺動角度）は、５秒までは上記第２実施形態と同様の軌道である。
【０１９５】
その後１１秒までリンク先端２０２ｂの軌道がｘ軸と平行な直線軌道となるように各リンク２０１，２０２の目標軌道（目標揺動角度）ｒａ１，ｒａ２を逆運動学により求めている。常にリンク先端２０２ｂのスティフネス楕円の軸がｘ−ｙ軸と水平になるように、各人工筋肉アクチュエータを剛性指令値Ｕ１，Ｕ２，Ｕ３により制御する。一関節駆動アクチュエータの剛性は実施例１と同様に、剛性指令値Ｕ１＝１０，Ｕ２＝５とし、剛性指令値Ｕ３を式（３０）より逐次求める。また、接触は４秒から８秒にかけて発生するものとする。そのとき、係数値Ｇｔ１，Ｇｔ２は、図１５に示すように０以外の値を用いる。
【０１９６】
図１６（ａ）に第１のリンク２０１の角度θ１を実線で、目標角度ｒａ１を一点鎖線で示す。また、図１６（ｂ）に第２のリンク２０２の角度θ２を実線で、目標角度ｒａ２を一点鎖線で示す。また、比較として、第２実施形態で示した、接触中は常に係数値Ｇｔ１，Ｇｔ２を０とする制御（「接触ゲイン０制御」と称す）の応答を破線で示す。さらに、図１７に駆動力ｕｅ３，ｕｆ３の差を実線で、和を破線で示す。
【０１９７】
図１６（ａ）及び図１６（ｂ）より、本第３実施形態の制御装置２５０Ａは、４秒で接触が発生するとフィードバック制御系が遮断されてリンク先端２０２ｂの柔らかさを発揮する。そして、第１のリンク２０１は６秒で、第２のリンク２０２は７秒で係数値を１としてフードバック制御系の遮断を解除しているため、８秒以降は目標軌道へと収束している。
【０１９８】
一方、破線で示す接触ゲイン０制御では、接触が発生している８秒までフィードバック制御が遮断されている。そのため、８秒以降に大きくなった角度偏差に対して急激に目標軌道への復帰動作が発生し、大きなオーバシュートを発生している。
【０１９９】
図１７において、破線で示すｕｆ３＋ｕｅ３＝Ｕ３は式（３０）に示したように目標角度ｒａ１，ｒａ２に応じて逐次演算されている。これにより、スティフネス楕円は軌道追従中において、ｘ−ｙ軸と平行になるように制御されている。
【０２００】
図１８（ａ）にリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。比較として、図１８（ｂ）に接触ゲイン０制御のリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。
【０２０１】
本第３実施形態の制御装置２５０Ａでは、接触が発生した後にリンク先端２０２ｂが柔軟性を持つが、同時に軌道追従を行い、リンク先端２０２ｂをｘ軸に水平な方向の軌道に追従させていることがわかる。接触ゲイン０制御では、接触中は軌道制御を行わないため、リンク先端２０２ｂの柔軟性は大きいが、目標軌道から大きく外れてしまうことがわかる。
【０２０２】
［第４実施形態］
次に、本発明の第４実施形態に係るロボット装置について説明する。図１９は、本発明の第４実施形態に係るロボット装置の制御装置における要部を示すブロック線図である。なお、本第４実施形態において、上記第２，第３実施形態と同様の構成については、同一符号を付して説明を省略する。
【０２０３】
上記第２実施形態ではフィードバック制御系の入力部を遮断したが、十分に関節が静定していない状態では、遮断時にスパイク状のトルク指令値を発生する可能性がある。上記第２，第３実施形態では、第１のトルク指令生成部がＰＩＤ制御器５５であり、第２のトルク指令生成部がＰＩＤ制御器６５である場合について説明したが、本第４実施形態では、第１，第２のトルク指令生成部の構成が異なるものである。
【０２０４】
具体的に説明すると、第１のトルク指令生成部４５５は、ＰＩＤ制御器５５と、ＰＩＤ制御器５５の出力側に設けられ、ＰＩＤ制御器５５が出力した演算結果に第１の係数値Ｇｔ１を乗じるゲイン調整器５５Ａとを有している。
【０２０５】
また、第１のトルク指令生成部４５５は、ＰＩＤ制御器５５の制御入力をフィードフォワード入力ｆｆｗ１として入力し、その入力ｆｆｗ１に（１−Ｇｔ１）の値を乗じるゲイン調整器５５Ｂを有している。また、第１のトルク指令生成部４５５は、ゲイン調整器５５Ａの出力値とゲイン調整器５５Ｂの出力値とを加算処理し、加算結果を第１のトルク指令値Ｔ１として出力する加算器５５Ｃを有している。
【０２０６】
第２のトルク指令生成部４６５は、第１のトルク指令生成部４５５と構成が同一であり、図１９を参照して説明する。第２のトルク指令生成部４６５は、ＰＩＤ制御器６５と、ＰＩＤ制御器６５の出力側に設けられ、ＰＩＤ制御器６５が出力した演算結果に第２の係数値Ｇｔ２を乗じるゲイン調整器６５Ａとを有している。
【０２０７】
また、第２のトルク指令生成部４６５は、ＰＩＤ制御器６５の制御入力をフィードフォワード入力ｆｆｗ２として入力し、その入力ｆｆｗ２に（１−Ｇｔ２）の値を乗じるゲイン調整器６５Ｂを有している。また、第２のトルク指令生成部４６５は、ゲイン調整器６５Ａの出力値とゲイン調整器６５Ｂの出力値とを加算処理し、加算結果を第２のトルク指令値Ｔ２として出力する加算器６５Ｃを有している。
【０２０８】
ここで、フィードフォワード入力ｆｆｗｎ（ｎ＝１，２）は、タッチセンサ５２が検知信号を出力する直前にＰＩＤ制御器５５，６５が出力したＰＩＤ制御演算による演算結果である。
【０２０９】
これにより、タッチセンサ５２から検知信号が出力されている期間は、ＰＩＤ制御器５５は、第１の偏差を示す値（ｒａ１−θ１）に対してＰＩＤ制御演算を行うが、ゲイン調整器５５Ａは、その演算結果に対して第１の係数値Ｇｔ１を乗じる。ゲイン調整器５５Ｂは、フィードフォワード入力ｆｆｗ１を入力し、その入力ｆｆｗ１に（１−Ｇｔ１）の値を乗じる。加算器５５Ｃは、ゲイン調整器５５Ａの出力値とゲイン調整器５５Ｂの出力値との加算結果を第１のトルク指令値Ｔ１として出力する。
【０２１０】
同様に、タッチセンサ５２から検知信号が出力されている期間は、ＰＩＤ制御器６５は、第２の偏差を示す値（ｒａ２−θ２）に対してＰＩＤ制御演算を行うが、ゲイン調整器６５Ａは、その演算結果に対して第２の係数値Ｇｔ２を乗じる。ゲイン調整器６５Ｂは、フィードフォワード入力ｆｆｗ２を入力し、その入力ｆｆｗ２に（１−Ｇｔ２）の値を乗じる。加算器６５Ｃは、ゲイン調整器６５Ａの出力値とゲイン調整器６５Ｂの出力値との加算結果を第２のトルク指令値Ｔ２として出力する。
【０２１１】
具体例を挙げて説明すると、タッチセンサ５２が接触を検知して係数値Ｇｔｎ＝０となった場合は、（１−Ｇｔｎ）＝１となり、トルク指令生成部４５５，４６５が出力するトルク指令値Ｔｎは、タッチセンサ５２が接触を検知する直前の値となる。
【０２１２】
従って、各リンク２０１，２０２の動作中にタッチセンサ５２により接触が検知された場合には、偏差が０となる場合のトルク指令値で制御するフィードバック制御に移行するのではなく、接触が生じる直前の偏差に基づくトルク指令値でフィードバック制御する。なお、上記第３実施形態と同様に、検知信号を検知してから時間が経過するに連れて、係数値Ｇｔ１，Ｇｔ２を大きくしていってもよい。そして、接触が生じた瞬間に係数値Ｇｔ１，Ｇｔ２＝０とならない、１よりも小さな値に係数値Ｇｔ１，Ｇｔ２を設定してもよい。
【０２１３】
以上、本第４実施形態によれば、各リンク２０１，２０２の動作中に、各リンク２０１，２０２の柔軟性を損なうことなく、トルク指令値Ｔ１，Ｔ２のスパイクを防止することができ、制御性が更に向上する。
【０２１４】
［第５実施形態］
次に、本発明の第５実施形態に係るロボット装置について説明する。上記第１〜４実施形態では、ロボット装置が作業ロボットとしてのマニピュレータである場合について説明したが、上記第１〜第４実施形態に示した制御装置を、歩行ロボットの脚制御に用いることが可能である。
【０２１５】
歩行ロボットでは地面との接地時に脚の剛性を低くすることで、不整地に対する歩行の安定性が高まる。また、接地時にスティフネス楕円を地面と平行にすることにより、地面に対する滑りを抑制することが可能である。しかし、支持脚相では脚の剛性を高くして、体を支持する必要がある。
【０２１６】
これに対して、図２０に、上記第３実施形態で示した剛性と追従の同時制御を適用した場合のシミュレーションによるスティック線図を示す。破線で脚先の軌道を、点線で目標軌道を示している。これより、接地時は脚の剛性は低いため柔らかく地面に接地し、支持脚相が進むにつれて脚の剛性が高まり、目標軌道に追従していることがわかる。
【０２１７】
［第６実施形態］
次に、本発明の第６実施形態に係るロボット装置について説明する。上記第１〜第５実施形態では、拮抗配置された人工筋肉アクチュエータを用いて位置決めと剛性の同時制御を行った。本第６実施形態では、関節に回転モータを用いる１リンクアームに対して、本発明の制御系を適用する。
【０２１８】
ロボット装置の構成は、駆動部として、図２に示すアクチュエータｅ１，ｆ１の代わりに、図２１のブロック線図に示す回転モータＰ０に置き換えたものである。なお、ロボット装置の制御装置は、回転モータＰ０を駆動するモータ回路Ｐ１を有している。回転モータＰ０は、図２を参照し、固定部材であるアーム部材１０４とリンク１０１との関節に配置され、リンク１０１を揺動させる駆動部である。１リンクアームの運動方程式を以下の式（３２）のように定義する。
【０２１９】
【数３２】
0005854695.tif 000033
【０２２０】
ここで、ｕｍは回転モータが発生する制御トルクである。つぎに、比例ゲインをｋ、微分ゲインをｂ、仮想的なモーメントアームの長さをｒ、仮想的な筋の駆動力を発生させる駆動力指令値をｕｅ１，ｕｆ１として、１リンクアームに対して以下の式（３３）のような比例−微分フィードバック制御系を構成する。
【０２２１】
【数３３】
0005854695.tif 000034
【０２２２】
ここで、第１項はトルク指令値、第２項、第３項は仮想的な筋の駆動力指令値ｕｅ１，ｕｆ１によりゲインが変動する比例−微分フィードバック制御系となっている。
【０２２３】
図２１に示すモータ回路Ｐ１は、式（３２）と式（３３）で構成されるフィードバック制御系である。モータ回路Ｐ１は、駆動力指令値ｕｅ１，ｕｆ１を入力として、回転モータＰ０を動作させ、回転モータＰ０にリンク１０１を駆動させて揺動角度θ１を出力とするシステムであることがわかる。式（３２）及び式（３３）は、第１実施形態の式（２）で示した人工筋肉アクチュエータを用いた１リンクアームの運動方程式と同一である。これより、回転モータを用いた１リンクアームに対して、式（３３）のように比例−微分フィードバックを施すことで、第１実施形態に示した位置と剛性の同時制御方法をそのまま適用することができることがわかる。
【０２２４】
具体的に説明すると、図２１に示すモータ回路Ｐ１は、回転モータＰ０を、駆動力の差によりリンク１０１を揺動させる一対の仮想アクチュエータと見做せるように構成されている。そして、図３に示す制御装置１５０と同一構成の制御装置によって、各仮想アクチュエータの駆動力を、各駆動力指令値ｕｅ１，ｕｆ１により調整して、リンク１０１の動作を制御する。
【０２２５】
図３を参照して、制御装置の各部の動作について説明すると、まず、揺動角度検知センサ１は、揺動角度（関節の角度）θ１を検知する。偏差算出部としての減算器３は、アーム部材１０４に対するリンク１０１の揺動角度θ１と目標揺動角度ｒａとの偏差（差分）を示す値を算出する（偏差算出ステップ）。
【０２２６】
トルク指令生成部としてのＰＩＤ制御器５は、偏差を示す値に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ１を生成する（トルク指令生成ステップ）。
【０２２７】
乗算部としての乗算器６は、リンク１０１の揺動中心点を中心とする仮想のモーメントアームの長さｒと、各駆動力指令値ｕｅ１，ｕｆ１の和を示す剛性指令値Ｕ１との乗算値Ｕ１×ｒを算出する（乗算ステップ）。仮想のモーメントアームの長さｒは、予め設定（不図示の記憶装置に記憶）された定数であり、乗算器６が剛性指令値Ｕ１を取得したときに、演算により乗算値Ｕ１×ｒを求める。なお、予め乗算値Ｕ１×ｒの計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておき、乗算器６が剛性指令値Ｕ１を取得したときにテーブルデータを参照して乗算値Ｕ１×ｒを求めてもよい。
【０２２８】
トルク指令補正部としての飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１をそのまま出力する。また、飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ１の絶対値が乗算値Ｕ１×ｒを上回った場合は、絶対値が乗算値Ｕ１×ｒ以下に制限された新たなトルク指令値Ｔ１を出力する（トルク指令補正ステップ）。
【０２２９】
駆動力指令値演算部８は、飽和回路７により出力されたトルク指令値Ｔ１を用いて、各駆動力指令値ｕｅ１，ｕｆ１を、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式に基づいて計算する（駆動力指令値演算ステップ）。なお、剛性指令値Ｕ１、トルク指令値Ｔ１及び仮想モーメントアームの長さｒに対応した（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部８は、剛性指令値Ｕ１、トルク指令値Ｔ１を取得したときに、（Ｕ１＋Ｔ１／ｒ）／２，（Ｕ１−Ｔ１／ｒ）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。
【０２３０】
これにより、フィードフォワード制御よりも簡便なフィードバック制御により、リンク１０１の角度制御と関節におけるリンク１０１の剛性制御の同時制御を実現することができる。これにより、リンク１０１の揺動角度と剛性の目標値が変更されても、繰り返し演算を行う必要がないので、迅速にリンク１０１の揺動角度及び剛性を目標値に調整することができる。
【０２３１】
また、本第６実施形態では、制御装置は、ゲイン調整器４を有し、タッチセンサ２から検知信号が出力されている期間は、偏差を示す値に０を乗じて新たな偏差（ｒａ−θ１＝０）を示す値を算出する。これにより、ＰＩＤ制御器５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ１を出力することとなり、リンク１０１の剛性は、目標剛性ｒｓに維持されることとなる。したがって、リンク１０１に外力が作用しても、速やかに係数値Ｇｔ＝０となり、フィードバック制御系が遮断され、リンク１０１の柔軟性が維持される。
【０２３２】
［第７実施形態］
上記第１〜第６実施形態では、式（１１）および式（１８）に示した制御入力の制約を行うために、図２２の破線で示す飽和関数を用いてきた。しかし、図２２の破線で示す飽和関数のように微分不可能な関数を用いて制御入力の制約を行うと、制約時に制御入力が急峻に変動してしまうことがある。このような制御入力は、制御対象の機械系高次振動を励起し、制御系が不安定となることがある。
【０２３３】
そこで、本第７実施形態では、制約関数として、制御入力の制約に微分可能な関数である双曲線正接関数を用いる。双曲線正接関数を図２２の実線で示す。この双曲線正接関数は、漸近線が乗算値±Ｕｎ×ｒｎ（ｎ＝１，２）である。双曲線正接関数で制約されるトルク指令値（フィードバック制御トルク）Ｔｔｎ（ｎ＝１，２）は、制約される前のトルク指令値Ｔｎ、制御入力の和Ｕｎおよびプーリ径ｒを用いて以下のように表すことができる。
【０２３４】
【数３４】
0005854695.tif 000035
【０２３５】
上記第１実施形態では１リンクアームの制御を行った。本第７実施形態では、第１実施形態で用いた図３のブロック線図中の飽和関数を、±Ｕ１×ｒを漸近線とする双曲線正接関数に置き換えシミュレーションを行う。図２３（ａ）に関節の角度θ１と目標角度ｒａを示し、実線で双曲線正接関数を用いる関節角度の応答を、破線で飽和関数を用いた関節角度の応答を、点線で目標角度を示す。また、図２３（ｂ）に駆動力ｕｆ１の、制約に双曲線正接関数と飽和関数を用いた応答をそれぞれ実線、破線で示す。駆動力ｕｆ１は、トルク指令値Ｔ１により式（８）を用いて決定される。そのため、実線で示す制約に双曲線正接関数を用いる応答では、トルクＴ１が制約される約０．７ｓと約２．５ｓにおいて、駆動力ｕｆ１の応答に急峻な変動が現れなくなっていることがわかる。また、図２３（ａ）より、制御入力の飽和に双曲線正接関数を用いても、位置決め性能に劣化はないことがわかる。
【０２３６】
上記第２実施形態では２リンクアームの制御を行った。１リンクアームの制御と同様に、本第７実施形態では、上記第２実施形態で用いた図９のブロック線図中の飽和関数を、双曲線正接関数に置き換えシミュレーションを行う。具体的には、第１の制約関数が、±Ｕ１×ｒ１を漸近線とする双曲線正接関数であり、第２の制約関数が、±Ｕ２×ｒ２を漸近線とする双曲線正接関数である。
【０２３７】
図２４（ａ），（ｂ）に、第１及び第２のリンク２０１，２０２の関節角度θ１，θ２と目標角度ｒａ１，ｒａ２を示し、実線で双曲線正接関数を用いる応答を、破線で飽和関数を用いた応答を、点線で目標角度を示す。また、図２５（ａ）及び図２５（ｂ）に駆動力ｕｆ１，ｕｆ２の、双曲線正接関数と飽和関数を用いた応答をそれぞれ実線、破線で示す。図２５（ａ）より、第１のリンク２０１に対するトルクＴ１が制約される約０．２ｓと約２．２ｓにおいて、双曲線正接関数により駆動力ｕｆ１の応答に急峻な変動が現れなくなっていることがわかる。上記第２実施形態では、第２のリンク２０２に対するトルクＴ２が制約の範囲内であるため、駆動力ｕｆ２は制約される必要がない。しかし、図２２より双曲線正接関数は制約の範囲内であっても信号を低減する効果がある。そのため、図２５（ｂ）に示す本第７実施形態の双曲線正接関数による駆動力ｕｆ２は飽和関数による駆動力と若干の差異が生じているが、図２４（ａ），（ｂ）から双曲線正接関数を用いても位置決め性能に変化はないことがわかる。
【０２３８】
［第８実施形態］
本第８実施形態では、接触ゲインの調整が不要で、また、大きな障害物への着地であっても緩やかな脚制御を可能とする歩行制御系を設計する。さらに、４足歩行ロボットのモデルを用いてシミュレーションを行い、本第８実施形態で提案している制御系により不整地歩行が可能であることを示す。
【０２３９】
（１）モデリング
図２６は、本発明の第８実施形態に係るロボット装置としての４足歩行ロボットのリンク配置と筋配置および座標系を示す側面図である。ロボット装置としての４足歩行ロボット８００は、固定部材としての胴体８０３と、４つの支持脚となる４つのリンク機構ＲＦ，ＬＦ，ＲＨ，ＬＨとを備えている。これら４つのリンク機構ＲＦ，ＬＦ，ＲＨ，ＬＨが胴体８０３に接続されている。
【０２４０】
ここで、絶対座標系は、ｘ−ｙ平面については紙面内に取り、ｚ座標は紙面に垂直で紙面の裏から表の向きに取る。座標（ｘＧ，ｙＧ，ｚＧ）は胴体８０３の重心の位置を表し、本第８実施形態では胴体８０３の重心を中心としてｘ軸まわり右ねじ方向の傾きをロール、ｚ軸まわり右ねじ方向の傾きをピッチ、ｙ軸まわり右ねじ方向の傾きをヨーと記す。
【０２４１】
本第８実施形態では、右後脚がリンク機構ＲＨ、右前脚がリンク機構ＲＦ、左後脚がリンク機構ＬＨ、左前脚がリンク機構ＬＦである。これら右後脚、右前脚、左後脚及び左前脚に対して、それぞれ支持脚にローカルなｘＲＨ−ｙＲＨ、ｘＲＦ−ｙＲＦ、ｘＬＨ−ｙＬＨおよびｘＬＦ−ｙＬＦ座標系を設定する。図２６にはｘＲＨ−ｙＲＨ座標系およびｘＲＦ−ｙＲＦ座標系のみ図示している。ここで支持脚に対する座標系は、ｘ−ｙ座標の軸がロボット装置の胴体８０３の底面部分と水平になるように設定する。
【０２４２】
本第８実施形態で制御対象とする４足歩行ロボット８００は、前脚ＲＦ，ＬＦは２リンクマニピュレータ、後脚ＲＨ，ＬＨは３リンクマニピュレータとするが、後脚は第３リンクの角度が９０ｄｅｇに固定された等価的な２リンクマニピュレータとして用いる。そのため、前脚と後脚の双方について、上記第２実施形態に示した３対６筋を有する２リンクマニピュレータに対する制御手法を適用することが可能である。
【０２４３】
また、脚先端部には球状の足を設定する。尾と首には質量を設定しているが、頭部には質量を設定していない。この４足歩行ロボットは機構解析ソフトウェアＲｅｃｕｒｄｙｎを用いてモデル化され、本第８実施形態において不整地歩行シミュレーションに用いる。
【０２４４】
前脚ＲＦ，ＬＦおよび後脚ＲＨ，ＬＨの第ｎ（ｎ＝１，２）のリンクのパラメータは、以下のとおりである。具体的には、リンク長ｌＦｎ，ｌＨｎは、ｌＦ１＝０．０７０ｍ，ｌＦ２＝０．１２ｍ，ｌＨ１＝０．０９０ｍ，ｌＨ２＝０．１２３５ｍである。また、慣性モーメントＩＦｎ，ＩＨｎは、ＩＦ１＝１１．７４×１０−６ｋｇ・ｍ２，ＩＦ２＝５２．９５９×１０−６ｋｇ・ｍ２，ＩＨ１＝２２．８１×１０−６ｋｇ・ｍ２，ＩＨ２＝９４．３５×１０−６ｋｇ・ｍ２である。また、質量ＭＦｎ，ＭＨｎは、ＭＦ１＝２．１６×１０−２ｋｇ，ＭＦ２＝３．６２９８×１０−２ｋｇ，ＭＨ１＝２．７１×１０−２ｋｇ，ＭＨ２＝５．２５５８×１０−２ｋｇである。また、胴体８０３のｘ，ｚ方向の長さは、ｌｂｘ＝０．２８ｍ，ｌｂｚ＝０．０５ｍ、ｚ方向の脚の付け根の間隔は、ｌｂｌ＝０．１５ｍ、質量ＭｂはＭｂ＝０．７３０９ｋｇ、プーリ径、足球半径は、ｒ＝０．０２ｍ，ｒｆ＝０．００８ｍである。人工筋肉アクチュエータの弾性力定数、粘性力定数は、ｋ＝１５０．５Ｎ／ｍ，ｂ＝０．０１Ｎ・ｓ／ｍである。なお、人工筋肉アクチュエータの弾性力定数は、神田通信工業製マッキベン型アクチュエータ０．２５インチタイプを測定して求めている。
【０２４５】
（２）制御系設計
まず、各支持脚ＲＦ〜ＬＨに対して、足先の軌道とスティフネス楕円を設定する。軌道は、遊脚相では足先が楕円の上半分を描く軌道とし、支持脚相では直線軌道とする。また、歩幅は０．０６ｍと設定する。本第８実施形態では、不整地において、支持脚に予期しない着地が生じた際に胴体８０３と垂直な方向に剛性を発揮させることで安定的な歩行を行う。そのため、スティフネス楕円は、上記第２実施形態で示した方法を用いて、各支持脚のローカルな座標系に垂直になるように設定する。
【０２４６】
図２７（ａ）に、右後脚であるリンク機構ＲＨのスティック線図を実線で、足先軌道を一点鎖線で、スティフネス楕円を破線で示す。後脚ＲＨはｘ軸正方向が胴体８０３の進行方向となるため、足先は図２７（ａ）中矢印の方向に駆動される。そこで、スティフネス楕円を足先が遊脚相の８割程度の位置でｘＨＲ−ｙＨＲ座標系に垂直になるように設定する。
【０２４７】
まず、一関節駆動アクチュエータの剛性を、Ｕ１Ｒ＝１３，Ｕ２Ｒ＝１５と設定すると、二関節駆動アクチュエータの剛性は、式（２９）により、Ｕ３Ｒ＝８．５３７７と求まる。ここで、一関節駆動アクチュエータの剛性は、上記第２実施形態で示した２リンクマニピュレータモデルを用いて、ロボット胴体重量の１／４を手先外乱として与えるシミュレーションを行うことによって決定している。左後脚ＬＨは、右後脚ＲＨと同様の軌道とスティフネス楕円を用いることができる。
【０２４８】
同様に、図２７（ｂ）に右前脚であるリンク機構ＲＦのスティック線図を実線で、足先軌道を一点鎖線で、スティフネス楕円を破線で示す。前脚ＲＦはｘ軸負方向が胴体８０３の進行方向となるため、足先は図２７（ｂ）中矢印の方向に駆動される。前脚ＲＦのアクチュエータの剛性は後脚と同様に、Ｕ１Ｆ＝１３，Ｕ２Ｆ＝１５と設定すると、二関節駆動アクチュエータの剛性は、式（２９）により、Ｕ３Ｆ＝５５．８０となる。しかし、この値は一関節駆動アクチュエータの剛性Ｕ１Ｆ，Ｕ２Ｆに比べて大きく、二関節駆動アクチュエータの剛性により脚の動作範囲を狭めることになってしまう。そこで、前脚ＲＦに対しては二関節アクチュエータの剛性を１／４に緩和し、Ｕ３Ｆ′＝１３．９５を用いる。
【０２４９】
つぎに、本第８実施形態で用いる歩容を図２８（ａ）の歩容位相ダイアグラムに示す。太線部分が接地面に接地している支持脚相を、細線部分が接地面から離れている遊脚相を表している。歩行周期は１秒とする。本第８実施形態では、不整地歩行を考慮して、接地率β＝０．８とし、右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦの順に遊脚化するウェーブ歩容を用いる。さらに、各支持脚における支持脚相を図２８（ｂ）に示すように複数（４つ）の支持脚相部分Ａ，Ｂ，Ｃ，Ｄに分割する。本第８実施形態では、図２８（ａ）に三角印で示す支持脚相部分Ａの終了時刻で、その時刻での足先位置から支持脚相Ｂの終了位置までの直線軌道を再計算する。そして、斜線部分示している支持脚相Ｂでは、再計算された軌道を用いる。
【０２５０】
つぎに、図２９のブロック線図により、接触ゲインＧｔ、即ち、第１の係数値Ｇｔ１及び第２の係数値Ｇｔ２の設定方法を示す。なお、図２９には、４つの支持脚ＲＦ〜ＬＦのうちのいずれか１つの支持脚（リンク機構）Ｐであって、制御装置のうち第１及び第２のアクチュエータのいずれかの制御系を示している。
【０２５１】
ここで、支持脚Ｐは、２リンクマニピュレータである。制御系Ｋ＾は、上記第２実施形態で示した制御系と同様である。ＫはＰＩＤ制御器５５（６５）と筋指令配分部分５６〜５８（６６〜６８）である。ゲイン調整器５４（６４）は接触ゲインＧｔを調整する。また、Ｍｅｍｏｒｙは予め設定したリンク機構Ｐの各リンクの軌道データが記憶されている記憶部８２である。Ｃｏｍｐｅｎｓａｔｏｒは軌道データを再計算する計算部８３である。
【０２５２】
具体的に説明すると、制御装置８５０は、上記第２実施形態の制御装置２５０の構成のほか、更に、第１の目標揺動角度ｒａ１（及び第２の目標揺動角度ｒａ２）を出力する目標設定部８１を備えている。目標設定部８１は、予め軌道データが記憶されている記憶部８２を有している。目標設定部８１は、遊脚相、第１の支持脚相部分Ａ、第３の支持脚相部分Ｃ及び第４の支持脚相部分Ｄでは、記憶部８２に記憶された、即ち、予め設定された軌道データに基づき、第１の目標揺動角度ｒａ１（及び第２の目標揺動角度ｒａ２）を出力する。また、目標設定部８１は、第２の支持脚相部分Ｂでは、軌道データを修正し、修正された軌道データに基づいて第１の目標揺動角度ｒａ１（及び第２の目標揺動角度ｒａ２）を設定する（目標設定ステップ）。
【０２５３】
また、制御装置８５０は、テーブル記憶部８５を有している。テーブル記憶部８５には、遊脚相及び複数の支持脚相部分と、センサ５２からの検知信号の有無に応じた係数値Ｇｔ（Ｇｔ１，Ｇｔ２）を決定するためのテーブルデータＴａが記憶されている。ゲイン調整器５４（６４）は、テーブルデータＴａを参照して、支持脚Ｐの脚相の状態、センサ５２からの検知信号の有無に応じて、係数値Ｇｔ（Ｇｔ１，Ｇｔ２）を決定する。
【０２５４】
具体的には、第１の偏差算出部の一部として機能するゲイン調整器５４は、リンク機構Ｐが遊脚相又は残りの３つの支持脚が接地している状態で支持脚相となった場合に、センサ５２から検知信号が出力されている期間は、第１の係数値Ｇｔ１を０に決定する。具体的にはゲイン調整器５４は、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２から検知信号が出力されている期間は、第１の係数値Ｇｔ１を０に決定する。また、ゲイン調整器５４は、それ以外、即ち、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２からの出力がＯＦＦになったときには、第１の係数値Ｇｔ１を１に決定する。さらに、ゲイン調整器５４は、それ以外、即ち、リンク機構Ｐが支持脚相部分Ｂ，Ｃ，Ｄの場合には、センサ５２からの出力がＯＮ／ＯＦＦに関わらず、第１の係数値Ｇｔ１を１に決定する。
【０２５５】
また、第２の偏差算出部の一部として機能するゲイン調整器６４は、リンク機構Ｐが遊脚相又は残りの３つの支持脚が接地している状態で支持脚相となった場合に、センサ５２から検知信号が出力されている期間は、第２の係数値Ｇｔ２を０に決定する。具体的にはゲイン調整器６４は、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２から検知信号が出力されている期間は、第２の係数値Ｇｔ２を０に決定する。また、ゲイン調整器６４は、それ以外、即ち、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２からの出力がＯＦＦになったときには、第２の係数値Ｇｔ２を１に決定する。さらに、ゲイン調整器６４は、それ以外、即ち、リンク機構Ｐが支持脚相部分Ｂ，Ｃ，Ｄの場合には、センサ５２からの出力がＯＮ／ＯＦＦに関わらず、第２の係数値Ｇｔ２を１に決定する。
【０２５６】
このように接触ゲインを変更することにより、遊脚相および第１の支持脚相部分Ａでは支持脚Ｐが地面と接触すると、フィードバックループが遮断される。これにより軌道追従は中断され、人工筋肉アクチュエータの粘弾性力のみによる衝撃吸収動作と胴体支持が行われる。
【０２５７】
そして、支持脚相部分Ｂに移行すると、支持脚の接触状態にかかわらず、軌道制御が再開され、再計算された軌道データを用いて制御がなされる。つまり、目標設定部８１は、第１の係数値Ｇｔ１及び第２の係数値Ｇｔ２が０から１に変更されたときに、第１の偏差及び第２の偏差に基づき軌道データを再計算し、第１の目標揺動角度ｒａ１及び第２の目標揺動角度ｒａ２を修正して出力する。この修正動作、つまり軌道データの再計算は、支持脚相部分Ｂの終了位置を目標としている。これにより、支持脚相Ｄの終了位置を目標とするよりも、腰または肩の落ち込みを防ぐことが可能である。支持脚相部分Ｃ、Ｄにおいては、予め設定された軌道データに基づく追従制御がなされ、胴体が前方向に移動する。
【０２５８】
また、地面に大きな障害物が存在する場合、遊脚相の初期に接触が発生する。そのとき、接触ゲインを０として支持脚相へ移行することとなるが、接触が解除されたとき、目標軌道と足先位置の誤差が大きな状態である。そこで、本第８実施形態では、軌道データ、即ち、第１の目標揺動角度ｒａ１及び第２の目標揺動角度ｒａ２を修正しているので、足先が急峻に復帰しようとするのを防ぐことができる。そのため、４足歩行ロボット８００が大きな障害物を乗り越えるときに平衡を崩すことなく、移動することができる。また、本第８実施形態では、接触ゲインＧｔは０と１のみが用いられるため、試行錯誤による設定の必要がない。
【０２５９】
（３）シミュレーション
前節の制御系を用いて不整地歩行のシミュレーションを行う。不整地として高さ１４ｍｍ、ｘ方向の幅７０ｍｍの板状の障害物を設定する。Ｒｅｃｕｒｄｙｎによる４足歩行ロボット８００と障害物Ｏの外観を図３０に示す。障害物Ｏは後脚ＲＨ，ＬＨからｘ方向に０．１ｍの位置に設置し、第２歩行周期で左後脚ＲＦから接触する。
【０２６０】
図２９に示した制御系を用いて、６秒間の歩行シミュレーションを行う。障害物Ｏとの接触が生じる後脚の時刻歴応答を、左後脚第１リンク角度θＬＨ１と第２リンク角度θＬＨ２の応答を図３１（ａ），（ｂ）に、右後脚第１リンク角度θＲＨ１と第２リンク角度θＲＨ２の応答を図３１（ｃ），（ｄ）に示す。関節角度の応答を実線で、目標軌道を破線で示している。なお、図２９中のテーブルによって接触ゲインが０とされ、フィードバックループが遮断された時間帯では、軌道制御は行わないため目標軌道は示していない。図３１（ａ），（ｂ）より、左後脚の１．５秒から開始される歩行周期において、遊脚相の途中の約１．６秒で障害物に接地している。その後、１．７５秒で支持脚相Ｂが開始するまでフィードバックループが遮断され、人工筋肉アクチュエータの粘弾性のみによる着地が行われている。そして、１．７５秒の時点での足先位置から、支持脚相Ｂの終了地点までの直線軌道が計算され、その軌道に追従するように制御が行われている。図３１（ｃ），（ｄ）より、右後脚ＲＨでは、障害物Ｏから降りる３〜３．２秒の遊脚相において、遊脚相の途中で地面と接触する。ここでも、接触時に人工筋肉アクチュエータの粘弾性のみによる着地が行われている。
【０２６１】
図３２（ａ），（ｂ）に、平地歩行時と、障害物乗り上げ時の左後脚足球中心の軌跡を実線で、目標軌道を破線で示す。図３２（ａ）の平地歩行では、足先は軌道へ追従制御されている。図３２（ｂ）の障害物乗り上げ時では、脚ＬＨが遊脚相の途中で障害物Ｏに接触すると、足先軌道がｙＬＨ座標で負の方向に変位している。これより、人工筋肉アクチュエータの粘弾性のみによる衝撃吸収着地動作が行われていることがわかる。そして、支持脚相Ｂの開始時間で、その時点での足先位置から支持脚相Ｂの終了位置までの直線軌道が計算され、その軌道に追従するように制御がなされる。本第８実施形態の制御系により、予期しない着地に対する柔軟な衝撃吸収動作と、軌道を再計算することによる急峻な乗り上げの抑制が実現していることがわかる。
【０２６２】
つぎに、このような脚制御の４足歩行ロボットに対する有効性を図３３（ａ），（ｂ）に示す足の接地状態と図３４（ａ）〜図３４（ｃ）に示す胴体重心の座標を用いて検証する。各脚の接地の状態を図３３（ａ）に歩容位相ダイアグラムを用いて示す。比較として、４足歩行ロボットの各脚に対して、あらかじめ設定した目標関節軌道のみを用いて脚を軌道制御する歩行シミュレーションを行い、その時の各脚の接地の状態を図３３（ｂ）に示す。図３３（ａ），（ｂ）において、白色部分は遊脚相を示し、灰色部分は支持脚相を示す。また、黒枠で囲まれた灰色部分は障害物Ｏに接地する支持脚相を示す。そして、歩行シミュレーションにおいて足が接地している状態を実線で示している。また、ＲＨ、ＲＦ、ＬＨ、ＬＦはそれぞれ右後脚、右前脚、左後脚、左前脚を表す。
【０２６３】
図３４（ａ）〜図３４（ｃ）に、胴体重心のロール角度、進行方向の変位および速度の応答を示す。ここで、本第８実施形態の制御手法による応答を実線で、関節軌道の追従のみによる歩行制御による応答を破線で示している。
【０２６４】
図３３（ａ）に示す本第８実施形態の制御手法による応答より、４足歩行ロボットは右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦの順に遊脚化することで歩行を行う。まず左後脚ＬＨが１．７秒で障害物Ｏに着地し、右後脚ＲＨは２．２秒で障害物Ｏに着地していることがわかる。そして、２．８秒で左後脚ＬＨが障害物Ｏから降りるが、その際に図３４（ａ）に実線で示すように、胴体８０３が左方向に大きくロールする。その結果、その後の左前脚ＬＦの遊脚相では、脚は遊脚化できず着地状態が続くため、人工筋肉アクチュエータの粘弾性のみによる着地動作が行われる。これにより、胴体８０３のロールが抑制され、姿勢を回復することができる。その結果、図３３（ａ）より、その後右後脚ＲＨから続く全ての遊脚相において、脚が遊脚化され、歩行を継続できていることがわかる。図３４（ａ）からも、障害物Ｏを降りた４〜６秒のロール角度の変動幅は、障害物Ｏに乗る前のロール角度の変動幅とほぼ同様であることがわかる。また、図３４（ｂ），（ｃ）より、障害物Ｏの前後で速度が落ちることなく前方に歩行していることがわかる。
【０２６５】
関節軌道の追従のみによる歩行制御では、図３３（ｂ）に示す着地状態より、４足歩行ロボットは本実施例の提案手法と同様に２．８秒で左後脚ＬＨが障害物Ｏから降りている。そして、その際に胴体８０３が左方向に大きくロールし、その後の左前脚ＬＦは遊脚化されていない。しかし、着地状態でも遊脚軌道に追従しようと制御されるためロールは抑制されず、さらに、左前脚ＬＦで地面を押すこととなる。その結果、右方向のロールが発生し、３．２秒の右前脚ＲＦの遊脚相においても、右前脚ＲＦは遊脚化されず地面を押し、３．８秒で再び左方向のロールを発生させる。胴体８０３のロール角度は、図３４（ａ）に破線で示すように、２．８秒で発生したロールが４．８秒付近まで抑制されていないことがわかる。そのため、図３３（ｂ）より、３秒以降は遊脚相で足が着地している状態が多く、歩行できない状態が４．５秒まで継続している。図３４（ｃ）に破線で示すように、２．８〜４．８秒にかけて、遊脚相において着地中に軌道制御が継続されることにより、脚が地面を押すため胴体重心に負方向の速度が発生していることがわかる。その結果、図３４（ｂ）に破線で示すように、２．８秒〜４．８秒にかけて前方への歩行が停滞していることがわかる。
【０２６６】
歩行シミュレーションにおける、足球の中心の軌跡を図３５（ａ），（ｂ）に示す。右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦをそれぞれ実線、点線、破線、一点鎖線で、さらに障害物Ｏを含む地面を太実線で示す。図３５（ａ）に示す本第８実施形態の制御系による応答では、障害物Ｏにより停滞せずに歩行が可能であることがわかる。一方、図３５（ｂ）に関節軌道の追従のみによる歩行による応答では、障害物Ｏから降りる時点以降で足先が後方に変位していることがわかる。これは、遊脚相で足が着地し、そのまま軌道制御が継続されてしまい、地面から押し戻されてしまうためである。
【０２６７】
図３６（ａ）に本第８実施形態の制御手法によるロボットの姿勢推移を、４．２秒から０．５秒おきに示す。比較として、図３６（ｂ）に関節軌道の追従のみによる歩行制御の姿勢推移を示す。本第８実施形態の制御手法では、段差を降りた後に安定した姿勢で歩行を行うことが可能である。一方、関節軌道の追従のみによる歩行制御では、上から５コマ目に示す段差を降りて２秒後の時点でも大きく姿勢を崩していることがわかる。
【０２６８】
図３７に胴体重心の重力方向の加速度を、本第８実施形態の制御手法を実線で、関節軌道の追従のみによる歩行制御を点線で示す。本第８実施形態の制御手法では、人工筋肉アクチュエータの粘弾性による柔らかな着地を実現しているため、重力方向の加速度変動が少なく、また加速度の最大値も小さいことがわかる。
【０２６９】
このように、本第８実施形態の制御手法により、柔らかな着地と急峻な乗り上げの抑制が実現されることから、障害物Ｏなどが存在する不整地であっても安定的な歩行を実現可能であることがわかる。
【０２７０】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。上記第１実施形態では、リンク機構が１リンク構成の場合について説明したが、多リンク構成であっても適用可能である。例えば、リンク機構が２リンク構成の場合において、２対４筋のアクチュエータとすることも可能であり、この場合、各一対のアクチュエータにおいて、それぞれ第１実施形態と同様に制御すればよい。また、上記第４，第５，第８実施形態では、リンク機構が２リンク構成の場合について説明したが、１リンク構成の場合についても適用可能である。また、制御装置の各部が個別のハードウェアで実現される場合について説明したが、ＣＰＵ及びメモリ等を有するコンピュータにおいてＣＰＵがメモリに記憶されたプログラムを実行することにより、ＣＰＵが制御装置の各部として機能する場合であってもよい。
【符号の説明】
【０２７１】
１…揺動角度検知センサ、２…タッチセンサ（センサ）、３…減算器（偏差算出部）、４…ゲイン調整器（偏差算出部）、５…ＰＩＤ制御器（トルク指令生成部）、６…乗算器（乗算部）、７…飽和回路（トルク指令補正部）、８…駆動力指令値演算部、５１…第１の揺動角度検知センサ、５２…タッチセンサ（センサ）、５３…減算器（第１の偏差算出部）、５４…ゲイン調整器（第１の偏差算出部）、５５…ＰＩＤ制御器（第１のトルク指令生成部）、５６…乗算器（第１の乗算部）、５７…飽和回路（第１のトルク指令補正部）、５８…第１の駆動力指令値演算部、６１…第２の揺動角度検知センサ、６３…減算器（第２の偏差算出部）、６４…ゲイン調整器（第２の偏差算出部）、６５…ＰＩＤ制御器（第２のトルク指令生成部）、６６…乗算器（第２の乗算部）、６７…飽和回路（第２のトルク指令補正部）、６８…第２の駆動力指令値演算部、７０…演算部、７８…第３の駆動力指令値演算部、１００…ロボット装置、１０１…リンク、１０１ａ…基端、１０１ｂ…先端、１０３…ベース部材（固定部材）、１０４…アーム部材（固定部材）、１５０…制御装置、２００…ロボット装置、２０１…第１のリンク、２０２…第２のリンク、２０３…プーリ（固定部材）、２５０…制御装置、２５０Ａ…制御装置、４５５…第１のトルク指令生成部、４６５…第２のトルク指令生成部、ｅ１，ｆ１…アクチュエータ，第１のアクチュエータ、ｅ２，ｆ２…第２のアクチュエータ、ｅ３，ｆ３…第３のアクチュエータ、Ｐ０…回転モータ
【図１】
0005854695.tif 000036
【図２】
0005854695.tif 000037
【図３】
0005854695.tif 000038
【図４】
0005854695.tif 000039
【図５】
0005854695.tif 000040
【図６】
0005854695.tif 000041
【図７】
0005854695.tif 000042
【図８】
0005854695.tif 000043
【図９】
0005854695.tif 000044
【図１０】
0005854695.tif 000045
【図１１】
0005854695.tif 000046
【図１２】
0005854695.tif 000047
【図１３】
0005854695.tif 000048
【図１４】
0005854695.tif 000049
【図１５】
0005854695.tif 000050
【図１６】
0005854695.tif 000051
【図１７】
0005854695.tif 000052
【図１８】
0005854695.tif 000053
【図１９】
0005854695.tif 000054
【図２０】
0005854695.tif 000055
【図２１】
0005854695.tif 000056
【図２２】
0005854695.tif 000057
【図２３】
0005854695.tif 000058
【図２４】
0005854695.tif 000059
【図２５】
0005854695.tif 000060
【図２６】
0005854695.tif 000061
【図２７】
0005854695.tif 000062
【図２８】
0005854695.tif 000063
【図２９】
0005854695.tif 000064
【図３０】
0005854695.tif 000065
【図３１】
0005854695.tif 000066
【図３２】
0005854695.tif 000067
【図３３】
0005854695.tif 000068
【図３４】
0005854695.tif 000069
【図３５】
0005854695.tif 000070
【図３６】
0005854695.tif 000071
【図３７】
0005854695.tif 000072