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

Description

本発明は、リンクの角度制御及び剛性制御を行うロボット装置の制御方法及びロボット装置に関するものである。好適には、作業ロボットや脚式移動ロボットに適用される。

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

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

これに対し、圧縮流体が供給されるゴム人工筋肉アクチュエータをリンクに拮抗配置し、関節の角度検出を行い、フィードバック制御することによって関節におけるリンクの揺動角度を制御する技術が開示されている（特許文献１参照）。

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

特開昭６３−６８３８２号公報 特許第３４３６３２０号公報

しかしながら、上記特許文献１では、リンクの揺動角度を検知して目標揺動角度となるようにフィードバック制御を行っているが、関節におけるリンクの揺動角度のみを制御しており、関節におけるリンクの柔軟さ（剛性）を変動させる制御は実現していない。

更に、上記特許文献２では、フィードフォワード制御によりリンクの揺動角度と関節におけるリンクの柔軟性の同時制御を行っている。フィードフォワード制御入力は、目標値との誤差を繰り返し修正することによって求められているため、その生成に要する計算量は非常に大きい。そのため、リンクの揺動角度やリンクとリンクを接続する関節の柔軟性の目標値を変更する度に、再び繰り返し演算をする必要があり、目標値を変更した際に、演算に時間を要するという問題がある。

なお、これらの問題は、リンクを駆動する駆動部として人工筋肉アクチュエータを用いた場合に限らず、駆動部として回転モータを用いて制御する場合についても同様である。

そこで、本発明は、フィードバック制御により、リンクの角度制御と関節の剛性制御を行うロボット装置の制御方法及びロボット装置を提供することを目的とするものである。

本発明のロボット装置の制御方法は、基端が固定部材に揺動可能に支持されたリンクと、前記固定部材に対して前記リンクを揺動させる駆動部と、を有するリンク機構を備え、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御するロボット装置の制御方法において、前記固定部材に対する前記リンクの揺動角度と目標揺動角度との偏差を算出する偏差算出ステップと、前記偏差を補償するトルク指令値Ｔ_１を生成するトルク指令生成ステップと、前記トルク指令値Ｔ_１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記第１及び第２の駆動力指令値の和を示す剛性指令値Ｕ_１との乗算値Ｕ_１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ_１を補正するトルク指令補正ステップと、前記トルク指令補正ステップにより補正されたトルク指令値Ｔ_１を用いて、前記各駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式により求める駆動力指令値演算ステップと、を備えたことを特徴とする。

また、本発明のロボット装置の制御方法は、基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構を備え、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御するロボット装置の制御方法において、前記固定部材に対する前記第１のリンクの第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出ステップと、前記第１の偏差を補償する第１のトルク指令値Ｔ_１を生成する第１のトルク指令生成ステップと、前記第１のトルク指令値Ｔ_１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ_１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ_１を補正する第１のトルク指令補正ステップと、前記第１のトルク指令補正ステップにより補正された第１のトルク指令値Ｔ_１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式により求める第１の駆動力指令値演算ステップと、前記第１のリンクに対する前記第２のリンクの第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出ステップと、前記第２の偏差を補償する第２のトルク指令値Ｔ_２を生成する第２のトルク指令生成ステップと、前記第２のトルク指令値Ｔ_２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ_２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ_２を補正する第２のトルク指令補正ステップと、前記第２のトルク指令補正ステップにより補正された第２のトルク指令値Ｔ_２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式により求める第２の駆動力指令値演算ステップと、を備えたことを特徴とする。

また、本発明のロボット装置は、基端が固定部材に揺動可能に支持されたリンク、前記固定部材に対して前記リンクを揺動させる駆動部を有するリンク機構と、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御する制御装置と、を備え、前記制御装置は、前記固定部材に対する前記リンクの揺動角度を検知する揺動角度検知センサと、前記揺動角度と目標揺動角度との偏差を算出する偏差算出部と、前記偏差を補償するトルク指令値Ｔ_１を生成するトルク指令生成部と、前記トルク指令値Ｔ_１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記各駆動力指令値の和を示す剛性指令値Ｕ_１との乗算値Ｕ_１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ_１を補正するトルク指令補正部と、前記トルク指令補正部で補正したトルク指令値Ｔ_１を用いて、前記各駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式により求める駆動力指令値演算部と、を有する特徴とする。

また、本発明のロボット装置は、基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構と、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御する制御装置と、を備え、前記制御装置は、前記固定部材に対する前記第１のリンクの第１の揺動角度を検知する第１の揺動角度検知センサと、前記第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出部と、前記第１の偏差を補償する第１のトルク指令値Ｔ_１を生成する第１のトルク指令生成部と、前記第１のトルク指令値Ｔ_１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ_１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ_１を補正する第１のトルク指令補正部と、前記第１のトルク指令補正部で補正した第１のトルク指令値Ｔ_１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式により求める第１の駆動力指令値演算部と、前記第２のリンクの前記第１のリンクに対する第２の揺動角度を検知する第２の揺動角度検知センサと、前記第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出部と、前記第２の偏差を補償する第２のトルク指令値Ｔ_２を生成する第２のトルク指令生成部と、前記第２のトルク指令値Ｔ_２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ_２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ_２を補正する第２のトルク指令補正部と、前記第２のトルク指令補正部で補正した第２のトルク指令値Ｔ_２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式により求める第２の駆動力指令値演算部と、を有することを特徴とする。

本発明によれば、簡便なフィードバック制御系により、リンクの角度制御と関節におけるリンクの剛性制御の同時制御を実現することができる。これにより、リンクの揺動角度と剛性の目標値が変更されても、繰り返し演算を行う必要がないので、迅速にリンクの揺動角度及び関節の剛性を目標値に調整することができる。

本発明の第１実施形態に係るロボット装置のアクチュエータの粘弾性モデルを示す図である。 本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第１実施形態に係るロボット装置の制御装置を示すブロック線図である。 本発明の第１実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）はリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は収縮力指令値に対する応答を示す図である。（ｃ）は収縮力指令値の差及び和に対する応答と、トルク指令値による応答を示す図である。 本発明の第１実施形態に係るロボット装置における剛性指令値に対する応答のシミュレーション結果を示す図である。 比較例のロボット装置におけるシミュレーション結果を示す図であり、（ａ）はリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は収縮力指令値に対する応答を示す図である。（ｃ）は収縮力指令値の差及び和に対する応答と、トルク指令値による応答を示す図である。 本発明の第２実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第２実施形態に係るロボット装置の第２のリンクの先端におけるスティフネス楕円を説明するための図である。 本発明の第２実施形態に係るロボット装置の制御装置を示すブロック線図である。 本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１のリンクの揺動角度と目標揺動角度の変化を示す図、（ｂ）は第２のリンクの揺動角度と目標揺動角度の変化を示す図である。 本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１の剛性指令値に対する応答を示す図、（ｂ）は第２の剛性指令値に対する応答を示す図、（ｃ）は第３の剛性指令値に対する応答を示す図である。 本発明の第２実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は第１の収縮力指令値の差及び和に対する応答と、第１のトルク指令値による応答を示す図である。（ｂ）は第２の収縮力指令値の差及び和に対する応答と、第２のトルク指令値による応答を示す図である。（ｃ）は第３の収縮力指令値の差及び和に対する応答を示す図である。 本発明の第２実施形態に係るロボット装置における第２のリンクの先端の軌跡と目標軌道のシミュレーション結果を示す図である。 本発明の第３実施形態に係るロボット装置の制御装置を示すブロック線図である。 本発明の第３実施形態の制御装置の各ゲイン調整器の係数値の変化を示す図である。 本発明の第３実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は外力が作用したときの第１のリンクの揺動角度を示す図であり、（ｂ）は外力が作用したときの第２のリンクの揺動角度を示す図である。 本発明の第３実施形態に係るロボット装置における第３のアクチュエータに対する収縮力指令値の差及び和に対する応答のシミュレーション結果を示す図である。 本発明の第３実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）はゲイン調整器の係数値を変化させた場合の第２のリンクの先端の軌跡と目標軌道を示す図である。（ｂ）はゲイン調整器の係数値を０とした場合の第２のリンクの先端の軌跡と目標軌道を示す図である。 本発明の第４実施形態に係るロボット装置の制御装置の要部を示すブロック線図である。 本発明の第５実施形態におけるロボット装置のシミュレーション結果を示すスティック線図である。 本発明の第６実施形態に係るロボット装置の要部を示すブロック線図である。 本発明の第７実施形態に係るロボット装置における制約関数を説明するための図である。 本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は目標角度と各制約関数を用いた応答の角度とを比較した図、（ｂ）は各制約関数で決定される収縮力指令値を示す図である。 本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は第１のリンクの目標角度と各制約関数の応答である実際の角度とを比較した図、（ｂ）は第２のリンクの目標角度と各制約関数の応答である実際の角度とを比較した図である。 本発明の第７実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は各制約関数を用いた第１の収縮力指令値の応答を示す図、（ｂ）は各制約関数を用いた第２の収縮力指令値の応答を示す図である。 本発明の第８実施形態に係るロボット装置としての４足歩行ロボットのリンク配置と筋配置および座標系を示す側面図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は右後脚のスティック線図、足先軌道及びスティフネス楕円を示す図である。（ｂ）は右前脚のスティック線図、足先軌道及びスティフネス楕円を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図であり、（ａ）は各支持脚の歩容位相ダイアグラム、（ｂ）は各支持脚における支持脚相を説明するための図である。 本発明の第８実施形態に係るロボット装置の制御装置の要部を示すブロック線図である。 本発明の第８実施形態に係るロボット装置の外観を示す斜視図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は左後脚第１リンク角度の応答を示す図、（ｂ）は左後脚第２リンク角度の応答を示す図である。（ｃ）は右後脚第１リンク角度の応答を示す図、（ｄ）は第２リンク角度の応答を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は平地歩行時の左後脚足球中心の軌跡を示す図、（ｂ）は障害物乗り上げ時の左後脚足球中心の軌跡を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の歩行制御による応答を示す図、（ｂ）は関節軌道の追従のみによる歩行制御による応答を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は胴体重心のロール角度、（ｂ）は進行方向の変位、（ｃ）は速度の応答を示す。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の制御系による歩行による応答を示す図、（ｂ）は関節軌道の追従のみによる歩行による応答を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果を示す図である。（ａ）は本第８実施形態の歩行制御によるロボット装置の姿勢推移を示す図、（ｂ）は関節軌道の追従のみによるロボット装置の姿勢推移を示す図である。 本発明の第８実施形態に係るロボット装置におけるシミュレーション結果であり、胴体重心の加速度を示す図である。

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

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

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

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

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

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

リンク１０１は、長手部材からなり、その基端１０１ａが固定部材であるアーム部材１０４の長手方向の先端に揺動可能に支持されている。このリンク１０１の先端１０１ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、リンク１０１は、関節で揺動可能に支持されている。

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

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

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

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

式（２）の右辺第１項における駆動力ｕ_ｅ１，ｕ_ｆ１の差が関節に回転トルクを与え、右辺第２項、第３項における駆動力ｕ_ｅ１，ｕ_ｆ１の和が関節におけるリンクの剛性、粘性を変動させることがわかる。

（２）制御系設計
本第１実施形態では、アーム部材１０４に対するリンク１０１の揺動角度を目標揺動角度に位置決めし、同時にアーム部材１０４とリンク１０１との関節におけるリンク１０１の剛性を制御することを目的としている。

図３に本第１実施形態における制御装置１５０のブロック線図を示す。制御装置１５０は、リンク１０１を第１の揺動方向（図１中、矢印と反対方向）に揺動させる駆動力をアクチュエータｅ_１に発生させる第１の駆動力指令値ｕ_ｅ１を生成する。また、制御装置１５０は、リンク１０１を第１の揺動方向とは反対の第２の揺動方向（図１中、矢印の方向）に揺動させる駆動力をアクチュエータｆ_１に発生させる第２の駆動力指令値ｕ_ｆ１を生成する。そして、制御装置１５０は、第１の駆動力指令値ｕ_ｅ１と第２の駆動力指令値ｕ_ｆ１との差に基づき、一対のアクチュエータｅ_１，ｆ_１を動作させてリンク１０１の揺動動作を制御する。これと共に、制御装置１５０は、第１の駆動力指令値ｕ_ｅ１と第２の駆動力指令値ｕ_ｆ１との和に基づき一対のアクチュエータｅ_１，ｆ_１を動作させてアーム部材１０４とリンク１０１との関節の剛性を制御する。

ここで、固定部材であるアーム部材１０４に対するリンク１０１の目標揺動角度をｒ_ａ、アーム部材１０４とリンク１０１との関節におけるリンク１０１の目標剛性をｒ_ｓとする。なお、図３中、符号Ｐで示すブロックは、アクチュエータｅ_１，ｆ_１及びリンク１０１からなるリンク機構を示している。以下、制御装置１５０の構成について詳細に説明する。

制御装置１５０は、アーム部材１０４に対するリンク１０１の揺動角度θ_１を検知する揺動角度検知センサ１と、リンク１０１に外力が作用したときに外力が作用したことを示す検知信号を出力するタッチセンサ２とを備えている。

また、制御装置１５０は、偏差算出部である減算器３及びゲイン調整器４と、トルク指令生成部であるＰＩＤ制御器５と、剛性指令値Ｕ_１にリンク１０１の揺動中心点を中心とするモーメントアームの長さｒを乗算する乗算部としての乗算器６とを備えている。

また、制御装置１５０は、ＰＩＤ制御器５が出力するトルク指令値Ｔ_１を補正するトルク指令補正部である飽和回路７を備えている。また、制御装置１５０は、トルク指令値Ｔ_１及び剛性指令値Ｕ_１から各アクチュエータｅ_１，ｆ_１への駆動力指令値ｕ_ｅ１，ｕ_ｆ１を演算により求める駆動力指令値演算部８を備えている。

以下、各部について詳細に説明する。まず、揺動角度検知センサ１は、ポテンショメータ又はエンコーダ等のセンサであり、図１において、図示は省略しているが、アーム部材１０４とリンク１０１との関節に設けられており、アーム部材１０４の延びる方向を基準とする関節の角度を検知する。

タッチセンサ２は、不図示のエンドエフェクタ（例えばハンド）又はリンク１０１に設けられ、エンドエフェクタ又はリンク１０１に接触物が接触したときに、検知信号を出力する。つまり、リンク１０１に外力が作用するとは、エンドエフェクタ又はリンク１０１に接触物が接触することを意味する。

減算器３は、揺動角度θ_１と目標揺動角度ｒ_ａとの偏差（差分）を示す値（ｒ_ａ−θ_１）を算出する。

ゲイン調整器４は、減算器３が出力した偏差を示す値（ｒ_ａ−θ_１）に接触ゲインである係数値Ｇ_ｔを乗じて出力する。係数値Ｇ_ｔは、０以上１以下の範囲内の値であり、タッチセンサ２から検知信号がない場合（センサ２からの検知信号の出力が停止されている期間）には、係数値Ｇ_ｔ＝１として、入力した偏差を示す値（ｒ_ａ−θ_１）を出力する。タッチセンサ２から検知信号が出力されている期間は、偏差を示す値（ｒ_ａ−θ_１）に０以上１以下の範囲内の値に設定された係数値Ｇ_ｔ（本実施形態では、Ｇ_ｔ＝０）を乗じて、新たな偏差を示す値（つまり、ｒ_ａ−θ_１＝０）を出力する。

ＰＩＤ制御器５は、偏差を示す値（ｒ_ａ−θ_１）に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ_１を生成する。即ち、このフィードバック制御系であるＰＩＤ制御器５は、リンク１０１の揺動角度θ_１と目標揺動角度ｒ_ａとの偏差を補償するための（つまり、揺動角度θ_１を目標揺動角度ｒ_ａに近づけるための）トルク指令値Ｔ_１を出力する。

本第１実施形態では、ＰＩＤ制御器５の伝達関数は、

としている。

ここで、式（２）より、

を満たすように駆動力指令値ｕ_ｅ１，ｕ_ｆ１を決定すれば、トルク指令値Ｔ_１のトルクでリンク１０１を揺動させることができる。

しかし、以下の式（５）に示す同時に関節におけるリンク１０１の剛性に関する条件を満たさなければならない。

ここで、２つの駆動力指令値ｕ_ｅ１，ｕ_ｆ１の和を示す剛性指令値Ｕ_１を定義すると、剛性指令値Ｕ_１は、以下の式（６）となる。

従って、剛性指令値Ｕ_１は、目標剛性ｒ_ｓを用いた以下の式（７）となる。なお、弾性力定数ｋ及びモーメントアームの長さｒは定数であるので、目標剛性ｒ_ｓと剛性指令値Ｕ_１は比例関係にあり同等である。

この式（７）を満たすように、駆動力指令値ｕ_ｅ１，ｕ_ｆ１を決定する必要がある。そこで、トルク指令値Ｔ_１と剛性指令値Ｕ_１とを同時に満たすには、式（４）と式（６）をｕ_ｅ１，ｕ_ｆ１について解き、以下の式（８）とすればよい。

しかし、人工筋肉アクチュエータは収縮方向にのみ力を発生する。そのため、以下の式（９）の条件を同時に満たさなければならない。

式（８）と式（９）より以下の式（１０）の条件が得られる。

つまりトルク指令値Ｔ_１は、以下の式（１１）の条件となる。

従って、式（１１）の条件を満たしていれば、関節におけるリンク１０１の剛性が目標剛性ｒ_ｓとなると同時に、トルク指令値Ｔ_１によりリンク１０１の揺動角度が目標角度ｒ_ａに位置決めされる。

そこで、本第１実施形態では、乗算器６は、モーメントアームの長さｒと剛性指令値Ｕ_１との乗算値Ｕ_１×ｒを算出し、飽和回路７は、式（１１）を実現するために、制約関数に従って、トルク指令値Ｔ_１を、式（１２）の値に制約する。

即ち、飽和回路７は、トルク指令値Ｔ_１の絶対値を、モーメントアームの長さｒと剛性指令値Ｕ_１との乗算値Ｕ_１×ｒ以下に制約する制約関数に従って、トルク指令値Ｔ_１を補正する。より具体的には、飽和回路７は、制約関数として飽和関数に従い、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１をそのまま出力する。また、飽和回路７は、飽和関数に従い、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒを上回った場合は、絶対値が乗算値Ｕ_１×ｒに制限された新たなトルク指令値Ｔ_１を生成して出力する。

なお、トルク指令補正部は、上述した飽和回路７である以外に、ＰＩＤ制御器５のゲインを変動させ、式（１１）の範囲にトルク指令値Ｔ_１（制御入力）の大きさを収めるように機能する回路であってもよい。

以下、タッチセンサ２が検知信号を出力していない場合（タッチセンサ２がＯＦＦ）について、図３を参照しながら制御装置１５０の動作を説明する。まず、揺動角度検知センサ１が、リンク１０１の揺動角度を検知する（揺動角度検知ステップ）。

そして、減算器３が、リンク１０１の揺動角度θ_１と目標揺動角度ｒ_ａとの偏差を示す値（ｒ_ａ−θ_１）を算出する（偏差算出ステップ）。

タッチセンサ２がＯＦＦであるので、ゲイン調整器４は、乗算する係数値Ｇ_ｔが１であり、偏差を示す値（ｒ_ａ−θ_１）をそのまま出力する。

ＰＩＤ制御器５は、ゲイン調整器４から取得した偏差を示す値（ｒ_ａ−θ_１）に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ_１を生成する（トルク指令生成ステップ）。

乗算器６は、取得した剛性指令値Ｕ_１とモーメントアームの長さｒとの乗算値Ｕ_１×ｒを算出する（乗算ステップ）。モーメントアームの長さｒは、予め設定（不図示の記憶装置に記憶）された定数であり、乗算器６が剛性指令値Ｕ_１を取得したときに、演算により乗算値Ｕ_１×ｒを求める。なお、予め乗算値Ｕ_１×ｒの計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておき、乗算器６が剛性指令値Ｕ_１を取得したときにテーブルデータを参照して乗算値Ｕ_１×ｒを求めてもよい。

飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１をそのまま出力する。また、飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒを上回った場合は、絶対値が乗算値Ｕ_１×ｒ以下に制限された新たなトルク指令値Ｔ_１を生成して出力する（トルク指令補正ステップ）。

具体的には、飽和回路７は、絶対値が乗算値Ｕ_１×ｒとする新たなトルク指令値Ｔ_１を出力する。例えば、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１が、Ｕ_１×ｒを上回る場合は、新たなトルク指令値Ｔ_１をＵ_１×ｒとする。また、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１が、−Ｕ_１×ｒを下回る場合は、新たなトルク指令値Ｔ_１を−Ｕ_１×ｒとする。これにより、トルク指令値Ｔ_１が式（１１）に制限されるので、最終的に求められる各駆動力指令値ｕ_ｅ１，ｕ_ｆ１が、負の値となることはない。

そして、駆動力指令値演算部８は、飽和回路７により出力されたトルク指令値Ｔ_１を用いて、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式に基づいて計算する（駆動力指令値演算ステップ）。なお、剛性指令値Ｕ_１、トルク指令値Ｔ_１及びモーメントアームの長さｒに対応した（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部８は、剛性指令値Ｕ_１、トルク指令値Ｔ_１を取得したときに、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。そして、各アクチュエータｅ_１，ｆ_１は、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１に従って空気圧が調整され、各アクチュエータｅ_１，ｆ_１の駆動力が各駆動力指令値ｕ_ｅ１，ｕ_ｆ１に設定される。

以上の動作により、リンク１０１の揺動角度θ_１が目標揺動角度ｒ_ａに調整され、且つ関節におけるリンク１０１の剛性が目標剛性ｒ_ｓ（＝Ｕ_１×ｋ×ｒ^２）に調整されように、フィードバック制御により、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１が設定される。

したがって、フィードフォワード制御よりも簡便なフィードバック制御により、リンク１０１の角度制御と剛性制御の同時制御を実現することができる。これにより、リンク１０１の目標揺動角度ｒ_ａと目標剛性ｒ_ｓ（つまり、剛性指令値Ｕ_１）が変更されても、フィードフォワード制御のように繰り返し演算を行う必要がないので、迅速にリンク１０１の揺動角度及び剛性を目標値に調整することができる。

ところで、フィードバック制御系のゲインが高い場合には、それによる関節の剛性が支配的になってしまう。つまり、リンク１０１に外力が作用した場合、リンク１０１が外力により目標揺動角度ｒ_ａの位置から揺動しようとするので、仮に係数値Ｇ_ｔ＝１としておくと、揺動角度θ_１と目標揺動角度ｒ_ａとの偏差を示す値（ｒ_ａ−θ_１）が０ではなくなる。

その結果、ＰＩＤ制御器５が出力するトルク指令値Ｔ_１によりリンク１０１を目標揺動角度ｒ_ａの定位置に戻すように各アクチュエータｅ_１，ｆ_１の駆動力が設定され、リンク１０１が定位置に位置決めされる。従って、リンク１０１に外力が作用した時、リンク１０１が移動しないように各アクチュエータｅ_１，ｆ_１の駆動力が設定されるので、リンク１０１の剛性が目標剛性ｒ_ｓよりも高くなり、リンク１０１の柔軟性が損なわれてしまうこととなる。

そこで、ゲイン調整器４は、タッチセンサ２がリンク１０１に外力が作用したことを検知して検知信号を出力している期間は、偏差を示す値（ｒ_ａ−θ_１）に係数値Ｇ_ｔ＝０を乗じて、実際に偏差があったとしても偏差なしを示すｒ_ａ−θ_１＝０を出力する。つまり、ゲイン調整器４は、新たな偏差を示す値（ｒ_ａ−θ_１＝０）を算出して出力する。

これにより、ＰＩＤ制御器５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ_１を出力することとなり、リンク１０１の剛性は、目標剛性ｒ_ｓに維持されることとなる。したがって、リンク１０１に外力が作用しても、速やかに係数値Ｇ_ｔ＝０となり、フィードバック制御系が遮断され、リンク１０１の柔軟性が維持される。

なお、タッチセンサ２がＯＦＦに切り換わったときは、ゲイン調整器４における係数値がＧ_ｔ＝１に切り換わり、実際の偏差（ｒ_ａ−θ_１）に基づいてフィードバック制御を行うこととなる。

（３）シミュレーション
図３の制御装置１５０を用いてシミュレーションを行った。ここで、リンク１０１の慣性モーメントをＩ＝８．３×１０^−２［ｋｇｍ^２］、モーメントアームの長さをｒ＝０．１［ｍ］、弾性力定数をｋ＝３、粘性力定数をｂ＝３とする。目標揺動角度ｒ_ａはランプ状とし、初期角度４５［ｄｅｇ］から２秒間で目標揺動角度ｒ_ａ＝６５［ｄｅｇ］に到達する位置決めを行う。

このとき、リンク１０１の関節の目標剛性をｒ_ｓ＝Ｕ_１×ｋ×ｒ^２＝０．４×ｋ×ｒ^２［Ｎｍ／ｒａｄ］と制御する。ここで、弾性力定数ｋ，モーメントアームの長さｒは定数であるので、剛性指令値Ｕ_１をＵ_１＝０．４として、実際の関節の剛性が０．４となるように制御すればよい。

図４（ａ）にリンク１０１の揺動角度θ_１を実線で示し、目標揺動角度ｒ_ａを破線で示す。また、図４（ｂ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１に対する各アクチュエータの実際の駆動力（収縮力、つまり応答）をそれぞれ実線、破線で示す。更に、図４（ｃ）に駆動力指令値ｕ_ｅ１，ｕ_ｆ１に対するアクチュエータの実際の駆動力の差を実線で、和を破線で、Ｔ_１／ｒの応答を一点鎖線で示す。

図４（ａ）より、リンク１０１の揺動角度θ_１は目標揺動角度ｒ_ａに位置決めされていることがわかる。図４（ｂ）より、駆動力指令値ｕ_ｅ１，ｕ_ｆ１に対する応答はフィードバック制御系のトルク指令値Ｔ_１を、式（７）と式（１０）に示した条件で配分されているが、人工筋肉の駆動力は正の値のみを取るという特性を満たしていることがわかる。図４（ｃ）において、実線で示すｕ_ｆ１−ｕ_ｅ１の応答は、一点鎖線で示すＴ_１／ｒの応答と重なっており、式（３）の条件を満たしている。また、破線で示すｕ_ｆ１＋ｕ_ｅ１の応答は常に０．４となっており、関節の剛性が目標剛性に制御されていることがわかる。

次に、リンク１０１の先端が外力により押されることを想定するシミュレーションを行う。１０秒から１１秒にかけて関節角度を正方向に回転させるトルク外乱をリンク１０１の関節に与えている。タッチセンサ２において接触が発生している間（１０秒から１１秒）、ゲイン調整器４の係数値Ｇ_ｔを０とする。図５に関節におけるリンク１０１の目標剛性ｒ_ｓを変化させ、剛性指令値Ｕ_１を０．３，１．０，１０とした場合の応答をそれぞれ実線、破線、点線で示す。

実線で示す剛性指令値Ｕ_１＝０．３の応答より、関節の剛性が、剛性指令値Ｕ_１＝１．０，１０とした場合の剛性よりも低く制御されているため、外力により大きくリンク１０１の揺動角度が変位していることがわかる。リンク１０１への接触物の接触が発生した際に、接触力と同一方向に関節が変位していることから、接触物の接触に対して柔らかい制御が実現していることを示している。そして、目標剛性ｒ_ｓ（つまり、剛性指令値Ｕ_１）を高めるに連れて関節の剛性が高まり、接触に対するリンクの変位量は減少している。

本第１実施形態により、リンク１０１の目標揺動角度ｒ_ａへの位置決めを実現しながら、自在に関節におけるリンク１０１の剛性を変化させることが可能であることがわかる。

比較として、式（７）を用いて駆動力指令値ｕ_ｅ１，ｕ_ｆ１を決定するが、式（１０）の条件を考慮せずに制御を行った場合の応答を、図６（ａ）〜図６（ｃ）に示す。式（１０）の条件なしでは、各アクチュエータｅ_１，ｆ１が収縮方向にのみ力を発生する、という制約が考慮されていない。そのため、図６（ｂ）に示すように駆動力指令値は負の値を取ってしまう。各アクチュエータｅ_１，ｆ１は膨張方向には力を発生しないため、負の値の場合は０としてシミュレーションを行った。

その結果、図６（ａ）に示すように、リンク１０１は目標揺動角度に制御されているが、図６（ｃ）に示すように、０〜０．３秒と２〜２．３秒では剛性指令値Ｕ_１に対する応答は目標値の０．４とならず、関節の剛性が目標剛性に制御されていないことがわかる。

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

（１）モデリング
本実施の形態で扱う３対６筋を有するリンク機構としての２リンクマニピュレータを備えたロボット装置を図７に示す。ロボット装置２００は、２リンクマニピュレータＰを備えている。２リンクマニピュレータＰは、第１のリンク２０１と、第２のリンク２０２と、固定部材であるプーリ２０３とを有している。

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

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

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

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

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

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

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

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

ここで、第１の揺動角度θ_１は、基準軸であるｘ軸を基準とする第１のリンク２０１の揺動角度であり、第２の揺動角度θ_２は、第１のリンク２０１の長手方向に延びる軸線を基準とする第２のリンク２０２の揺動角度である。

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

本第２実施形態では、各筋の弾性力定数，粘性力定数をいずれもｋ，ｂとすると、２リンクマニピュレータの運動方程式は、以下の式（１３）及び式（１４）となる。

（２）制御系設計
２リンクマニピュレータであるロボット装置２００では、手先（ハンド）が外界と直に接触するので、手先剛性、即ちリンク先端２０２ｂの剛性を制御することが重要である。手先剛性は、図８に示すような剛性の大きさを示すスティフネス特性を示す楕円（スティフネス楕円）によって表される。

本第２実施形態では、作業平面内にｘ−ｙ軸直交座標系を定義し、リンク先端２０２ｂにおけるスティフネス特性がｘ−ｙ軸直交座標系にリンク先端２０２ｂを中心とするスティフネス楕円で表される。このスティフネス楕円は、各方向に対する剛性の分布を示し、リンク先端２０２ｂとスティフネス楕円の距離が離れるほど剛性が高いことを表している。

図９に本第２実施形態における制御装置２５０のブロック線図を示す。ここで、固定部材であるプーリ２０３に対する第１のリンク２０１の第１の目標揺動角度をｒ_ａ１、第１のリンク２０１に対する第２のリンク２０２の第２の目標揺動角度をｒ_ａ２とする。また、プーリ２０３と第１のリンク２０１との第１の関節における第１のリンク２０１の目標剛性をｒ_ｓ１、第１のリンク２０１と第２のリンク２０２との第２の関節における第２のリンク２０２の目標剛性をｒ_ｓ２とする。

制御装置２５０は、プーリ２０３に対する第１のリンク２０１の第１の揺動角度θ_１を検知する第１の揺動角度検知センサ５１と、第１のリンク２０１に対する第２のリンク２０２の第２の揺動角度θ_２を検知する第２の揺動角度検知センサ６１とを備えている。また、制御装置２５０は、第２のリンク２０２に外力が作用したときに外力が作用したことを示す検知信号を出力するタッチセンサ５２を備えている。

また、制御装置２５０は、第１の偏差算出部である減算器５３及びゲイン調整器５４と、第１のトルク指令生成部であるＰＩＤ制御器５５と、を備えている。また、制御装置２５０は、第１の剛性指令値Ｕ_１に第１のリンク２０１の揺動中心点を中心とするモーメントアームの長さｒ_１を乗算する第１の乗算部としての乗算器５６を備えている。なお、図９では、モーメントアームの長さをｒ_１＝ｒとしている。

また、制御装置２５０は、ＰＩＤ制御器５５が出力するトルク指令値Ｔ_１を補正する第１のトルク指令補正部である飽和回路５７を備えている。また、制御装置２５０は、トルク指令値Ｔ_１及び剛性指令値Ｕ_１から各アクチュエータｅ_１，ｆ_１への駆動力指令値ｕ_ｅ１，ｕ_ｆ１を演算により求める第１の駆動力指令値演算部５８を備えている。

同様に、制御装置２５０は、第２の偏差算出部である減算器６３及びゲイン調整器６４と、第２のトルク指令生成部であるＰＩＤ制御器６５と、を備えている。また、制御装置２５０は、第２の剛性指令値Ｕ_２に第２のリンク２０２の揺動中心点を中心とするモーメントアームの長さｒ_２を乗算する第２の乗算部としての乗算器６６を備えている。なお、図９では、モーメントアームの長さをｒ_２＝ｒとしている。

また、制御装置２５０は、ＰＩＤ制御器６５が出力するトルク指令値Ｔ_２を補正する第２のトルク指令補正部である飽和回路６７を備えている。また、制御装置２５０は、トルク指令値Ｔ_２及び剛性指令値Ｕ_２から各アクチュエータｅ_２，ｆ_２への駆動力指令値ｕ_ｅ２，ｕ_ｆ２を演算により求める第２の駆動力指令値演算部６８を備えている。

以下、各部について詳細に説明する。まず、各揺動角度検知センサ５１，６１は、ポテンショメータ又はエンコーダ等のセンサであり、図７において、図示は省略しているが、各関節に設けられている。

タッチセンサ５２は、不図示のエンドエフェクタ（例えばハンド）又は第２のリンク２０２に設けられ、エンドエフェクタ又は第２のリンク２０２に接触物が接触したときに、検知信号を出力する。つまり、第２のリンク２０２に外力が作用するとは、エンドエフェクタ又は第２のリンク２０２に接触物が接触することを意味する。

減算器５３は、第１の揺動角度θ_１と第１の目標揺動角度ｒ_ａ１との第１の偏差（差分）を示す値（ｒ_ａ１−θ_１）を算出する。同様に、減算器６３は、第２の揺動角度θ_２と第２の目標揺動角度ｒ_ａ２との第２の偏差（差分）を示す値（ｒ_ａ２−θ_２）を算出する。

ゲイン調整器５４は、減算器５３が出力した第１の偏差を示す値（ｒ_ａ１−θ_１）に接触ゲインである第１の係数値Ｇ_ｔ１を乗じて出力する。同様に、ゲイン調整器６４は、減算器６３が出力した第２の偏差を示す値（ｒ_ａ２−θ_２）に接触ゲインである第２の係数値Ｇ_ｔ２を乗じて出力する。各係数値Ｇ_ｔ１，Ｇ_ｔ２は、０以上１以下の範囲内の値であり、ゲイン調整器５４，６４は、タッチセンサ５２から検知信号がない場合には、係数値Ｇ_ｔ１，Ｇ_ｔ２＝１として、入力した偏差を示す値（ｒ_ａ１−θ_１），（ｒ_ａ２−θ_２）を出力する。

また、ゲイン調整器５４，６４は、タッチセンサ５２から検知信号が出力されている期間は、偏差を示す値（ｒ_ａ１−θ_１），（ｒ_ａ２−θ_２）に０以上１以下の範囲内の値の係数値、本実施形態では係数値Ｇ_ｔ１，Ｇ_ｔ２＝０を乗じる。これにより、ゲイン調整器５４，６４は、新たな偏差を示す値（ｒ_ａ１−θ_１＝０，ｒ_ａ２−θ_２＝０）を出力する。

ＰＩＤ制御器５５は、第１の偏差を示す値（ｒ_ａ１−θ_１）に対してＰＩＤ制御演算を行い、第１のリンク２０１を揺動させる第１のトルク指令値Ｔ_１を生成する。即ち、フィードバック制御系であるＰＩＤ制御器５５は、第１のリンク２０１の第１の揺動角度θ_１と第１の目標揺動角度ｒ_ａ１との第１の偏差を補償するための（つまり、揺動角度θ_１を目標揺動角度ｒ_ａ１に近づけるための）第１のトルク指令値Ｔ_１を出力する。

同様に、ＰＩＤ制御器６５は、第２の偏差を示す値（ｒ_ａ２−θ_２）に対してＰＩＤ制御演算を行い、第２のリンク２０２を揺動させる第２のトルク指令値Ｔ_２を生成する。即ち、フィードバック制御系であるＰＩＤ制御器６５は、第２のリンク２０２の第２の揺動角度θ_２と第２の目標揺動角度ｒ_ａ２との第２の偏差を補償するための（つまり、揺動角度θ_２を目標揺動角度ｒ_ａ２に近づけるための）第２のトルク指令値Ｔ_２を出力する。

第１のアクチュエータｅ_１，ｆ_１及び第２のアクチュエータｅ_２，ｆ_２の制御方法は、上記第１実施形態と同様である。第ｎのアクチュエータｅ_ｎ，ｆ_ｎに対する駆動力指令値ｕ_ｅｎ，ｕ_ｆｎ、第ｎのトルク指令値Ｔ_ｎ、第ｎの剛性指令値Ｕ_ｎ（ただし、ｎ＝１，２）は、以下の式（１５）及び式（１６）の関係にある。

そして、トルク指令値Ｔ_１と剛性指令値Ｕ_１とを同時に満たすには、駆動力指令値ｕ_ｆ１＝（Ｕ_１＋Ｔ_１／ｒ_１）／２，駆動力指令値ｕ_ｅ１＝（Ｕ_１−Ｔ_１／ｒ_１）／２を満たせばよい。また、トルク指令値Ｔ_２と剛性指令値Ｕ_２とを同時に満たすには、駆動力指令値ｕ_ｆ２＝（Ｕ_２＋Ｔ_２／ｒ_２）／２，駆動力指令値ｕ_ｅ２＝（Ｕ_２−Ｔ_２／ｒ_２）／２を満たせばよい。

そして、各駆動力指令値は、以下の式（１７）の条件を満たす必要がある。

つまりトルク指令値Ｔ_ｎは、以下の式（１８）の条件となる。

従って、式（１８）の条件を満たしていれば、関節におけるリンク２０１，２０２の剛性が目標剛性ｒ_ｓ１，ｒ_ｓ２となると同時に、トルク指令値Ｔ_１，Ｔ_２によりリンク２０１，２０２の揺動角度が目標角度ｒ_ａ１，ｒ_ａ２に位置決めされる。

本第２実施形態では、乗算器５６は、モーメントアームの長さｒ_１と第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１を算出し、飽和回路５７は、式（１８）を実現するために、トルク指令値Ｔ_１を、以下の式（１９）の値に制約する。同様に、乗算器６６は、モーメントアームの長さｒ_２と第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２を算出し、飽和回路６７は、式（１８）を実現するために、トルク指令値Ｔ_２を、以下の式（１９）の値に制約する。

即ち、飽和回路５７は、第１のトルク指令値Ｔ_１の絶対値を、モーメントアームの長さｒ_１と第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ_１を補正する。より具体的には、飽和回路５７は、制約関数として飽和関数に従い、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１の絶対値が第１の乗算値Ｕ_１×ｒ_１以下の場合は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１をそのまま出力する。また、飽和回路５７は、飽和関数に従い、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１の絶対値が第１の乗算値Ｕ_１×ｒ_１を上回った場合は、絶対値が第１の乗算値Ｕ_１×ｒ_１以下に制限された新たな第１のトルク指令値Ｔ_１を生成して出力する。

同様に、飽和回路６７は、第２のトルク指令値Ｔ_２の絶対値を、モーメントアームの長さｒ_２と第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ_２を補正する。より具体的には、飽和回路６７は、制約関数として飽和関数に従い、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２の絶対値が第２の乗算値Ｕ_２×ｒ_２以下の場合は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２をそのまま出力する。また、飽和回路６７は、飽和関数に従い、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２の絶対値が第２の乗算値Ｕ_２×ｒ_２を上回った場合は、絶対値が第２の乗算値Ｕ_２×ｒ_２以下に制限された新たな第２のトルク指令値Ｔ_２を生成して出力する。

ここで、第３のアクチュエータｅ_３，ｆ_３に対する駆動力指令値ｕ_ｅ３，ｕ_ｆ３に差を与えることによって、第１，第２のアクチュエータと同様に関節にトルクを与えることは可能である。しかし、一つのリンクを複数の位置フィードバック制御系が制御することは冗長であり、フィードバック制御系の設計が複雑になってしまう。

そこで、本第２実施形態では第３のアクチュエータｅ_３，ｆ_３は各関節に剛性を与えるために用いる。そこで、駆動力指令値ｕ_ｅ３，ｕ_ｆ３の差を０、第３の剛性指令値をＵ_３とすると、以下の式（２０）の関係となる。

これを解くと、駆動力指令値ｕ_ｅ３，ｕ_ｆ３は、

となる。

そこで、本第２実施形態では、制御装置２５０は、各第３のアクチュエータｅ_３，ｆ_３の駆動力指令値ｕ_ｅ３，ｕ_ｆ３を、式（２１）に示すように、第３の剛性指令値Ｕ_３の１／２に設定する第３の駆動力指令値演算部７８を備えている。

上記第１実施形態の１リンクマニュピレータでは、関節の剛性と手先の剛性は同一であった。本第２実施形態の３対６筋を有する２リンクマニュピレータでは、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を調整することにより手先の剛性が求まる。

例えば、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を

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

以下、スティフネス楕円の長軸及び短軸のうちの一方の軸がｘ−ｙ軸直交座標系における基準軸であるｘ軸と平行になる剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求める。

ｘ−ｙ軸に平行な微小な外力ΔＦ_ｘ，ΔＦ_ｙによるリンク先端２０２ｂの微小な変位をΔｘ_ｔ，Δｙ_ｔとする。また、微小な外力ΔＦ_ｘ，ΔＦ_ｙによる、各リンク２０１，２０２の微小な回転角度をΔθ_１，Δθ_２とする。微小な回転が起こると、筋の粘弾性を有するアクチュエータは、筋の弾性力によりリンクに微小なトルクΔＴ_ｐ１，ΔＴ_ｐ２を発生させ、以下のような式（２３）及び式（２４）で表せる。

これを行列で表記し、微小な角度とトルクの関係を剛性行列Ｋ_Ｓで表すと、以下の式（２５）となる。

ここで、ヤコビ行列Ｊを導入するが、ヤコビ行列Ｊは、以下の式（２６）で表される。

これより、微小手先変位と微小外力との関係をコンプライアンス行列Ｊ_Ｓで表すと、以下の式（２７）となる。

コンプライアンス行列の逆行列が剛性を表すので、スティフネス楕円がｘ−ｙ軸に平行であるという条件は、

となる。この方程式を剛性指令値Ｕ_３について解くと、

となる。

以下、タッチセンサ５２が検知信号を出力していない場合（タッチセンサ５２がＯＦＦ）について、図９を参照しながら制御装置２５０の動作を説明する。まず、第１の揺動角度検知センサ５１が、第１のリンク２０１の第１の揺動角度θ_１を検知する（第１の揺動角度検知ステップ）。同様に、第２の揺動角度検知センサ６１が、第２のリンク２０２の第２の揺動角度θ_２を検知する（第２の揺動角度検知ステップ）。

そして、減算器５３が、第１のリンク２０１の第１の揺動角度θ_１と第１の目標揺動角度ｒ_ａ１との第１の偏差を示す値（ｒ_ａ１−θ_１）を算出する（第１の偏差算出ステップ）。同様に、減算器６３が、第２のリンク２０２の第２の揺動角度θ_２と第２の目標揺動角度ｒ_ａ２との第２の偏差を示す値（ｒ_ａ２−θ_２）を算出する（第２の偏差算出ステップ）。

タッチセンサ５２がＯＦＦであるので、各ゲイン調整器５４，６４は、乗算する各係数値Ｇ_ｔ１，Ｇ_ｔ２が１であり、各偏差を示す値（ｒ_ａ１−θ_１），（ｒ_ａ２−θ_２）をそのまま出力する。

ＰＩＤ制御器５５は、ゲイン調整器５４から取得した偏差を示す値（ｒ_ａ１−θ_１）に対してＰＩＤ制御演算を行い、第１のリンク２０１を揺動させる第１のトルク指令値Ｔ_１を生成する（第１のトルク指令生成ステップ）。同様に、ＰＩＤ制御器６５は、ゲイン調整器６４から取得した偏差を示す値（ｒ_ａ２−θ_２）に対してＰＩＤ制御演算を行い、第２のリンク２０２を揺動させる第２のトルク指令値Ｔ_２を生成する（第２のトルク指令生成ステップ）。

乗算器５６は、取得した第１の剛性指令値Ｕ_１と第１のリンク２０１のモーメントアームの長さｒ_１との第１の乗算値Ｕ_１×ｒ_１を算出する（第１の乗算ステップ）。同様に、乗算器６６は、取得した第２の剛性指令値Ｕ_２と第２のリンク２０２のモーメントアームの長さｒ_２との第２の乗算値Ｕ_２×ｒ_２を算出する（第２の乗算ステップ）。

ここで、モーメントアームの長さｒ_１，ｒ_２は、予め設定（不図示の記憶装置に記憶）された定数である。乗算器５６が剛性指令値Ｕ_１を取得したときに、演算により乗算値Ｕ_１×ｒ_１を求め、乗算器６６が剛性指令値Ｕ_２を取得したときに、演算により乗算値Ｕ_２×ｒ_２を求める。なお、予め乗算値Ｕ_１×ｒ_１の計算結果、乗算値Ｕ_２×ｒ_２の計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておいてもよい。そして、乗算器５６が剛性指令値Ｕ_１を取得したときにテーブルデータを参照して乗算値Ｕ_１×ｒ_１を求め、乗算器６６が剛性指令値Ｕ_２を取得したときにテーブルデータを参照して乗算値Ｕ_２×ｒ_２を求めてもよい。

飽和回路５７は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１の絶対値が第１の乗算値Ｕ_１×ｒ_１以下の場合は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１をそのまま出力する。飽和回路５７は、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１の絶対値が第１の乗算値Ｕ_１×ｒ_１を上回った場合は、絶対値が第１の乗算値Ｕ_１×ｒ_１以下に制限された新たな第１のトルク指令値Ｔ_１を生成して出力する（第１のトルク指令補正ステップ）。なお、図９では、モーメントアームの長さをｒ_１＝ｒとしている。

具体的には、飽和回路５７は、絶対値が乗算値Ｕ_１×ｒ_１とする新たな第１のトルク指令値Ｔ_１を出力する。例えば、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１が、Ｕ_１×ｒ_１を上回る場合は、新たな第１のトルク指令値Ｔ_１をＵ_１×ｒ_１とする。また、ＰＩＤ制御器５５で生成した第１のトルク指令値Ｔ_１が、−Ｕ_１×ｒ_１を下回る場合は、新たなトルク指令値Ｔ_１を−Ｕ_１×ｒ_１とする。これにより、第１のトルク指令値Ｔ_１が式（１８）に制限されるので、最終的に求められる各駆動力指令値ｕ_ｅ１，ｕ_ｆ１が、負の値となることはない。

同様に、飽和回路６７は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２の絶対値が第２の乗算値Ｕ_２×ｒ_２以下の場合は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２をそのまま出力する。飽和回路６７は、ＰＩＤ制御器６５で生成した第２のトルク指令値Ｔ_２の絶対値が第２の乗算値Ｕ_２×ｒ_２を上回った場合は、絶対値が第２の乗算値Ｕ_２×ｒ_２以下に制限された新たな第２のトルク指令値Ｔ_２を生成して出力する（第２のトルク指令補正ステップ）。なお、図９では、モーメントアームの長さをｒ_２＝ｒとしている。

そして第１の駆動力指令値演算部５８は、飽和回路５７により出力された第１のトルク指令値Ｔ_１を用いて、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１を（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式に基づいて計算する（第１の駆動力指令値演算ステップ）。同様に、第２の駆動力指令値演算部６８は、飽和回路６７により出力された第２のトルク指令値Ｔ_２を用いて、各駆動力指令値ｕ_ｅ２，ｕ_ｆ２を（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式に基づいて計算する（第２の駆動力指令値演算ステップ）。なお、剛性指令値Ｕ_１、トルク指令値Ｔ_１及びモーメントアームの長さｒ_１に対応した（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。また、剛性指令値Ｕ_２、トルク指令値Ｔ_２及びモーメントアームの長さｒ_２に対応した（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部５８は、剛性指令値Ｕ_１、トルク指令値Ｔ_１を取得したときに、（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。また、駆動力指令値演算部６８は、剛性指令値Ｕ_２、トルク指令値Ｔ_２を取得したときに、（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。

以上のフィードバック制御により、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１が設定されることで、リンク２０１の揺動角度θ_１が目標揺動角度ｒ_ａ１に調整され、且つ第１の関節におけるリンク２０１の剛性が目標剛性ｒ_ｓ１（＝Ｕ_１×ｋ×ｒ_１ ^２）に調整される。同様に、フィードバック制御により、各駆動力指令値ｕ_ｅ２，ｕ_ｆ２が設定されることで、リンク２０２の揺動角度θ_２が目標揺動角度ｒ_ａ２に調整され、且つ第２の関節におけるリンク２０２の剛性が目標剛性ｒ_ｓ２（＝Ｕ_２×ｋ×ｒ_２ ^２）に調整される。

したがって、フィードフォワード制御よりも簡便なフィードバック制御により、リンク２０１，２０２の角度制御と剛性制御の同時制御を実現することができる。これにより、リンク２０１，２０２の目標揺動角度と目標剛性（つまり、剛性指令値Ｕ_１，Ｕ_２）が変更されても、フィードフォワード制御のように繰り返し演算を行う必要がないので、迅速にリンクの揺動角度及び関節の剛性を目標値に調整することができる。

また、ゲイン調整器５４は、タッチセンサ５２が第２のリンク２０２に外力が作用したことを検知して検知信号を出力している期間は、第１の偏差を示す値（ｒ_ａ１−θ_１）に第１の係数値Ｇ_ｔ１＝０を乗じて出力する。つまり、ゲイン調整器５４は、実際に偏差があったとしても偏差なしを示す新たな第１の偏差を示す値（ｒ_ａ１−θ_１＝０）を算出して出力する。

同様に、ゲイン調整器６４は、タッチセンサ５２が第２のリンク２０２に外力が作用したことを検知して検知信号を出力している期間は、第２の偏差を示す値（ｒ_ａ２−θ_２）に第２の係数値Ｇ_ｔ２＝０を乗じて出力する。つまり、ゲイン調整器６４は、実際に偏差があったとしても偏差なしを示す新たな第２の偏差を示す値（ｒ_ａ２−θ_２＝０）を算出して出力する。

これにより、各ＰＩＤ制御器５５，６５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ_１，Ｔ_２を出力することとなる。したがって、リンク２０１，２０２の剛性は、目標剛性に維持され、リンク２０１，２０２に外力が作用しても、速やかに係数値Ｇ_ｔ１，Ｇ_ｔ２＝０となり、フィードバック制御系が遮断され、リンク２０１，２０２の柔軟性が維持される。

なお、タッチセンサ５２がＯＦＦに切り換わったときは、各ゲイン調整器５４，６４における係数値がＧ_ｔ１，Ｇ_ｔ２＝１に切り換わり、実際の偏差（ｒ_ａ１−θ_１），（ｒ_ａ２−θ_２）に基づいてフィードバック制御を行うこととなる。

（３）シミュレーション
図９に示した制御装置２５０を用いてシミュレーションを行った。第１のリンク２０１と第２のリンク２０２の物理パラメータは同一とする。リンク２０１，２０２の長さを０．５［ｍ］、リンク２０１，２０２の慣性モーメントを８．３×１０^−２［ｋｇｍ^２］、モーメントアームの長さｒ（＝ｒ_１＝ｒ_２）を０．１［ｍ］、弾性力定数をｋ＝３、粘性力定数をｂ＝３とする。

第１，第２の目標揺動角度はランプ状とし、第１のリンク２０１は初期角度４５［ｄｅｇ］から２秒間で目標揺動角度ｒ_ａ１＝３５［ｄｅｇ］に到達する位置決めを行う。また、第２のリンク２０２は初期角度９０［ｄｅｇ］から２秒間で目標揺動角度ｒ_ａ２＝８５［ｄｅｇ］に到達する位置決めを行う。

リンクが位置決めされる目標角度において、リンク先端２０２ｂにおけるスティフネス楕円がｘ−ｙ軸と水平となるように、各アクチュエータの剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を制御する。一関節駆動アクチュエータの剛性指令値Ｕ_１，Ｕ_２は任意に設定し、Ｕ_１＝１０，Ｕ_２＝５に設定した。このとき、二関節駆動アクチュエータの剛性指令値Ｕ_３は、式（２９）に基づいて、剛性指令値Ｕ_３＝１．９０７となるように制御すればよい。

図１０（ａ），図１０（ｂ）に、第１，第２のリンク２０１，２０２の揺動角度θ_１，θ_２を実線で、目標揺動角度ｒ_ａ１，ｒ_ａ２を破線で示す。また、図１１（ａ）〜図１１（ｃ）に、駆動力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３の応答を実線で、駆動力指令値ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３の応答を破線で示す。さらに、図１２（ａ）〜図１２（ｃ）に、駆動力指令値ｕ_ｅｎ，ｕ_ｆｎ（ｎ＝１，２，３）の応答の差を実線で、応答の和を破線で、Ｔ_ｎ／ｒ（ｎ＝１，２）の応答を一点鎖線で示す。

まず、図１０（ａ）及び図１０（ｂ）よりリンク２０１，２０２は目標揺動角度に位置決めされていることがわかる。次に、図１１（ａ）より、駆動力指令値ｕ_ｅ１，ｕ_ｆ１は、トルク指令値Ｔ_１を｜Ｔ_１｜≦Ｕ_１×ｒに制限した状態で（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式を用いて配分されている。従って、第１のアクチュエータｅ_１，ｆ_１の駆動力は正の値を取るという特性を満たしていることがわかる。

また、図１１（ｂ）より、第２のアクチュエータｅ_２，ｆ_２の駆動力も同様に正の値を取っていることがわかる。また、図１１（ｃ）より、第３のアクチュエータｅ_３，ｆ_３の駆動力は一定値のＵ_３／２である。

また、図１２（ａ）において、実線で示すｕ_ｆ１−ｕ_ｅ１の応答は、一点鎖線で示すＴ_１／ｒの応答と重なっており、式（１５）の条件を満たしている。また、破線で示すｕ_ｆ１＋ｕ_ｅ１＝Ｕ_１の応答は常に設定値である１０となっている。

同様に、図１２（ｂ）において、実線で示すｕ_ｆ２−ｕ_ｅ２の応答は、一点鎖線で示すＴ_２／ｒの応答と重なっており、式（１５）の条件を満たしている。また、破線で示すｕ_ｆ２＋ｕ_ｅ２＝Ｕ_２の応答は常に設定値である５となっている。

更に、図１２（ｃ）において、実線で示すｕ_ｆ３−ｕ_ｅ３の応答は０であり、式（２０）の条件を満たしている。また、破線で示すｕ_ｆ３＋ｕ_ｅ３＝Ｕ_３の応答は常に１．９０７となっている。各アクチュエータの拮抗対による剛性が目標値に制御されていることから、リンク先端２０２ｂのスティフネス楕円は目標角度において、ｘ−ｙ軸と平行になるように制御されていることがわかる。

次に、リンク先端２０２ｂが外力により押されることを想定するシミュレーションを行う。各リンク２０１，２０２が目標角度に静定後、リンク先端２０２ｂにｙ軸負方向の外力を与える。接触が発生している間、係数値Ｇ_ｔ１，Ｇ_ｔ２を０としている。

図１３にリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。位置決めにおいては、目標軌道に追従している。接触の発生により、係数値Ｇ_ｔ１，Ｇ_ｔ２を０とすると、マニピュレータは人工筋肉アクチュエータの弾性力によって手先の剛性が制御される。本第２実施形態では、手先剛性はｘ−ｙ軸方向に平行になるように剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を設定し、設定値を満たすように３対６筋の駆動力を制御している。そのため、ｙ軸負方向の外乱に対して、手先はｙ軸に平行な方向に柔軟性を発揮していることがわかる。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について説明する。図１４は、本発明の第３実施形態に係るロボット装置の制御装置を示すブロック線図である。なお、本第３実施形態において、上記第２実施形態と同様の構成については、同一符号を付して説明を省略する。

上記第２実施形態では、関節が目標角度に到達した後、接触ゲインである係数値Ｇ_ｔ１，Ｇ_ｔ２を０としてフィードバック制御系を遮断し、筋の弾性による手先剛性の制御を実現した。しかし、目標軌道に追従しながらリンクに柔軟性を持たせるものではない。また、目標角度においてのみスティフネス楕円がｘ−ｙ軸に平行になる制御を行っていた。

（１）制御系設計
本第３実施形態では、リンク先端２０２ｂを目標軌道に追従させながら剛性の制御を同時に行う。また、軌道追従中であっても、スティフネス楕円を常にｘ−ｙ軸に平行になるように制御する。

図１４に示す制御装置２５０Ａは、演算部７０を備えている。演算部７０は、第１，第２のリンク２０１，２０２に対する目標揺動角度（目標軌道）ｒ_ａ１，ｒ_ａ２に応じて常にスティフネス楕円の軸がｘ−ｙ軸と平行になるように第３の剛性指令値Ｕ_３を以下の式（３０）に示す演算式により演算する。

つまり、演算部７０は、図８に示したｘ−ｙ軸直交座標系で、リンク先端２０２ｂにおけるスティフネス楕円の長軸及び短軸のうちの一方の軸が、直交座標系の基準軸（例えばｘ軸）と平行となるように、剛性指令値Ｕ_３を演算式により求める。この式（３０）に示す演算式は、第１の剛性指令値Ｕ_１、第２の剛性指令値Ｕ_２、第１の目標揺動角度ｒ_ａ１、及び第２の目標揺動角度ｒ_ａ２を変数として、第３の剛性指令値Ｕ_３を求める演算式である。

そして、各パラメータＵ_１，Ｕ_２，ｒ_ａ１，ｒ_ａ２を入力した演算部７０は、式（３０）の演算式に基づき、第３の剛性指令値Ｕ_３を演算により生成する（剛性指令値生成ステップ）。そして、第３の駆動力指令値演算部７８は、各駆動力指令値ｕ_ｅ３，ｕ_ｆ３を、第３の剛性指令値Ｕ_３に基づいて設定する（第３の駆動力指令値演算ステップ）。本実施形態では、第３の駆動力指令値演算部７８は、各駆動力指令値ｕ_ｅ３，ｕ_ｆ３を、第３の剛性指令値Ｕ_３に１／２を乗じて得られる値に設定する。これにより、各リンク２０１，２０２を各目標揺動角度ｒ_ａ１，ｒ_ａ２に基づいて揺動させるときに、スティフネス楕円の軸がｘ−ｙ軸に平行となる状態を保つことができる。

更に、本第３実施形態ではゲイン調整器５４，６４の係数値Ｇ_ｔ１，Ｇ_ｔ２を、タッチセンサ５２から検知信号が出力されている期間は、以下の式（３１）に示すように、０から１の間で変化させた任意の値とする。

係数値Ｇ_ｔ１，Ｇ_ｔ２に微小な値をとれば、スティフネス楕円の軸は微小にｘ−ｙ軸から傾くものの、接触物の接触中に各リンク２０１，２０２の目標軌道への追従が可能となる。

特に、第１の係数値Ｇ_ｔ１及び第２の係数値Ｇ_ｔ２は、タッチセンサ５２から検知信号が出力されている期間中、その期間における初期の時点の初期値から時間が経過するに連れて大きくなるように設定されるのがよい。

例えば、図１５に示すように、タッチセンサ５２から検知信号が出力されている期間を時刻４秒から時刻８秒の期間とする。その期間における時刻４秒の時点から時間が経過するに連れて、係数値Ｇ_ｔ１，Ｇ_ｔ２が大きくなるように設定される。図１５に示す例では、各係数値Ｇ_ｔ１，Ｇ_ｔ２が段階的に大きくなるように設定される。

本第３実施形態では、係数値Ｇ_ｔ１，Ｇ_ｔ２は、タッチセンサ５２から検知信号が出力されている期間中においても、０とはならないように設定されており、所定の時間が経過したら、１となるように設定される。これにより、接触物が第２のリンク２０２又はエンドエフェクタに接触した瞬間は、係数値Ｇ_ｔ１，Ｇ_ｔ２が１よりも低く設定されて、各リンク２０１，２０２の柔軟性を保つようにしている。そして、係数値Ｇ_ｔ１，Ｇ_ｔ２は０ではないので、スティフネス楕円の軸は微小にｘ−ｙ軸から傾くものの、接触物の接触中に各リンク２０１，２０２の目標軌道への追従が可能となる。

次いで、所定の時間が経過したら、接触中であっても係数値Ｇ_ｔ１，Ｇ_ｔ２を１とし、各リンク２０１，２０２を目標揺動角度及び目標剛性に調整される。従って、各リンク２０１，２０２は、接触が生じた瞬間は柔軟性を保ち、その後は迅速に目標の位置に位置決めされるので、制御性が向上する。

（２）シミュレーション
図１４に示した制御装置２５０Ａを用いてシミュレーションを行った。リンク２０１，２０２の物理パラメータは上記第２実施形態と同様である。目標軌道（目標揺動角度）は、５秒までは上記第２実施形態と同様の軌道である。

その後１１秒までリンク先端２０２ｂの軌道がｘ軸と平行な直線軌道となるように各リンク２０１，２０２の目標軌道（目標揺動角度）ｒ_ａ１，ｒ_ａ２を逆運動学により求めている。常にリンク先端２０２ｂのスティフネス楕円の軸がｘ−ｙ軸と水平になるように、各人工筋肉アクチュエータを剛性指令値Ｕ_１，Ｕ_２，Ｕ_３により制御する。一関節駆動アクチュエータの剛性は実施例１と同様に、剛性指令値Ｕ_１＝１０，Ｕ_２＝５とし、剛性指令値Ｕ_３を式（３０）より逐次求める。また、接触は４秒から８秒にかけて発生するものとする。そのとき、係数値Ｇ_ｔ１，Ｇ_ｔ２は、図１５に示すように０以外の値を用いる。

図１６（ａ）に第１のリンク２０１の角度θ_１を実線で、目標角度ｒ_ａ１を一点鎖線で示す。また、図１６（ｂ）に第２のリンク２０２の角度θ_２を実線で、目標角度ｒ_ａ２を一点鎖線で示す。また、比較として、第２実施形態で示した、接触中は常に係数値Ｇ_ｔ１，Ｇ_ｔ２を０とする制御（「接触ゲイン０制御」と称す）の応答を破線で示す。さらに、図１７に駆動力ｕ_ｅ３，ｕ_ｆ３の差を実線で、和を破線で示す。

図１６（ａ）及び図１６（ｂ）より、本第３実施形態の制御装置２５０Ａは、４秒で接触が発生するとフィードバック制御系が遮断されてリンク先端２０２ｂの柔らかさを発揮する。そして、第１のリンク２０１は６秒で、第２のリンク２０２は７秒で係数値を１としてフードバック制御系の遮断を解除しているため、８秒以降は目標軌道へと収束している。

一方、破線で示す接触ゲイン０制御では、接触が発生している８秒までフィードバック制御が遮断されている。そのため、８秒以降に大きくなった角度偏差に対して急激に目標軌道への復帰動作が発生し、大きなオーバシュートを発生している。

図１７において、破線で示すｕ_ｆ３＋ｕ_ｅ３＝Ｕ_３は式（３０）に示したように目標角度ｒ_ａ１，ｒ_ａ２に応じて逐次演算されている。これにより、スティフネス楕円は軌道追従中において、ｘ−ｙ軸と平行になるように制御されている。

図１８（ａ）にリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。比較として、図１８（ｂ）に接触ゲイン０制御のリンク先端２０２ｂの軌跡を破線で、位置決めの目標軌道を点線で示す。

本第３実施形態の制御装置２５０Ａでは、接触が発生した後にリンク先端２０２ｂが柔軟性を持つが、同時に軌道追従を行い、リンク先端２０２ｂをｘ軸に水平な方向の軌道に追従させていることがわかる。接触ゲイン０制御では、接触中は軌道制御を行わないため、リンク先端２０２ｂの柔軟性は大きいが、目標軌道から大きく外れてしまうことがわかる。

［第４実施形態］
次に、本発明の第４実施形態に係るロボット装置について説明する。図１９は、本発明の第４実施形態に係るロボット装置の制御装置における要部を示すブロック線図である。なお、本第４実施形態において、上記第２，第３実施形態と同様の構成については、同一符号を付して説明を省略する。

上記第２実施形態ではフィードバック制御系の入力部を遮断したが、十分に関節が静定していない状態では、遮断時にスパイク状のトルク指令値を発生する可能性がある。上記第２，第３実施形態では、第１のトルク指令生成部がＰＩＤ制御器５５であり、第２のトルク指令生成部がＰＩＤ制御器６５である場合について説明したが、本第４実施形態では、第１，第２のトルク指令生成部の構成が異なるものである。

具体的に説明すると、第１のトルク指令生成部４５５は、ＰＩＤ制御器５５と、ＰＩＤ制御器５５の出力側に設けられ、ＰＩＤ制御器５５が出力した演算結果に第１の係数値Ｇ_ｔ１を乗じるゲイン調整器５５Ａとを有している。

また、第１のトルク指令生成部４５５は、ＰＩＤ制御器５５の制御入力をフィードフォワード入力ｆｆｗ_１として入力し、その入力ｆｆｗ_１に（１−Ｇ_ｔ１）の値を乗じるゲイン調整器５５Ｂを有している。また、第１のトルク指令生成部４５５は、ゲイン調整器５５Ａの出力値とゲイン調整器５５Ｂの出力値とを加算処理し、加算結果を第１のトルク指令値Ｔ_１として出力する加算器５５Ｃを有している。

第２のトルク指令生成部４６５は、第１のトルク指令生成部４５５と構成が同一であり、図１９を参照して説明する。第２のトルク指令生成部４６５は、ＰＩＤ制御器６５と、ＰＩＤ制御器６５の出力側に設けられ、ＰＩＤ制御器６５が出力した演算結果に第２の係数値Ｇ_ｔ２を乗じるゲイン調整器６５Ａとを有している。

また、第２のトルク指令生成部４６５は、ＰＩＤ制御器６５の制御入力をフィードフォワード入力ｆｆｗ_２として入力し、その入力ｆｆｗ_２に（１−Ｇ_ｔ２）の値を乗じるゲイン調整器６５Ｂを有している。また、第２のトルク指令生成部４６５は、ゲイン調整器６５Ａの出力値とゲイン調整器６５Ｂの出力値とを加算処理し、加算結果を第２のトルク指令値Ｔ_２として出力する加算器６５Ｃを有している。

ここで、フィードフォワード入力ｆｆｗ_ｎ（ｎ＝１，２）は、タッチセンサ５２が検知信号を出力する直前にＰＩＤ制御器５５，６５が出力したＰＩＤ制御演算による演算結果である。

これにより、タッチセンサ５２から検知信号が出力されている期間は、ＰＩＤ制御器５５は、第１の偏差を示す値（ｒ_ａ１−θ_１）に対してＰＩＤ制御演算を行うが、ゲイン調整器５５Ａは、その演算結果に対して第１の係数値Ｇ_ｔ１を乗じる。ゲイン調整器５５Ｂは、フィードフォワード入力ｆｆｗ_１を入力し、その入力ｆｆｗ_１に（１−Ｇ_ｔ１）の値を乗じる。加算器５５Ｃは、ゲイン調整器５５Ａの出力値とゲイン調整器５５Ｂの出力値との加算結果を第１のトルク指令値Ｔ_１として出力する。

同様に、タッチセンサ５２から検知信号が出力されている期間は、ＰＩＤ制御器６５は、第２の偏差を示す値（ｒ_ａ２−θ_２）に対してＰＩＤ制御演算を行うが、ゲイン調整器６５Ａは、その演算結果に対して第２の係数値Ｇ_ｔ２を乗じる。ゲイン調整器６５Ｂは、フィードフォワード入力ｆｆｗ_２を入力し、その入力ｆｆｗ_２に（１−Ｇ_ｔ２）の値を乗じる。加算器６５Ｃは、ゲイン調整器６５Ａの出力値とゲイン調整器６５Ｂの出力値との加算結果を第２のトルク指令値Ｔ_２として出力する。

具体例を挙げて説明すると、タッチセンサ５２が接触を検知して係数値Ｇ_ｔｎ＝０となった場合は、（１−Ｇ_ｔｎ）＝１となり、トルク指令生成部４５５，４６５が出力するトルク指令値Ｔ_ｎは、タッチセンサ５２が接触を検知する直前の値となる。

従って、各リンク２０１，２０２の動作中にタッチセンサ５２により接触が検知された場合には、偏差が０となる場合のトルク指令値で制御するフィードバック制御に移行するのではなく、接触が生じる直前の偏差に基づくトルク指令値でフィードバック制御する。なお、上記第３実施形態と同様に、検知信号を検知してから時間が経過するに連れて、係数値Ｇ_ｔ１，Ｇ_ｔ２を大きくしていってもよい。そして、接触が生じた瞬間に係数値Ｇ_ｔ１，Ｇ_ｔ２＝０とならない、１よりも小さな値に係数値Ｇ_ｔ１，Ｇ_ｔ２を設定してもよい。

以上、本第４実施形態によれば、各リンク２０１，２０２の動作中に、各リンク２０１，２０２の柔軟性を損なうことなく、トルク指令値Ｔ_１，Ｔ_２のスパイクを防止することができ、制御性が更に向上する。

［第５実施形態］
次に、本発明の第５実施形態に係るロボット装置について説明する。上記第１〜４実施形態では、ロボット装置が作業ロボットとしてのマニピュレータである場合について説明したが、上記第１〜第４実施形態に示した制御装置を、歩行ロボットの脚制御に用いることが可能である。

歩行ロボットでは地面との接地時に脚の剛性を低くすることで、不整地に対する歩行の安定性が高まる。また、接地時にスティフネス楕円を地面と平行にすることにより、地面に対する滑りを抑制することが可能である。しかし、支持脚相では脚の剛性を高くして、体を支持する必要がある。

これに対して、図２０に、上記第３実施形態で示した剛性と追従の同時制御を適用した場合のシミュレーションによるスティック線図を示す。破線で脚先の軌道を、点線で目標軌道を示している。これより、接地時は脚の剛性は低いため柔らかく地面に接地し、支持脚相が進むにつれて脚の剛性が高まり、目標軌道に追従していることがわかる。

［第６実施形態］
次に、本発明の第６実施形態に係るロボット装置について説明する。上記第１〜第５実施形態では、拮抗配置された人工筋肉アクチュエータを用いて位置決めと剛性の同時制御を行った。本第６実施形態では、関節に回転モータを用いる１リンクアームに対して、本発明の制御系を適用する。

ロボット装置の構成は、駆動部として、図２に示すアクチュエータｅ_１，ｆ_１の代わりに、図２１のブロック線図に示す回転モータＰ_０に置き換えたものである。なお、ロボット装置の制御装置は、回転モータＰ_０を駆動するモータ回路Ｐ_１を有している。回転モータＰ_０は、図２を参照し、固定部材であるアーム部材１０４とリンク１０１との関節に配置され、リンク１０１を揺動させる駆動部である。１リンクアームの運動方程式を以下の式（３２）のように定義する。

ここで、ｕ_ｍは回転モータが発生する制御トルクである。つぎに、比例ゲインをｋ、微分ゲインをｂ、仮想的なモーメントアームの長さをｒ、仮想的な筋の駆動力を発生させる駆動力指令値をｕ_ｅ１，ｕ_ｆ１として、１リンクアームに対して以下の式（３３）のような比例−微分フィードバック制御系を構成する。

ここで、第１項はトルク指令値、第２項、第３項は仮想的な筋の駆動力指令値ｕ_ｅ１，ｕ_ｆ１によりゲインが変動する比例−微分フィードバック制御系となっている。

図２１に示すモータ回路Ｐ_１は、式（３２）と式（３３）で構成されるフィードバック制御系である。モータ回路Ｐ_１は、駆動力指令値ｕ_ｅ１，ｕ_ｆ１を入力として、回転モータＰ_０を動作させ、回転モータＰ_０にリンク１０１を駆動させて揺動角度θ_１を出力とするシステムであることがわかる。式（３２）及び式（３３）は、第１実施形態の式（２）で示した人工筋肉アクチュエータを用いた１リンクアームの運動方程式と同一である。これより、回転モータを用いた１リンクアームに対して、式（３３）のように比例−微分フィードバックを施すことで、第１実施形態に示した位置と剛性の同時制御方法をそのまま適用することができることがわかる。

具体的に説明すると、図２１に示すモータ回路Ｐ_１は、回転モータＰ_０を、駆動力の差によりリンク１０１を揺動させる一対の仮想アクチュエータと見做せるように構成されている。そして、図３に示す制御装置１５０と同一構成の制御装置によって、各仮想アクチュエータの駆動力を、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１により調整して、リンク１０１の動作を制御する。

図３を参照して、制御装置の各部の動作について説明すると、まず、揺動角度検知センサ１は、揺動角度（関節の角度）θ_１を検知する。偏差算出部としての減算器３は、アーム部材１０４に対するリンク１０１の揺動角度θ_１と目標揺動角度ｒ_ａとの偏差（差分）を示す値を算出する（偏差算出ステップ）。

トルク指令生成部としてのＰＩＤ制御器５は、偏差を示す値に対してＰＩＤ制御演算を行い、リンク１０１を揺動させるトルク指令値Ｔ_１を生成する（トルク指令生成ステップ）。

乗算部としての乗算器６は、リンク１０１の揺動中心点を中心とする仮想のモーメントアームの長さｒと、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１の和を示す剛性指令値Ｕ_１との乗算値Ｕ_１×ｒを算出する（乗算ステップ）。仮想のモーメントアームの長さｒは、予め設定（不図示の記憶装置に記憶）された定数であり、乗算器６が剛性指令値Ｕ_１を取得したときに、演算により乗算値Ｕ_１×ｒを求める。なお、予め乗算値Ｕ_１×ｒの計算結果をテーブルデータとして設定（不図示の記憶装置に記憶）しておき、乗算器６が剛性指令値Ｕ_１を取得したときにテーブルデータを参照して乗算値Ｕ_１×ｒを求めてもよい。

トルク指令補正部としての飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒ以下の場合は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１をそのまま出力する。また、飽和回路７は、ＰＩＤ制御器５で生成したトルク指令値Ｔ_１の絶対値が乗算値Ｕ_１×ｒを上回った場合は、絶対値が乗算値Ｕ_１×ｒ以下に制限された新たなトルク指令値Ｔ_１を出力する（トルク指令補正ステップ）。

駆動力指令値演算部８は、飽和回路７により出力されたトルク指令値Ｔ_１を用いて、各駆動力指令値ｕ_ｅ１，ｕ_ｆ１を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式に基づいて計算する（駆動力指令値演算ステップ）。なお、剛性指令値Ｕ_１、トルク指令値Ｔ_１及び仮想モーメントアームの長さｒに対応した（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の計算結果をテーブルデータとして予め設定（不図示の記憶装置に記憶）しておいてもよい。この場合、駆動力指令値演算部８は、剛性指令値Ｕ_１、トルク指令値Ｔ_１を取得したときに、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式に基づいた計算結果を、テーブルデータを参照して求めてもよい。

これにより、フィードフォワード制御よりも簡便なフィードバック制御により、リンク１０１の角度制御と関節におけるリンク１０１の剛性制御の同時制御を実現することができる。これにより、リンク１０１の揺動角度と剛性の目標値が変更されても、繰り返し演算を行う必要がないので、迅速にリンク１０１の揺動角度及び剛性を目標値に調整することができる。

また、本第６実施形態では、制御装置は、ゲイン調整器４を有し、タッチセンサ２から検知信号が出力されている期間は、偏差を示す値に０を乗じて新たな偏差（ｒ_ａ−θ_１＝０）を示す値を算出する。これにより、ＰＩＤ制御器５は、外力による作用で実際に偏差が生じたとしても、偏差が０であるとして、検知信号を検知する直前の値と同じ値のトルク指令値Ｔ_１を出力することとなり、リンク１０１の剛性は、目標剛性ｒ_ｓに維持されることとなる。したがって、リンク１０１に外力が作用しても、速やかに係数値Ｇ_ｔ＝０となり、フィードバック制御系が遮断され、リンク１０１の柔軟性が維持される。

［第７実施形態］
上記第１〜第６実施形態では、式（１１）および式（１８）に示した制御入力の制約を行うために、図２２の破線で示す飽和関数を用いてきた。しかし、図２２の破線で示す飽和関数のように微分不可能な関数を用いて制御入力の制約を行うと、制約時に制御入力が急峻に変動してしまうことがある。このような制御入力は、制御対象の機械系高次振動を励起し、制御系が不安定となることがある。

そこで、本第７実施形態では、制約関数として、制御入力の制約に微分可能な関数である双曲線正接関数を用いる。双曲線正接関数を図２２の実線で示す。この双曲線正接関数は、漸近線が乗算値±Ｕ_ｎ×ｒ_ｎ（ｎ＝１，２）である。双曲線正接関数で制約されるトルク指令値（フィードバック制御トルク）Ｔ_ｔｎ（ｎ＝１，２）は、制約される前のトルク指令値Ｔ_ｎ、制御入力の和Ｕ_ｎおよびプーリ径ｒを用いて以下のように表すことができる。

上記第１実施形態では１リンクアームの制御を行った。本第７実施形態では、第１実施形態で用いた図３のブロック線図中の飽和関数を、±Ｕ_１×ｒを漸近線とする双曲線正接関数に置き換えシミュレーションを行う。図２３（ａ）に関節の角度θ_１と目標角度ｒ_ａを示し、実線で双曲線正接関数を用いる関節角度の応答を、破線で飽和関数を用いた関節角度の応答を、点線で目標角度を示す。また、図２３（ｂ）に駆動力ｕ_ｆ１の、制約に双曲線正接関数と飽和関数を用いた応答をそれぞれ実線、破線で示す。駆動力ｕ_ｆ１は、トルク指令値Ｔ_１により式（８）を用いて決定される。そのため、実線で示す制約に双曲線正接関数を用いる応答では、トルクＴ_１が制約される約０．７ｓと約２．５ｓにおいて、駆動力ｕ_ｆ１の応答に急峻な変動が現れなくなっていることがわかる。また、図２３（ａ）より、制御入力の飽和に双曲線正接関数を用いても、位置決め性能に劣化はないことがわかる。

上記第２実施形態では２リンクアームの制御を行った。１リンクアームの制御と同様に、本第７実施形態では、上記第２実施形態で用いた図９のブロック線図中の飽和関数を、双曲線正接関数に置き換えシミュレーションを行う。具体的には、第１の制約関数が、±Ｕ_１×ｒ_１を漸近線とする双曲線正接関数であり、第２の制約関数が、±Ｕ_２×ｒ_２を漸近線とする双曲線正接関数である。

図２４（ａ），（ｂ）に、第１及び第２のリンク２０１，２０２の関節角度θ_１，θ_２と目標角度ｒ_ａ１，ｒ_ａ２を示し、実線で双曲線正接関数を用いる応答を、破線で飽和関数を用いた応答を、点線で目標角度を示す。また、図２５（ａ）及び図２５（ｂ）に駆動力ｕ_ｆ１，ｕ_ｆ２の、双曲線正接関数と飽和関数を用いた応答をそれぞれ実線、破線で示す。図２５（ａ）より、第１のリンク２０１に対するトルクＴ_１が制約される約０．２ｓと約２．２ｓにおいて、双曲線正接関数により駆動力ｕ_ｆ１の応答に急峻な変動が現れなくなっていることがわかる。上記第２実施形態では、第２のリンク２０２に対するトルクＴ_２が制約の範囲内であるため、駆動力ｕ_ｆ２は制約される必要がない。しかし、図２２より双曲線正接関数は制約の範囲内であっても信号を低減する効果がある。そのため、図２５（ｂ）に示す本第７実施形態の双曲線正接関数による駆動力ｕ_ｆ２は飽和関数による駆動力と若干の差異が生じているが、図２４（ａ），（ｂ）から双曲線正接関数を用いても位置決め性能に変化はないことがわかる。

［第８実施形態］
本第８実施形態では、接触ゲインの調整が不要で、また、大きな障害物への着地であっても緩やかな脚制御を可能とする歩行制御系を設計する。さらに、４足歩行ロボットのモデルを用いてシミュレーションを行い、本第８実施形態で提案している制御系により不整地歩行が可能であることを示す。

（１）モデリング
図２６は、本発明の第８実施形態に係るロボット装置としての４足歩行ロボットのリンク配置と筋配置および座標系を示す側面図である。ロボット装置としての４足歩行ロボット８００は、固定部材としての胴体８０３と、４つの支持脚となる４つのリンク機構ＲＦ，ＬＦ，ＲＨ，ＬＨとを備えている。これら４つのリンク機構ＲＦ，ＬＦ，ＲＨ，ＬＨが胴体８０３に接続されている。

ここで、絶対座標系は、ｘ−ｙ平面については紙面内に取り、ｚ座標は紙面に垂直で紙面の裏から表の向きに取る。座標（ｘ_Ｇ，ｙ_Ｇ，ｚ_Ｇ）は胴体８０３の重心の位置を表し、本第８実施形態では胴体８０３の重心を中心としてｘ軸まわり右ねじ方向の傾きをロール、ｚ軸まわり右ねじ方向の傾きをピッチ、ｙ軸まわり右ねじ方向の傾きをヨーと記す。

本第８実施形態では、右後脚がリンク機構ＲＨ、右前脚がリンク機構ＲＦ、左後脚がリンク機構ＬＨ、左前脚がリンク機構ＬＦである。これら右後脚、右前脚、左後脚及び左前脚に対して、それぞれ支持脚にローカルなｘ_ＲＨ−ｙ_ＲＨ、ｘ_ＲＦ−ｙ_ＲＦ、ｘ_ＬＨ−ｙ_ＬＨおよびｘ_ＬＦ−ｙ_ＬＦ座標系を設定する。図２６にはｘ_ＲＨ−ｙ_ＲＨ座標系およびｘ_ＲＦ−ｙ_ＲＦ座標系のみ図示している。ここで支持脚に対する座標系は、ｘ−ｙ座標の軸がロボット装置の胴体８０３の底面部分と水平になるように設定する。

本第８実施形態で制御対象とする４足歩行ロボット８００は、前脚ＲＦ，ＬＦは２リンクマニピュレータ、後脚ＲＨ，ＬＨは３リンクマニピュレータとするが、後脚は第３リンクの角度が９０ｄｅｇに固定された等価的な２リンクマニピュレータとして用いる。そのため、前脚と後脚の双方について、上記第２実施形態に示した３対６筋を有する２リンクマニピュレータに対する制御手法を適用することが可能である。

また、脚先端部には球状の足を設定する。尾と首には質量を設定しているが、頭部には質量を設定していない。この４足歩行ロボットは機構解析ソフトウェアＲｅｃｕｒｄｙｎを用いてモデル化され、本第８実施形態において不整地歩行シミュレーションに用いる。

前脚ＲＦ，ＬＦおよび後脚ＲＨ，ＬＨの第ｎ（ｎ＝１，２）のリンクのパラメータは、以下のとおりである。具体的には、リンク長ｌ_Ｆｎ，ｌ_Ｈｎは、ｌ_Ｆ１＝０．０７０ｍ，ｌ_Ｆ２＝０．１２ｍ，ｌ_Ｈ１＝０．０９０ｍ，ｌ_Ｈ２＝０．１２３５ｍである。また、慣性モーメントＩ_Ｆｎ，Ｉ_Ｈｎは、Ｉ_Ｆ１＝１１．７４×１０^−６ｋｇ・ｍ^２，Ｉ_Ｆ２＝５２．９５９×１０^−６ｋｇ・ｍ^２，Ｉ_Ｈ１＝２２．８１×１０^−６ｋｇ・ｍ^２，Ｉ_Ｈ２＝９４．３５×１０^−６ｋｇ・ｍ^２である。また、質量Ｍ_Ｆｎ，Ｍ_Ｈｎは、Ｍ_Ｆ１＝２．１６×１０^−２ｋｇ，Ｍ_Ｆ２＝３．６２９８×１０^−２ｋｇ，Ｍ_Ｈ１＝２．７１×１０^−２ｋｇ，Ｍ_Ｈ２＝５．２５５８×１０^−２ｋｇである。また、胴体８０３のｘ，ｚ方向の長さは、ｌ_ｂｘ＝０．２８ｍ，ｌ_ｂｚ＝０．０５ｍ、ｚ方向の脚の付け根の間隔は、ｌ_ｂｌ＝０．１５ｍ、質量Ｍ_ｂはＭ_ｂ＝０．７３０９ｋｇ、プーリ径、足球半径は、ｒ＝０．０２ｍ，ｒ_ｆ＝０．００８ｍである。人工筋肉アクチュエータの弾性力定数、粘性力定数は、ｋ＝１５０．５Ｎ／ｍ，ｂ＝０．０１Ｎ・ｓ／ｍである。なお、人工筋肉アクチュエータの弾性力定数は、神田通信工業製マッキベン型アクチュエータ０．２５インチタイプを測定して求めている。

（２）制御系設計
まず、各支持脚ＲＦ〜ＬＨに対して、足先の軌道とスティフネス楕円を設定する。軌道は、遊脚相では足先が楕円の上半分を描く軌道とし、支持脚相では直線軌道とする。また、歩幅は０．０６ｍと設定する。本第８実施形態では、不整地において、支持脚に予期しない着地が生じた際に胴体８０３と垂直な方向に剛性を発揮させることで安定的な歩行を行う。そのため、スティフネス楕円は、上記第２実施形態で示した方法を用いて、各支持脚のローカルな座標系に垂直になるように設定する。

図２７（ａ）に、右後脚であるリンク機構ＲＨのスティック線図を実線で、足先軌道を一点鎖線で、スティフネス楕円を破線で示す。後脚ＲＨはｘ軸正方向が胴体８０３の進行方向となるため、足先は図２７（ａ）中矢印の方向に駆動される。そこで、スティフネス楕円を足先が遊脚相の８割程度の位置でｘ_ＨＲ−ｙ_ＨＲ座標系に垂直になるように設定する。

まず、一関節駆動アクチュエータの剛性を、Ｕ_１Ｒ＝１３，Ｕ_２Ｒ＝１５と設定すると、二関節駆動アクチュエータの剛性は、式（２９）により、Ｕ_３Ｒ＝８．５３７７と求まる。ここで、一関節駆動アクチュエータの剛性は、上記第２実施形態で示した２リンクマニピュレータモデルを用いて、ロボット胴体重量の１／４を手先外乱として与えるシミュレーションを行うことによって決定している。左後脚ＬＨは、右後脚ＲＨと同様の軌道とスティフネス楕円を用いることができる。

同様に、図２７（ｂ）に右前脚であるリンク機構ＲＦのスティック線図を実線で、足先軌道を一点鎖線で、スティフネス楕円を破線で示す。前脚ＲＦはｘ軸負方向が胴体８０３の進行方向となるため、足先は図２７（ｂ）中矢印の方向に駆動される。前脚ＲＦのアクチュエータの剛性は後脚と同様に、Ｕ_１Ｆ＝１３，Ｕ_２Ｆ＝１５と設定すると、二関節駆動アクチュエータの剛性は、式（２９）により、Ｕ_３Ｆ＝５５．８０となる。しかし、この値は一関節駆動アクチュエータの剛性Ｕ_１Ｆ，Ｕ_２Ｆに比べて大きく、二関節駆動アクチュエータの剛性により脚の動作範囲を狭めることになってしまう。そこで、前脚ＲＦに対しては二関節アクチュエータの剛性を１／４に緩和し、Ｕ_３Ｆ′＝１３．９５を用いる。

つぎに、本第８実施形態で用いる歩容を図２８（ａ）の歩容位相ダイアグラムに示す。太線部分が接地面に接地している支持脚相を、細線部分が接地面から離れている遊脚相を表している。歩行周期は１秒とする。本第８実施形態では、不整地歩行を考慮して、接地率β＝０．８とし、右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦの順に遊脚化するウェーブ歩容を用いる。さらに、各支持脚における支持脚相を図２８（ｂ）に示すように複数（４つ）の支持脚相部分Ａ，Ｂ，Ｃ，Ｄに分割する。本第８実施形態では、図２８（ａ）に三角印で示す支持脚相部分Ａの終了時刻で、その時刻での足先位置から支持脚相Ｂの終了位置までの直線軌道を再計算する。そして、斜線部分示している支持脚相Ｂでは、再計算された軌道を用いる。

つぎに、図２９のブロック線図により、接触ゲインＧ_ｔ、即ち、第１の係数値Ｇ_ｔ１及び第２の係数値Ｇ_ｔ２の設定方法を示す。なお、図２９には、４つの支持脚ＲＦ〜ＬＦのうちのいずれか１つの支持脚（リンク機構）Ｐであって、制御装置のうち第１及び第２のアクチュエータのいずれかの制御系を示している。

ここで、支持脚Ｐは、２リンクマニピュレータである。制御系Ｋ＾は、上記第２実施形態で示した制御系と同様である。ＫはＰＩＤ制御器５５（６５）と筋指令配分部分５６〜５８（６６〜６８）である。ゲイン調整器５４（６４）は接触ゲインＧ_ｔを調整する。また、Ｍｅｍｏｒｙは予め設定したリンク機構Ｐの各リンクの軌道データが記憶されている記憶部８２である。Ｃｏｍｐｅｎｓａｔｏｒは軌道データを再計算する計算部８３である。

具体的に説明すると、制御装置８５０は、上記第２実施形態の制御装置２５０の構成のほか、更に、第１の目標揺動角度ｒ_ａ１（及び第２の目標揺動角度ｒ_ａ２）を出力する目標設定部８１を備えている。目標設定部８１は、予め軌道データが記憶されている記憶部８２を有している。目標設定部８１は、遊脚相、第１の支持脚相部分Ａ、第３の支持脚相部分Ｃ及び第４の支持脚相部分Ｄでは、記憶部８２に記憶された、即ち、予め設定された軌道データに基づき、第１の目標揺動角度ｒ_ａ１（及び第２の目標揺動角度ｒ_ａ２）を出力する。また、目標設定部８１は、第２の支持脚相部分Ｂでは、軌道データを修正し、修正された軌道データに基づいて第１の目標揺動角度ｒ_ａ１（及び第２の目標揺動角度ｒ_ａ２）を設定する（目標設定ステップ）。

また、制御装置８５０は、テーブル記憶部８５を有している。テーブル記憶部８５には、遊脚相及び複数の支持脚相部分と、センサ５２からの検知信号の有無に応じた係数値Ｇ_ｔ（Ｇ_ｔ１，Ｇ_ｔ２）を決定するためのテーブルデータＴａが記憶されている。ゲイン調整器５４（６４）は、テーブルデータＴａを参照して、支持脚Ｐの脚相の状態、センサ５２からの検知信号の有無に応じて、係数値Ｇ_ｔ（Ｇ_ｔ１，Ｇ_ｔ２）を決定する。

具体的には、第１の偏差算出部の一部として機能するゲイン調整器５４は、リンク機構Ｐが遊脚相又は残りの３つの支持脚が接地している状態で支持脚相となった場合に、センサ５２から検知信号が出力されている期間は、第１の係数値Ｇ_ｔ１を０に決定する。具体的にはゲイン調整器５４は、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２から検知信号が出力されている期間は、第１の係数値Ｇ_ｔ１を０に決定する。また、ゲイン調整器５４は、それ以外、即ち、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２からの出力がＯＦＦになったときには、第１の係数値Ｇ_ｔ１を１に決定する。さらに、ゲイン調整器５４は、それ以外、即ち、リンク機構Ｐが支持脚相部分Ｂ，Ｃ，Ｄの場合には、センサ５２からの出力がＯＮ／ＯＦＦに関わらず、第１の係数値Ｇ_ｔ１を１に決定する。

また、第２の偏差算出部の一部として機能するゲイン調整器６４は、リンク機構Ｐが遊脚相又は残りの３つの支持脚が接地している状態で支持脚相となった場合に、センサ５２から検知信号が出力されている期間は、第２の係数値Ｇ_ｔ２を０に決定する。具体的にはゲイン調整器６４は、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２から検知信号が出力されている期間は、第２の係数値Ｇ_ｔ２を０に決定する。また、ゲイン調整器６４は、それ以外、即ち、リンク機構Ｐが遊脚相又は支持脚相部分Ａとなった場合に、センサ５２からの出力がＯＦＦになったときには、第２の係数値Ｇ_ｔ２を１に決定する。さらに、ゲイン調整器６４は、それ以外、即ち、リンク機構Ｐが支持脚相部分Ｂ，Ｃ，Ｄの場合には、センサ５２からの出力がＯＮ／ＯＦＦに関わらず、第２の係数値Ｇ_ｔ２を１に決定する。

このように接触ゲインを変更することにより、遊脚相および第１の支持脚相部分Ａでは支持脚Ｐが地面と接触すると、フィードバックループが遮断される。これにより軌道追従は中断され、人工筋肉アクチュエータの粘弾性力のみによる衝撃吸収動作と胴体支持が行われる。

そして、支持脚相部分Ｂに移行すると、支持脚の接触状態にかかわらず、軌道制御が再開され、再計算された軌道データを用いて制御がなされる。つまり、目標設定部８１は、第１の係数値Ｇ_ｔ１及び第２の係数値Ｇ_ｔ２が０から１に変更されたときに、第１の偏差及び第２の偏差に基づき軌道データを再計算し、第１の目標揺動角度ｒ_ａ１及び第２の目標揺動角度ｒ_ａ２を修正して出力する。この修正動作、つまり軌道データの再計算は、支持脚相部分Ｂの終了位置を目標としている。これにより、支持脚相Ｄの終了位置を目標とするよりも、腰または肩の落ち込みを防ぐことが可能である。支持脚相部分Ｃ、Ｄにおいては、予め設定された軌道データに基づく追従制御がなされ、胴体が前方向に移動する。

また、地面に大きな障害物が存在する場合、遊脚相の初期に接触が発生する。そのとき、接触ゲインを０として支持脚相へ移行することとなるが、接触が解除されたとき、目標軌道と足先位置の誤差が大きな状態である。そこで、本第８実施形態では、軌道データ、即ち、第１の目標揺動角度ｒ_ａ１及び第２の目標揺動角度ｒ_ａ２を修正しているので、足先が急峻に復帰しようとするのを防ぐことができる。そのため、４足歩行ロボット８００が大きな障害物を乗り越えるときに平衡を崩すことなく、移動することができる。また、本第８実施形態では、接触ゲインＧｔは０と１のみが用いられるため、試行錯誤による設定の必要がない。

（３）シミュレーション
前節の制御系を用いて不整地歩行のシミュレーションを行う。不整地として高さ１４ｍｍ、ｘ方向の幅７０ｍｍの板状の障害物を設定する。Ｒｅｃｕｒｄｙｎによる４足歩行ロボット８００と障害物Ｏの外観を図３０に示す。障害物Ｏは後脚ＲＨ，ＬＨからｘ方向に０．１ｍの位置に設置し、第２歩行周期で左後脚ＲＦから接触する。

図２９に示した制御系を用いて、６秒間の歩行シミュレーションを行う。障害物Ｏとの接触が生じる後脚の時刻歴応答を、左後脚第１リンク角度θ_ＬＨ１と第２リンク角度θ_ＬＨ２の応答を図３１（ａ），（ｂ）に、右後脚第１リンク角度θ_ＲＨ１と第２リンク角度θ_ＲＨ２の応答を図３１（ｃ），（ｄ）に示す。関節角度の応答を実線で、目標軌道を破線で示している。なお、図２９中のテーブルによって接触ゲインが０とされ、フィードバックループが遮断された時間帯では、軌道制御は行わないため目標軌道は示していない。図３１（ａ），（ｂ）より、左後脚の１．５秒から開始される歩行周期において、遊脚相の途中の約１．６秒で障害物に接地している。その後、１．７５秒で支持脚相Ｂが開始するまでフィードバックループが遮断され、人工筋肉アクチュエータの粘弾性のみによる着地が行われている。そして、１．７５秒の時点での足先位置から、支持脚相Ｂの終了地点までの直線軌道が計算され、その軌道に追従するように制御が行われている。図３１（ｃ），（ｄ）より、右後脚ＲＨでは、障害物Ｏから降りる３〜３．２秒の遊脚相において、遊脚相の途中で地面と接触する。ここでも、接触時に人工筋肉アクチュエータの粘弾性のみによる着地が行われている。

図３２（ａ），（ｂ）に、平地歩行時と、障害物乗り上げ時の左後脚足球中心の軌跡を実線で、目標軌道を破線で示す。図３２（ａ）の平地歩行では、足先は軌道へ追従制御されている。図３２（ｂ）の障害物乗り上げ時では、脚ＬＨが遊脚相の途中で障害物Ｏに接触すると、足先軌道がｙ_ＬＨ座標で負の方向に変位している。これより、人工筋肉アクチュエータの粘弾性のみによる衝撃吸収着地動作が行われていることがわかる。そして、支持脚相Ｂの開始時間で、その時点での足先位置から支持脚相Ｂの終了位置までの直線軌道が計算され、その軌道に追従するように制御がなされる。本第８実施形態の制御系により、予期しない着地に対する柔軟な衝撃吸収動作と、軌道を再計算することによる急峻な乗り上げの抑制が実現していることがわかる。

つぎに、このような脚制御の４足歩行ロボットに対する有効性を図３３（ａ），（ｂ）に示す足の接地状態と図３４（ａ）〜図３４（ｃ）に示す胴体重心の座標を用いて検証する。各脚の接地の状態を図３３（ａ）に歩容位相ダイアグラムを用いて示す。比較として、４足歩行ロボットの各脚に対して、あらかじめ設定した目標関節軌道のみを用いて脚を軌道制御する歩行シミュレーションを行い、その時の各脚の接地の状態を図３３（ｂ）に示す。図３３（ａ），（ｂ）において、白色部分は遊脚相を示し、灰色部分は支持脚相を示す。また、黒枠で囲まれた灰色部分は障害物Ｏに接地する支持脚相を示す。そして、歩行シミュレーションにおいて足が接地している状態を実線で示している。また、ＲＨ、ＲＦ、ＬＨ、ＬＦはそれぞれ右後脚、右前脚、左後脚、左前脚を表す。

図３４（ａ）〜図３４（ｃ）に、胴体重心のロール角度、進行方向の変位および速度の応答を示す。ここで、本第８実施形態の制御手法による応答を実線で、関節軌道の追従のみによる歩行制御による応答を破線で示している。

図３３（ａ）に示す本第８実施形態の制御手法による応答より、４足歩行ロボットは右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦの順に遊脚化することで歩行を行う。まず左後脚ＬＨが１．７秒で障害物Ｏに着地し、右後脚ＲＨは２．２秒で障害物Ｏに着地していることがわかる。そして、２．８秒で左後脚ＬＨが障害物Ｏから降りるが、その際に図３４（ａ）に実線で示すように、胴体８０３が左方向に大きくロールする。その結果、その後の左前脚ＬＦの遊脚相では、脚は遊脚化できず着地状態が続くため、人工筋肉アクチュエータの粘弾性のみによる着地動作が行われる。これにより、胴体８０３のロールが抑制され、姿勢を回復することができる。その結果、図３３（ａ）より、その後右後脚ＲＨから続く全ての遊脚相において、脚が遊脚化され、歩行を継続できていることがわかる。図３４（ａ）からも、障害物Ｏを降りた４〜６秒のロール角度の変動幅は、障害物Ｏに乗る前のロール角度の変動幅とほぼ同様であることがわかる。また、図３４（ｂ），（ｃ）より、障害物Ｏの前後で速度が落ちることなく前方に歩行していることがわかる。

関節軌道の追従のみによる歩行制御では、図３３（ｂ）に示す着地状態より、４足歩行ロボットは本実施例の提案手法と同様に２．８秒で左後脚ＬＨが障害物Ｏから降りている。そして、その際に胴体８０３が左方向に大きくロールし、その後の左前脚ＬＦは遊脚化されていない。しかし、着地状態でも遊脚軌道に追従しようと制御されるためロールは抑制されず、さらに、左前脚ＬＦで地面を押すこととなる。その結果、右方向のロールが発生し、３．２秒の右前脚ＲＦの遊脚相においても、右前脚ＲＦは遊脚化されず地面を押し、３．８秒で再び左方向のロールを発生させる。胴体８０３のロール角度は、図３４（ａ）に破線で示すように、２．８秒で発生したロールが４．８秒付近まで抑制されていないことがわかる。そのため、図３３（ｂ）より、３秒以降は遊脚相で足が着地している状態が多く、歩行できない状態が４．５秒まで継続している。図３４（ｃ）に破線で示すように、２．８〜４．８秒にかけて、遊脚相において着地中に軌道制御が継続されることにより、脚が地面を押すため胴体重心に負方向の速度が発生していることがわかる。その結果、図３４（ｂ）に破線で示すように、２．８秒〜４．８秒にかけて前方への歩行が停滞していることがわかる。

歩行シミュレーションにおける、足球の中心の軌跡を図３５（ａ），（ｂ）に示す。右後脚ＲＨ、右前脚ＲＦ、左後脚ＬＨ、左前脚ＬＦをそれぞれ実線、点線、破線、一点鎖線で、さらに障害物Ｏを含む地面を太実線で示す。図３５（ａ）に示す本第８実施形態の制御系による応答では、障害物Ｏにより停滞せずに歩行が可能であることがわかる。一方、図３５（ｂ）に関節軌道の追従のみによる歩行による応答では、障害物Ｏから降りる時点以降で足先が後方に変位していることがわかる。これは、遊脚相で足が着地し、そのまま軌道制御が継続されてしまい、地面から押し戻されてしまうためである。

図３６（ａ）に本第８実施形態の制御手法によるロボットの姿勢推移を、４．２秒から０．５秒おきに示す。比較として、図３６（ｂ）に関節軌道の追従のみによる歩行制御の姿勢推移を示す。本第８実施形態の制御手法では、段差を降りた後に安定した姿勢で歩行を行うことが可能である。一方、関節軌道の追従のみによる歩行制御では、上から５コマ目に示す段差を降りて２秒後の時点でも大きく姿勢を崩していることがわかる。

図３７に胴体重心の重力方向の加速度を、本第８実施形態の制御手法を実線で、関節軌道の追従のみによる歩行制御を点線で示す。本第８実施形態の制御手法では、人工筋肉アクチュエータの粘弾性による柔らかな着地を実現しているため、重力方向の加速度変動が少なく、また加速度の最大値も小さいことがわかる。

このように、本第８実施形態の制御手法により、柔らかな着地と急峻な乗り上げの抑制が実現されることから、障害物Ｏなどが存在する不整地であっても安定的な歩行を実現可能であることがわかる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。上記第１実施形態では、リンク機構が１リンク構成の場合について説明したが、多リンク構成であっても適用可能である。例えば、リンク機構が２リンク構成の場合において、２対４筋のアクチュエータとすることも可能であり、この場合、各一対のアクチュエータにおいて、それぞれ第１実施形態と同様に制御すればよい。また、上記第４，第５，第８実施形態では、リンク機構が２リンク構成の場合について説明したが、１リンク構成の場合についても適用可能である。また、制御装置の各部が個別のハードウェアで実現される場合について説明したが、ＣＰＵ及びメモリ等を有するコンピュータにおいてＣＰＵがメモリに記憶されたプログラムを実行することにより、ＣＰＵが制御装置の各部として機能する場合であってもよい。

１…揺動角度検知センサ、２…タッチセンサ（センサ）、３…減算器（偏差算出部）、４…ゲイン調整器（偏差算出部）、５…ＰＩＤ制御器（トルク指令生成部）、６…乗算器（乗算部）、７…飽和回路（トルク指令補正部）、８…駆動力指令値演算部、５１…第１の揺動角度検知センサ、５２…タッチセンサ（センサ）、５３…減算器（第１の偏差算出部）、５４…ゲイン調整器（第１の偏差算出部）、５５…ＰＩＤ制御器（第１のトルク指令生成部）、５６…乗算器（第１の乗算部）、５７…飽和回路（第１のトルク指令補正部）、５８…第１の駆動力指令値演算部、６１…第２の揺動角度検知センサ、６３…減算器（第２の偏差算出部）、６４…ゲイン調整器（第２の偏差算出部）、６５…ＰＩＤ制御器（第２のトルク指令生成部）、６６…乗算器（第２の乗算部）、６７…飽和回路（第２のトルク指令補正部）、６８…第２の駆動力指令値演算部、７０…演算部、７８…第３の駆動力指令値演算部、１００…ロボット装置、１０１…リンク、１０１ａ…基端、１０１ｂ…先端、１０３…ベース部材（固定部材）、１０４…アーム部材（固定部材）、１５０…制御装置、２００…ロボット装置、２０１…第１のリンク、２０２…第２のリンク、２０３…プーリ（固定部材）、２５０…制御装置、２５０Ａ…制御装置、４５５…第１のトルク指令生成部、４６５…第２のトルク指令生成部、ｅ_１，ｆ_１…アクチュエータ，第１のアクチュエータ、ｅ_２，ｆ_２…第２のアクチュエータ、ｅ_３，ｆ_３…第３のアクチュエータ、Ｐ_０…回転モータ

Claims (14)

1. 基端が固定部材に揺動可能に支持されたリンクと、前記固定部材に対して前記リンクを揺動させる駆動部と、を有するリンク機構を備え、前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御するロボット装置の制御方法において、
   前記固定部材に対する前記リンクの揺動角度と目標揺動角度との偏差を算出する偏差算出ステップと、
   前記偏差を補償するトルク指令値Ｔ_１を生成するトルク指令生成ステップと、
   前記トルク指令値Ｔ_１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記第１及び第２の駆動力指令値の和を示す剛性指令値Ｕ_１との乗算値Ｕ_１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ_１を補正するトルク指令補正ステップと、
   前記トルク指令補正ステップにより補正されたトルク指令値Ｔ_１を用いて、前記各駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式により求める駆動力指令値演算ステップと、
   を備えたことを特徴とするロボット装置の制御方法。
2. 前記ロボット装置が、前記リンクに外力が作用したことにより検知信号を発生するセンサを有しており、
   前記偏差算出ステップでは、前記センサから検知信号が出力されている期間には０以上１以下の範囲内の値であり、前記センサからの検知信号の出力が停止されている期間には１の値である係数値Ｇ_ｔを前記偏差に乗じた、新たな偏差を算出することを特徴とする請求項１に記載のロボット装置の制御方法。
3. 前記偏差算出ステップでは、前記センサから検知信号が出力されている期間において、前記係数値Ｇ_ｔを０に設定することを特徴とする請求項２に記載のロボット装置の制御方法。
4. 前記偏差算出ステップでは、前記センサから検知信号が出力されている期間において、前記係数値Ｇ_ｔを、時間が経過するに連れて初期値から大きくなるように設定することを特徴とする請求項２に記載のロボット装置の制御方法。
5. 前記トルク指令生成ステップでは、前記センサから検知信号が出力されている期間は、前記偏差算出ステップにより算出された偏差に対してＰＩＤ制御演算を行い、この演算結果に対して前記係数値Ｇ_ｔを乗じた値に、前記センサから検知信号が出力される直前のＰＩＤ制御演算の演算結果に（１−Ｇ_ｔ）の値を乗じた値を加算して、トルク指令値Ｔを生成する、ことを特徴とする請求項２乃至４のいずれか１項に記載のロボット装置の制御方法。
6. 前記固定部材が４足歩行ロボットの胴体であり、前記リンク機構が前記胴体に接続された４つの支持脚のうちのいずれかの支持脚であり、
   前記リンクの軌道データに基づいて前記目標揺動角度を設定する目標設定ステップを備え、
   前記偏差算出ステップでは、前記センサから検知信号が出力されている期間においては、前記リンクの脚相に応じて、前記係数値Ｇ_ｔを０又は１に決定し、
   前記目標設定ステップでは、前記係数値Ｇ_ｔが０から１に変更されたときに、前記軌道データを前記偏差に基づいて再計算することを特徴とする請求項２乃至５のいずれか１項に記載のロボット装置の制御方法。
7. 前記制約関数は、前記トルク指令生成ステップで生成したトルク指令値Ｔ_１の絶対値が前記乗算値Ｕ_１×ｒ以下の場合は、前記トルク指令生成ステップで生成したトルク指令値Ｔ_１のままとし、前記トルク指令生成ステップで生成したトルク指令値Ｔ_１の絶対値が前記乗算値Ｕ_１×ｒを上回った場合は、絶対値が前記乗算値Ｕ_１×ｒに制限された新たなトルク指令値Ｔ_１を生成する飽和関数であることを特徴とする請求項１乃至６のいずれか１項に記載のロボット装置の制御方法。
8. 前記制約関数は、±Ｕ_１×ｒを漸近線とする双曲線正接関数であることを特徴とする請求項１乃至６のいずれか１項に記載のロボット装置の制御方法。
9. 前記駆動部が、一端が前記固定部材に接続され、他端が前記リンクに接続され、収縮により駆動力を発生させて前記リンクを揺動させる一対の一関節筋アクチュエータであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置の制御方法。
10. 前記駆動部が、前記関節に設けられた回転モータであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置の制御方法。
11. 基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構を備え、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御するロボット装置の制御方法において、
    前記固定部材に対する前記第１のリンクの第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出ステップと、
    前記第１の偏差を補償する第１のトルク指令値Ｔ_１を生成する第１のトルク指令生成ステップと、
    前記第１のトルク指令値Ｔ_１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ_１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ_１を補正する第１のトルク指令補正ステップと、
    前記第１のトルク指令補正ステップにより補正された第１のトルク指令値Ｔ_１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式により求める第１の駆動力指令値演算ステップと、
    前記第１のリンクに対する前記第２のリンクの第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出ステップと、
    前記第２の偏差を補償する第２のトルク指令値Ｔ_２を生成する第２のトルク指令生成ステップと、
    前記第２のトルク指令値Ｔ_２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ_２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ_２を補正する第２のトルク指令補正ステップと、
    前記第２のトルク指令補正ステップにより補正された第２のトルク指令値Ｔ_２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式により求める第２の駆動力指令値演算ステップと、
    を備えたことを特徴とするロボット装置の制御方法。
12. 前記第１のリンク及び前記第２のリンクが揺動する平面内に直交座標系を定義し、前記第２のリンクの先端における剛性の大きさを、前記直交座標系に前記第２のリンクの先端を中心とするスティフネス楕円で表し、前記スティフネス楕円の長軸及び短軸のうちの一方の軸が前記直交座標系における基準軸に対して平行となるように、前記一対の第３のアクチュエータの駆動力指令値の和を示す第３の剛性指令値Ｕ_３を生成する剛性指令値生成ステップと、
    前記第３の剛性指令値Ｕ_３に基づいて前記各第３のアクチュエータの駆動力指令値を求める第３の駆動力指令値演算ステップと、を備えたことを特徴とする請求項１１に記載のロボット装置の制御方法。
13. 基端が固定部材に揺動可能に支持されたリンク、前記固定部材に対して前記リンクを揺動させる駆動部を有するリンク機構と、
    前記リンクを第１の揺動方向に揺動させる駆動力を示す第１の駆動力指令値と、前記リンクを前記第１の揺動方向とは反対の第２の揺動方向に揺動させる駆動力を示す第２の駆動力指令値との差に基づき前記駆動部を動作させて前記リンクの揺動動作を制御すると共に、前記第１の駆動力指令値と前記第２の駆動力指令値との和に基づき前記駆動部を動作させて前記固定部材と前記リンクとの関節の剛性を制御する制御装置と、を備え、
    前記制御装置は、
    前記固定部材に対する前記リンクの揺動角度を検知する揺動角度検知センサと、
    前記揺動角度と目標揺動角度との偏差を算出する偏差算出部と、
    前記偏差を補償するトルク指令値Ｔ_１を生成するトルク指令生成部と、
    前記トルク指令値Ｔ_１の絶対値を、前記リンクの揺動中心点を中心とするモーメントアームの長さｒと前記各駆動力指令値の和を示す剛性指令値Ｕ_１との乗算値Ｕ_１×ｒ以下に制約する制約関数に従って、前記トルク指令値Ｔ_１を補正するトルク指令補正部と、
    前記トルク指令補正部で補正したトルク指令値Ｔ_１を用いて、前記各駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ）／２，（Ｕ_１−Ｔ_１／ｒ）／２の演算式により求める駆動力指令値演算部と、を有する特徴とするロボット装置。
14. 基端が固定部材に揺動可能に支持された第１のリンクと、基端が前記第１のリンクの先端に揺動可能に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、駆動力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、駆動力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、駆動力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を有するリンク機構と、
    前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータの各アクチュエータの駆動力を、各駆動力指令値により設定して、前記第１のリンク及び前記第２のリンクの動作を制御する制御装置と、を備え、
    前記制御装置は、
    前記固定部材に対する前記第１のリンクの第１の揺動角度を検知する第１の揺動角度検知センサと、
    前記第１の揺動角度と第１の目標揺動角度との第１の偏差を算出する第１の偏差算出部と、
    前記第１の偏差を補償する第１のトルク指令値Ｔ_１を生成する第１のトルク指令生成部と、
    前記第１のトルク指令値Ｔ_１の絶対値を、前記第１のリンクの揺動中心点を中心とするモーメントアームの長さｒ_１と前記一対の第１のアクチュエータの駆動力指令値の和を示す第１の剛性指令値Ｕ_１との第１の乗算値Ｕ_１×ｒ_１以下に制約する第１の制約関数に従って、前記第１のトルク指令値Ｔ_１を補正する第１のトルク指令補正部と、
    前記第１のトルク指令補正部で補正した第１のトルク指令値Ｔ_１を用いて、前記各第１のアクチュエータの駆動力指令値を、（Ｕ_１＋Ｔ_１／ｒ_１）／２，（Ｕ_１−Ｔ_１／ｒ_１）／２の演算式により求める第１の駆動力指令値演算部と、
    前記第２のリンクの前記第１のリンクに対する第２の揺動角度を検知する第２の揺動角度検知センサと、
    前記第２の揺動角度と第２の目標揺動角度との第２の偏差を算出する第２の偏差算出部と、
    前記第２の偏差を補償する第２のトルク指令値Ｔ_２を生成する第２のトルク指令生成部と、
    前記第２のトルク指令値Ｔ_２の絶対値を、前記第２のリンクの揺動中心点を中心とするモーメントアームの長さｒ_２と、前記一対の第２のアクチュエータの駆動力指令値の和を示す第２の剛性指令値Ｕ_２との第２の乗算値Ｕ_２×ｒ_２以下に制約する第２の制約関数に従って、前記第２のトルク指令値Ｔ_２を補正する第２のトルク指令補正部と、
    前記第２のトルク指令補正部で補正した第２のトルク指令値Ｔ_２を用いて、前記各第２のアクチュエータの駆動力指令値を、（Ｕ_２＋Ｔ_２／ｒ_２）／２，（Ｕ_２−Ｔ_２／ｒ_２）／２の演算式により求める第２の駆動力指令値演算部と、を有することを特徴とするロボット装置。

Images