JP5582937B2 - ロボット装置 - Google Patents

Description

本発明は、リンクの先端におけるステフィネス特性を表す楕円の軸を任意に傾けるロボット装置に関するものである。

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

ハンドの柔軟性の制御を実現するために、ハンドに力センサを装着するインピーダンス制御や、人工筋肉アクチュエータを用いる制御等が行われている。人間の筋はアクチュエータであると同時に、粘弾性可変の制御機構であることが知られている。人工筋肉の中でも、特にＭｃＫｉｂｂｅｎ型人工筋肉に代表される空気圧ゴム人工筋肉は、粘弾性特性が筋に類似している。

人工筋肉アクチュエータは、収縮方向のみに力が発生するため、拮抗配置する必要がある。第１のリンクと第２のリンクにそれぞれ人工筋肉アクチュエータを拮抗配置すると共に、二関節同時駆動アクチュエータを拮抗配置し、３対６筋を有する２リンクモデルが提案されている（非特許文献１参照）。二関節同時駆動アクチュエータとは、第１のリンクと第２のリンクを同時に駆動するように配置されているアクチュエータである。そして、第２のリンクの先端における剛性は、スティフネス特性（剛性の分布）を示す楕円（以下、「スティフネス楕円」という）で表される。

非特許文献１では、スティフネス楕円の長軸、短軸及び傾斜角度を任意に設定している。そして、スティフネス楕円の傾きを調整することで、ハンドに外力が加わった場合に、外力方向とハンドの移動方向とが一致する剛性特性とすることができる。つまり、２つのリンクの長さを同一とした条件の下で、人工筋肉アクチュエータの弾性を等しくし、第１のリンクの基端における関節と第２のリンクの先端とを結ぶ線上に、スティフネス楕円の長軸又は短軸が重なるように傾斜角度を設定すればよい。これにより、人工筋肉アクチュエータの弾性を制御することのみで、コンプライアンス制御が可能となる。

二関節筋機能を有するロボットアーム、大島徹、熊本水頼、機械学会論文集、Ｃ編、Ｖｏｌ６１、１９９５、ｐｐ４６９６−４７０３

しかし、非特許文献１では、２つのリンクの長さが同一である場合を前提としており、２つのリンクの長さが異なる場合に３対６筋の人工筋肉アクチュエータの弾性を個別に設定し、任意のスティフネス楕円を実現する方法については述べられていない。

つまり、２つのリンクの長さを同一とした場合に、スティフネス楕円の軸が傾斜角度φ傾いた条件でヤコビ行列を導入して得られる微小手先変位と微小外力との関係式を、各アクチュエータ対による剛性について代数的に解くことは可能である。従って、２つのリンクの長さが異なる場合も、任意のスティフネス楕円を実現する方法として、傾斜角度φ傾いた条件でヤコビ行列を導入して得られる微小手先変位と微小外力との関係式を、各アクチュエータ対による剛性について解くことが考えられる。

しかし、関係式の解を代数的に導き出すのは困難であったため、収束計算を行う方法で任意のスティフネス楕円を得るのが現実的であった。ところが、収束計算の計算量は膨大であり、収束計算に時間を要し、スティフネス楕円を高速に求めることができなかった。従って、迅速に所望のスティフネス楕円を得ることができず、アクチュエータによる剛性の制御性が低いものであった。

そこで、本発明は、２つのリンクの長さが異なっても、迅速にスティフネス特性を示す楕円を求めることができるロボット装置を提供することを目的とするものである。

本発明は、長手部材からなり、基端が固定部材に揺動可能に支持された第１のリンクと、長手部材からなり、前記第１のリンクが揺動する平面内で揺動可能に基端が前記第１のリンクの先端に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、収縮力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、収縮力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、収縮力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を備えたロボット装置において、前記平面内に直交座標系を定義し、前記第２のリンクの先端におけるスティフネス特性が前記直交座標系に前記第２のリンクの先端を中心とする楕円で表されるときの前記楕円の長軸及び短軸のうちの一方の軸が、前記直交座標系における基準軸に対して平行となる条件で導かれた演算式であって、前記基準軸を基準とする前記第１のリンクの第１の揺動角度、及び前記第１のリンクの長手方向に延びる軸線を基準とする前記第２のリンクの第２の揺動角度を変数とし、前記第１のリンクの長さ及び前記第２のリンクの長さを定数として、前記一対の第１のアクチュエータの各収縮力指令値の和を示す第１の剛性指令値を求める第１の演算式、前記一対の第２のアクチュエータの各収縮力指令値の和を示す第２の剛性指令値を求める第２の演算式、及び前記一対の第３のアクチュエータの各収縮力指令値の和を示す第３の剛性指令値を求める第３の演算式を予め記憶する記憶部と、前記第１のリンクの第１の揺動角度の値を示す信号と、前記基準軸を基準とする前記一方の軸の目標傾斜角度の値を示す信号とを取得し、第１の揺動角度の値から目標傾斜角度の値を減算した結果を新たな第１の揺動角度の値として、新たな第１の揺動角度の値を示す信号を出力する減算部と、前記減算部が出力した第１の揺動角度の値を示す信号と、前記第２のリンクの第２の揺動角度の値を示す信号とを取得し、前記記憶部に記憶された前記第１の演算式、前記第２の演算式及び前記第３の演算式から、前記第１の剛性指令値、前記第２の剛性指令値及び前記第３の剛性指令値を算出する演算部と、前記演算部により算出された前記各剛性指令値から、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータのそれぞれのアクチュエータの収縮力指令値を生成する収縮力指令値生成部と、を備えたことを特徴とする。

本発明によれば、減算部は、第１のリンクの第１の揺動角度の値からスティフネス特性を示す楕円の目標傾斜角度の値を減算することで、新たな第１の揺動角度の値を得ている。記憶部には、スティフネス特性を示す楕円の軸が直交座標系の基準軸と平行な条件で第１、第２及び第３の剛性指令値を求める各演算式が記憶されている。従って、減算部から第１の揺動角度の値を示す信号を取得した演算部は、記憶部に記憶された各演算式を用いて、スティフネス特性を示す楕円の軸を、直交座標系の基準軸から目標傾斜角度の値傾斜させた場合の各剛性指令値を求めることとなる。このように、演算部は、２つのリンクの長さが異なる場合であっても、収束計算によらず代数的に求められた演算式を用いて各剛性指令値を求めることができるので、計算負荷が軽減し、迅速にスティフネス特性を示す楕円を求めることができる。

本発明の実施の形態に係るロボット装置の概略構成を示す説明図である。 ロボット装置のアクチュエータの粘弾性モデルを示す図である。 第２のリンクの先端におけるスティフネス楕円を説明するための図である。 スティフネス楕円の設定法を説明するための図である。 ロボット装置の制御装置を示すブロック線図である。 制御装置の制御部を示すブロック線図である。 スティフネス楕円の傾斜角度を変化させたシミュレーション結果を示す図であり、（ａ）は目標剛性係数の値をα^＊＝３０，β^＊＝１００と設定した場合を示す図、（ｂ）は目標剛性係数の値をα^＊＝１０，β^＊＝１００と設定した場合を示す図である。 各リンクの揺動角度の値を変化させたときのシミュレーション結果を示す図であり、（ａ）はスティフネス楕円の設定パラメータ｛α，β，φ｝を｛０．６２，２，−２０｝とした場合の図である。（ｂ）はスティフネス楕円の設定パラメータ｛α，β，φ｝を｛０．２，２，４０｝とした場合の図である。（ｃ）はスティフネス楕円の設定パラメータ｛α，β，φ｝を｛２，２，０｝とした場合の図である。 第２のリンクの先端に外力を作用させたときのスティフネス楕円のシミュレーション条件を示す図である。 図９のスティフネス楕円の条件で第２のリンクの先端に外力を作用させたときの第２のリンクの先端の軌跡を示すシミュレーション結果の図である。

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

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

第１のリンク１０１は、長手部材からなり、その基端１０１ａがプーリ１０３にｘ−ｙ直交座標系の平面（以下、「作業平面」という）内で揺動可能に支持されている。第２のリンク１０２は、長手部材からなり、その基端１０２ａが第１のリンク１０１の先端１０１ｂに作業平面内で揺動可能に支持されている。この第２のリンク１０２の先端１０２ｂには、不図示のエンドエフェクタ（例えばハンド）が設けられている。つまり、第１のリンク１０１は、第１の関節と第２の関節との間に配置され、第１の関節で揺動可能に支持されており、第２のリンク１０２は、第２の関節で揺動可能に支持されている。

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

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

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

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

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

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

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

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

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

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

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

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

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

ここで、スティフネス楕円の短半径（短軸方向の剛性係数）をα、スティフネス楕円の長半径（長軸方向の剛性係数）をβ、ｘ−ｙ軸直交座標系における基準軸をｘ軸とする。また、基準軸であるｘ軸に対するスティフネス楕円の長軸及び短軸のうちの一方の軸（図３では短軸）の傾斜角度をφとする。つまり、楕円の短半径αは傾斜角度φを０とするときのｘ軸方向の第２のリンク１０２の先端１０２ｂの作業平面における剛性係数を表し、長半径βは傾斜角度φを０とするときのｙ軸方向の第２のリンク１０２の先端１０２ｂの作業平面における剛性係数を表す。

ｘ−ｙ軸に平行な微小な外力ΔＦ_ｘ，ΔＦ_ｙによるリンク先端の微小な変位をΔｘ_ｔ，Δｙ_ｔとする。このとき、スティフネス楕円がｘ−ｙ軸に平行であり、手先の作業平面において、ｘ，ｙ軸方向の剛性係数がそれぞれα，βであるという条件は、

である。

人工筋肉アクチュエータは収縮力指令が与えられると、力を発生すると同時に、それに比例して弾性力が高まる。一対の人工筋肉アクチュエータが拮抗して配置されると、拮抗している筋の弾性力の和が関節周りの剛性係数を変動させる。式（２）より、例えば一対の第１のアクチュエータｅ_１，ｆ_１の拮抗対による第１関節周りの剛性係数は、（ｕ_ｆ１＋ｕ_ｅ１）×ｋ×ｒ^２となる。

これより、３対６筋を有する２リンクマニュピレータのロボット装置１００では、各アクチュエータに収縮力指令を与えて弾性力を変動させることにより手先の剛性を可変にすることができる。そこで、本実施の形態では、一対のアクチュエータ｛ｅ_ｎ，ｆ_ｎ｝（ｎ＝１，２，３）による収縮力指令値ｕ_ｅｎ，ｕ_ｆｎの和である剛性指令値をＵ_ｎとおき、

と表す。つまり、各一対のアクチュエータ｛ｅ_ｎ，ｆ_ｎ｝（ｎ＝１，２，３）によりリンク１０１，１０２を引っ張る収縮力が、第２のリンク１０２の先端１０２ｂにおける剛性（スティフネス楕円）を規定する。

ここで、３対６筋を有する２リンクマニピュレータにおいて、各リンク長が等しい場合、収縮力指令値の和である剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を

と制御すると、スティフネス楕円の軸は、マニピュレータの姿勢に関わらず第１の関節と手先を結ぶ方向を向くことを示している。しかし、リンク長が異なる場合に、任意のスティフネス楕円を設定する方法は明らかではなかった。

本実施の形態では、第１のリンク１０１の長手方向の長さをｌ_１、第２のリンク１０２の長手方向の長さをｌ_２とする。また、第２のリンク１０２の先端１０２ｂにおけるスティフネス楕円の短半径をα，長半径をβとし、傾斜角度をφとするための収縮力指令値の和である第１の剛性指令値Ｕ_１、第２の剛性指令値Ｕ_２及び第３の剛性指令値Ｕ_３を導出する。この場合、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めるには数式が複雑になり、直接的に剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めることは難しい。

そこで、まず、スティフネス楕円の短半径がα，長半径がβとなり、傾斜角度が０で条件（つまり、短軸及び長軸のうちのいずれか一方の軸が、基準軸であるｘ軸と平行になる条件）における剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求める演算式を導く。演算式の導出方法を以下に示す。

微小な外力ΔＦ_ｘ，ΔＦ_ｙによる、関節の微小な回転角度をΔθ_１，Δθ_２とする。微小な回転が起こると、筋の粘弾性を有するアクチュエータは、筋の弾性力によりリンク１０１，１０２に微小なトルクΔＴ_ｐ１，ΔＴ_ｐ２を発生させ、以下のような式（７），（８）で表せる。

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

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

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

この式（１１）と式（４）と比較すると、Ｊ_Ｓ（１，２）＝Ｊ_Ｓ（２，１）であるため、以下の連立方程式（１１）〜（１４）が求まる。

式（１２）〜（１４）は、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３について解くことができ、以下のように求まる。

各リンク１０１，１０２の揺動角度θ_１，θ_２の値と剛性係数α，βの値を、これらの式（１５）〜式（１７）に代入することにより、スティフネス楕円の一方の軸（短軸）をｘ−ｙ直交座標系のｘ軸に平行となる剛性指令値Ｕ_１，Ｕ_２，Ｕ_３が求まる。つまり、式（１５）により第１の剛性指令値Ｕ_１が求まり、式（１６）により第２の剛性指令値Ｕ_２が求まり、式（１７）により第３の剛性指令値Ｕ_３が求まる。

次に、楕円の短半径をα，長半径をβにする条件に、楕円をφ回転させる条件を加える方法を示す。ここで、図４に示すように、ｘ−ｙ座標をφ回転したｘ_ｏ−ｙ_ｏ座標系を導入する。スティフネス楕円がｘ_ｏ−ｙ_ｏ座標系に平行となる剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めれば、元のｘ−ｙ座標から見ればスティフネス楕円はφの傾きを持つことになる。

本実施の形態では、第２の揺動角度θ_２を第１のリンク１０１に対する第２のリンク１０２の相対角度としている。従って、スティフネス楕円がｘ_ｏ−ｙ_ｏ座標系に平行となる剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めるには、式（１５）〜式（１７）における第１の揺動角度θ_１を、第１の揺動角度θ_１から傾きφを差分した値、

に置き換え、式（１５）〜式（１７）に代入すればよい。

以下、本実施の形態のロボット装置１００の制御装置１５０について説明する。図５に、本実施形態のスティフネス楕円を導出する制御装置１５０をブロック線図で示す。

制御装置１５０は、基準軸であるｘ軸を基準とする第１のリンク１０１の第１の揺動角度を検出し、第１の揺動角度の値θ_１ ^＊を示す信号を出力する第１の揺動角度検知センサ１１を備えている。また、制御装置１５０は、第１のリンク１０１の長手方向に延びる軸線を基準とする第２のリンク１０２の第２の揺動角度を検知し、第２の揺動角度の値θ_２ ^＊を示す信号を出力する第２の揺動角度検知センサ１２を備えている。

各揺動角度検知センサ１１，１２は、ポテンショメータ又はエンコーダ等のセンサである。第１の揺動角度検知センサ１１は、図１において図示は省略しているが、プーリ１０３とリンク１０１との第１の関節に設けられており、第１の関節の角度を検知する。また、第２の揺動角度検知センサ１２は、図１において図示は省略しているが、第１のリンク１０１と第２のリンク１０２との第２の関節に設けられたおり、第２の関節の角度を検知する。

また、制御装置１５０は、オペレータの操作により、スティフネス楕円の目標短半径である短軸方向の目標剛性係数の値α^＊を示す信号、スティフネス楕円の目標長半径である長軸方向の目標剛性係数の値β^＊を示す信号を出力する端末１３を備えている。この端末１３は、オペレータの操作により、更に目標傾斜角度の値φ^＊を示す信号を出力する。ここで、目標傾斜角度の値φ^＊とは、ｘ−ｙ軸直交座標系における基準軸であるｘ軸を基準とした、スティフネス楕円の長軸及び短軸のうちの一方の軸（例えば短軸）の目標の傾斜角度の値である。

また、制御装置１５０は、揺動角度の値θ_１ ^＊，θ_２ ^＊を示す信号を揺動角度検知センサ１１，１２から取得し、目標剛性係数の値α^＊，β^＊を示す信号及び目標傾斜角度の値φ^＊を示す信号を端末１３から取得する制御部１を備えている。制御部１は、ＣＰＵ及びメモリ等を有しており、取得した各値から演算により剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を算出する。

また、制御装置１５０は、制御部１により算出された各剛性指令値Ｕ_１，Ｕ_２，Ｕ_３から、各一対のアクチュエータのそれぞれアクチュエータｅ_１，ｆ_１，ｅ_２，ｆ_２，ｅ_３，ｆ_３の収縮力指令値ｕ_ｅｎ，ｕ_ｆｎを生成する収縮力指令値生成部１４を備えている。そして、各剛性指令値Ｕ_ｎ（ｎ＝１，２，３）となるように、各アクチュエータの収縮力指令値ｕ_ｅｎ，ｕ_ｆｎ（ｎ＝１，２，３）を設定することで、短半径αが値α^＊となり、長半径βが値β^＊となり、傾斜角度φが値φ^＊となるスティフネス楕円が得られる。

ここで、制御部１は、図６に示すように、演算部２と、減算部３と、記憶部４と、を有して構成されている。記憶部４は、例えばＲＯＭやＥＥＰＲＯＭ等の不揮発性メモリで構成され、記憶部４には、式（１５）に示す第１の演算式、式（１６）に示す第２の演算式及び式（１７）に示す第３の演算式が予め記憶されている。また、記憶部４には、演算部２としてＣＰＵを動作させる制御プログラムが記憶されている。なお、第１の演算式、第２の演算式及び第３の演算式は、制御プログラムに組み込まれていてもよいし、制御プログラムにより動作するＣＰＵが、各演算式を参照するようにしてもよい。

第１、第２及び第３の演算式のそれぞれは、スティフネス楕円の長軸及び短軸のうちの一方の軸（例えば短軸）が、ｘ−ｙ軸直交座標系における基準軸としてのｘ軸に対して平行となる条件で導かれた、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３をそれぞれ算出する演算式である。

本実施の形態において、式（１５）〜式（１７）に示す各演算式における第１の揺動角度θ_１、第２の揺動角度θ_２、スティフネス楕円の短半径である剛性係数α、及びスティフネス楕円の長半径である剛性係数βは変数である。また、第１のリンク１０１の長さｌ_１、第２のリンク１０２の長さｌ_２は、一定の定数である。また、弾性力定数ｋ、モーメントアームの長さｒも一定の定数である。なお、目標剛性係数α，βは、定数として設定されていてもよい。この場合には、端末１３は、目標傾斜角度φのみ出力することとなる。

減算部３は、第１の揺動角度検知センサ１１から第１の揺動角度の値θ_１ ^＊を示す信号と、端末１３から目標傾斜角度の値φ^＊を示す信号とを取得し、第１の揺動角度の値θ_１ ^＊から目標傾斜角度の値φ^＊を減算する。そして、減算部３は、減算結果を新たな第１の揺動角度の値θ_１ ^＊’（＝θ_１ ^＊−φ^＊）として、新たな第１の揺動角度の値θ_１ ^＊’を示す信号を出力する。

このように、第１の揺動角度検知センサ１１から第１の揺動角度の値θ_１ ^＊から目標傾斜角度の値φ^＊を減算する補正を行うことで、スティフネス楕円の基準となる直交座標系を角度φ^＊回転させたこととなる。つまり、第１の演算式、第２の演算式及び第３の演算式は、図４に示すｘｏ−ｙｏ軸直交座標系を基準としたものであり、減算部３がθ_１ ^＊−φ^＊の減算演算を行うことで、元のｘ−ｙ軸直交座標系に変換していることとなる。なお、第２のリンク１０２の第２の揺動角度の値θ_２ ^＊は、第１のリンク１０１を基準としているので、座標系が回転しても値は変わらないので、補正の必要はない。

演算部２は、減算部３から補正後の第１の揺動角度の値θ_１ ^＊’を示す信号を取得し、第２の揺動角度検知センサ１２から第２の揺動角度の値θ_２ ^＊を示す信号を取得し、端末１３から目標剛性係数及び目標傾斜角度の値α^＊，β^＊，φ^＊を示す信号を取得する。

そして、演算部２は、記憶部４に記憶された式（１５）〜式（１７）に示した各演算式に、これら取得した各値を代入（θ_１＝θ_１ ^＊’、θ_２＝θ_２ ^＊、α＝α^＊、β＝β^＊）して、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を演算する。

この演算部２は、減算部３から第１の揺動角度の値θ_１ ^＊’（＝θ_１ ^＊−φ^＊）を示す信号を取得しているので、各演算式を用いて目標傾斜角度の値φ^＊だけ軸を傾けたスティフネス楕円となる各剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めていることとなる。つまり、演算部２は、目標傾斜角度の値φ^＊が０の場合（楕円の短軸がｘ軸に平行な場合）の演算と同等の計算量で、楕円の短軸をｘ軸から目標傾斜角度φ傾斜させた場合の剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を演算する。

このように、演算部２は、２つのリンク１０１，１０２の長さｌ_１，ｌ_２が異なる場合であっても、収束計算によらず代数的に求められた演算式（１５）〜（１７）を用いて各剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めることができる。したがって、計算負荷が軽減し、迅速にスティフネス楕円を求めることができる。

本実施の形態では、収縮力指令値生成部１４は、導出された剛性指令値Ｕ_１，Ｕ_２，Ｕ_３から、式（５）を満たす以下の式（１９）から各収縮力指令値ｕ_ｅｎ，ｕ_ｆｎを求める。

ここで、パラメータｐ_ｎは拮抗対に対する収縮力を配分する０以上１以下の実数である。演算部２で演算した剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を満たすように、各収縮力指令値ｕ_ｅｎ，ｕ_ｆｎを設定すれば、所望のスティフネス楕円を得ることができる。したがって、ｐ_ｎの値は、０以上１以下の範囲内で任意に設定することができるので、各リンク１０１，１０２を揺動させるトルク指令値に応じて設定することができる。

また、ロボット装置１００は、画像を表示する表示部１５を備えている。演算部２は、端末１３により取得した目標傾斜角度の値φ^＊に対して、各演算式に代入する第１の揺動角度の値θ_１ ^＊’（＝θ_１ ^＊−φ^＊）、つまり、第１の揺動角度の値θ_１ ^＊と、第２の揺動角度の値θ_２ ^＊とを変化させる。このとき、演算部２は、各演算式から算出した各剛性指令値Ｕ_１，Ｕ_２，Ｕ_３が正の値となる場合の領域を示す画像を表示部１５に表示させる。これにより、オペレータは、目標傾斜角度の値φ^＊に対して実現可能な各リンク１０１，１０２の姿勢を目視により把握することができる。

（３）シミュレーション
まず、本発明の実施の形態による導出方法により、任意のスティフネス楕円が実現できることを示す。リンク姿勢、即ちリンク１０１，１０２の揺動角度の値をθ_１ ^＊＝３５［ｄｅｇ］，θ_２ ^＊＝８０［ｄｅｇ］とし、リンク１０１，１０２の長さをｌ_１＝０．５［ｍ］，ｌ_２＝０．７［ｍ］とする。

そして、作業平面内での目標剛性係数の値を、α^＊＝３０，β^＊＝１００と設定し、目標傾斜角度の値φ^＊を３０［ｄｅｇ］毎に変化させて、式（１５）〜（１７）及び式（１８）に代入し、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求める。これを、式（１１）に代入し、作業平面でのスティフネス楕円をプロットする。その結果を図７（ａ）に示す。また、別の一例として、目標剛性係数の値をα^＊＝１０，β^＊＝１００とするときのスティフネス楕円を図７（ｂ）に示す。

図７（ａ）及び図７（ｂ）により、スティフネス楕円の軸長はα^＊，β^＊によって変化し、傾きは３０［ｄｅｇ］ごとに変化していることがわかる。これより、２つのリンクの長さが異なっていても、スティフネス楕円を任意の長短半径（剛性係数）と傾きに設定する剛性指令値Ｕ_１，Ｕ_２，Ｕ_３が導出されていることがわかる。

図７（ａ）及び図７（ｂ）では、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３の符号に制約を設けていない。しかし、ＭｃＫｉｂｂｅｎ型の人工筋肉アクチュエータなどでは、負の弾性力を実現することは難しい。これより、スティフネス楕円の設定によっては、それを実現不可能なマニピュレータの姿勢角が存在する。実現可能な姿勢角を調べるには、あらゆる揺動角度の値θ_１ ^＊，θ_２ ^＊を式（１５）〜（１７）及び式（１８）に代入し、剛性指令値Ｕ_１，Ｕ_２，Ｕ_３が全て正となる揺動角度の値θ_１ ^＊，θ_２ ^＊を抽出すればよい。

図８に、スティフネス楕円の設定パラメータ｛α，β，φ｝に対し、揺動角度の値θ_１ ^＊，θ_２ ^＊をそれぞれ０度から５度おきに１８０度まで変化させ、式（１５）〜式（１７）及び式（１８）に代入したときの、実現可能な揺動角度の範囲を示す。図８（ａ）は、スティフネス楕円の設定パラメータ｛α，β，φ｝を｛０．６２，２，−２０｝とした場合の図である。図８（ｂ）は、スティフネス楕円の設定パラメータ｛α，β，φ｝を｛０．２，２，４０｝とした場合の図である。図８（ｃ）は、スティフネス楕円の設定パラメータ｛α，β，φ｝を｛２，２，０｝とした場合の図である。

図８中、各リンク１０１，１０２の実現可能な揺動角度の範囲をｏ印で示す。第２のリンク１０２の先端１０２ｂ（手先）が図中ｏ印の範囲にあれば、設定したスティフネス楕円が人工筋肉アクチュエータの弾性力のみで実現可能であることを示している。図８（ａ）及び図８（ｂ）より、スティフネス楕円を細長く設定し、特定の方向に手先剛性を持たせると、それを実現可能な範囲は減少することがわかる。

また、あらゆる方向の外乱に対してコンプライアンス制御行うには、図８（ｃ）のようにスティフネス楕円を正円とすればよいが、筋の弾性の設定のみではそれを実現可能な姿勢角の範囲は限られることがわかる。

次に、式（２）及び式（３）の運動方程式を用いてシミュレーションを行い、設定したスティフネス楕円の動的な実現性を検証する。図９に示すように、マニピュレータの姿勢をθ_１＝４５［ｄｅｇ］，θ_２＝９０［ｄｅｇ］とし、矢印の方向から第２のリンク１０２の先端１０２ｂ（つまり手先）に外力を与える。このとき、スティフネス楕円の軸（図９では長軸）が外乱の方向と同一になるようにスティフネス楕円を設定すれば、手先は外乱と同じ方向に柔らかさを持つコンプライアンス制御が実現する。

そこで、図９のようなスティフネス楕円を設定する。設定パラメータ｛α，β，φ｝は、｛７５，２５０，−４５｝とする。式（１５）〜式（１７）及び式（１８）により剛性指令値Ｕ_１，Ｕ_２，Ｕ_３を求めると、Ｕ_１＝３．０，Ｕ_２＝０，Ｕ_３＝１９．６となる。これを、ｐ_ｎ＝０．５（ｎ＝１，２，３）として、式（１９）により配分し、各アクチュエータｅ_１，ｆ_１，ｅ_２，ｆ_２，ｅ_３，ｆ_３に収縮力を与える。

図１０に第２のリンク１０２の先端１０２ｂに外力を与えるときの先端１０２ｂの軌跡を破線で示す。２つのリンク１０１，１０２の長さが異なっていても、先端１０２ｂ（手先）は外力と同じ方向に柔らかさを有し、外乱に対して柔軟な接触ができるコンプライアンス制御が実現していることがわかる。

以上、上記実施の形態に基づいて本発明を説明したが、本発明はこれに限定されるものではない。上記実施の形態では、作業ロボット等のマニピュレータの場合について説明したが、脚式移動ロボットの場合であってもよい。

また、上記実施の形態では、揺動角度検知センサ１１，１２により各リンク１０１，１０２の揺動角度θ_１，θ_２を検知し、この検知結果を第１，第２の揺動角度の値として剛性指令値の演算に用いる場合について説明したが、これに限定するものではない。これら第１，第２の揺動角度の値は、検知結果の代わりに目標揺動角度の値であってもよく、この場合、揺動角度検知センサ１１，１２は省略可能である。

このフィードフォワード制御の場合、電気的なフィードバック制御を行わずに、第２のリンク１０２の先端１０２ｂのスティフネス楕円を維持することが可能となり、省エネルギ化を実現するという効果が得られる。

また、上記実施の形態では、スティフネス楕円の長軸及び短軸のうちの一方の軸が、短軸である場合について説明したが、長軸である場合であってもよい。そして、上記実施の形態では、ｘ−ｙ軸直交座標系のｘ軸が基準軸である場合について説明したが、ｙ軸が基準軸である場合であってもよい。

２…演算部、３…減算部、４…記憶部、１１…第１の揺動角度検知センサ、１２…第２の揺動角度検知センサ、１３…端末、１４…収縮力指令値生成部、１５…表示部、１００…ロボット装置、１０１…第１のリンク、１０２…第２のリンク、１０３…プーリ（固定部材）、ｅ_１，ｆ_１…第１のアクチュエータ、ｅ_２，ｆ_２…第２のアクチュエータ、ｅ_３，ｆ_３…第３のアクチュエータ

Claims (4)

1. 長手部材からなり、基端が固定部材に揺動可能に支持された第１のリンクと、長手部材からなり、前記第１のリンクが揺動する平面内で揺動可能に基端が前記第１のリンクの先端に支持された第２のリンクと、前記固定部材と前記第１のリンクとに接続され、収縮力の差により前記第１のリンクを揺動させる一対の第１のアクチュエータと、前記第１のリンクと前記第２のリンクとに接続され、収縮力の差により前記第２のリンクを揺動させる一対の第２のアクチュエータと、前記固定部材と前記第２のリンクとに接続され、収縮力の差により前記第１のリンク及び前記第２のリンクを揺動させる一対の第３のアクチュエータと、を備えたロボット装置において、
   前記平面内に直交座標系を定義し、前記第２のリンクの先端におけるスティフネス特性が前記直交座標系に前記第２のリンクの先端を中心とする楕円で表されるときの前記楕円の長軸及び短軸のうちの一方の軸が、前記直交座標系における基準軸に対して平行となる条件で導かれた演算式であって、前記基準軸を基準とする前記第１のリンクの第１の揺動角度、及び前記第１のリンクの長手方向に延びる軸線を基準とする前記第２のリンクの第２の揺動角度を変数とし、前記第１のリンクの長さ及び前記第２のリンクの長さを定数として、前記一対の第１のアクチュエータの各収縮力指令値の和を示す第１の剛性指令値を求める第１の演算式、前記一対の第２のアクチュエータの各収縮力指令値の和を示す第２の剛性指令値を求める第２の演算式、及び前記一対の第３のアクチュエータの各収縮力指令値の和を示す第３の剛性指令値を求める第３の演算式を予め記憶する記憶部と、
   前記第１のリンクの第１の揺動角度の値を示す信号と、前記基準軸を基準とする前記一方の軸の目標傾斜角度の値を示す信号とを取得し、第１の揺動角度の値から目標傾斜角度の値を減算した結果を新たな第１の揺動角度の値として、新たな第１の揺動角度の値を示す信号を出力する減算部と、
   前記減算部が出力した第１の揺動角度の値を示す信号と、前記第２のリンクの第２の揺動角度の値を示す信号とを取得し、前記記憶部に記憶された前記第１の演算式、前記第２の演算式及び前記第３の演算式から、前記第１の剛性指令値、前記第２の剛性指令値及び前記第３の剛性指令値を算出する演算部と、
   前記演算部により算出された前記各剛性指令値から、前記一対の第１のアクチュエータ、前記一対の第２のアクチュエータ及び前記一対の第３のアクチュエータのそれぞれのアクチュエータの収縮力指令値を生成する収縮力指令値生成部と、を備えたことを特徴とするロボット装置。
2. 前記基準軸を基準とする前記第１のリンクの第１の揺動角度を検出する第１の揺動角度検知センサと、
   前記第１のリンクの長手方向に延びる軸線を基準とする前記第２のリンクの第２の揺動角度を検知する第２の揺動角度検知センサと、を備え、
   前記減算部は、前記第１の揺動角度検知センサから前記第１のリンクの第１の揺動角度の値を示す信号を取得し、
   前記演算部は、前記第２の揺動角度検知センサから前記第２のリンクの第２の揺動角度の値を示す信号を取得することを特徴とする請求項１に記載のロボット装置。
3. オペレータの操作により、前記一方の軸の目標傾斜角度の値を示す信号を出力する端末を備えたことを特徴とする請求項１又は２に記載のロボット装置。
4. 画像を表示する表示部を備え、
   前記演算部は、前記端末により取得した前記一方の軸の目標傾斜角度の値に対して、前記第１のリンクの第１の揺動角度の値と前記第２のリンクの第２の揺動角度の値とを変化させたときに、前記各演算式から算出した前記各剛性指令値が正の値となる場合の領域を示す画像を前記表示部に表示させることを特徴とする請求項３に記載のロボット装置。

Images