JP2014004653A - ロボット及びロボット制御方法 - Google Patents

Abstract

【課題】関節の柔軟性を損なわずに、リンクの高精度な位置決めを可能とする。
【解決手段】張力最低値計算部１８１が、各紐状部材の固有振動数をｆ_ｍｉｎ、位置制御部の制御帯域をｆ_ｃ、位置制御部のナイキスト周波数をｆ_ｎとしたとき、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす固有振動数ｆ_ｍｉｎを求める。そして、張力最低値計算部１８１が、第１及び第２紐状部材の長さをＬ、第１及び第２紐状部材の線密度をμ、第１及び第２紐状部材の張力最低値をＴ_ｍｉｎとしたとき、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から張力最低値Ｔ_ｍｉｎを算出する。張力補償量演算部１７３が、第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が張力最低値Ｔ_ｍｉｎとなるように、張力最低値Ｔ_ｍｉｎ、関節角度θ及びトルク指令値τ_ｒｅｆを用いて、第１及び第２アクチュエータの収縮力指令値を算出する。
【選択図】図６

Description

本発明は、駆動力の伝達機構にワイヤやベルト等の紐状部材を用いるロボット及びロボット制御方法に関する。

近年、人間と共同作業を行うことが可能なロボットの実現が望まれている。このようなロボットに用いられるマニピュレータには、従来のマニピュレータと同様に高精度な位置決めが可能であるだけでなく、人間と衝突した際に柔軟であることが求められる。

柔軟なマニピュレータを実現する方法として、可変剛性機構を備えるアクチュエータを用いて関節の剛性を制御する方法が知られている。また、ワイヤやベルトを用いて関節に駆動力を伝達することでアクチュエータを基台に設置し、可動部の質量を低減する方法が知られている。したがって、可変剛性アクチュエータとワイヤ駆動機構を併用すれば、非常に柔軟性の高いマニュピレータを実現できる。

従来、ワイヤを用いて駆動力を伝達する経路に、張力に比例して弾性係数が変化する非線形ばね要素を挿入することで、可変剛性機構を実現するものが提案されている（非特許文献１参照）。そして、遺伝的アルゴリズムを用いた学習によって、手先の軌道追従制御と関節剛性の制御を同時に実現するためのワイヤ張力目標値を演算している。

また、可変剛性機構として伸縮量に比例して弾性係数が非線形に変化するワイヤを用いているロボットが提案されている（特許文献１参照）。そして、フィードバック制御器を用いて関節角度を目標軌道へ追従させている。しかし、フィードバック制御では、ワイヤの伸縮方向と垂直な振動（以下、「弦振動」と呼ぶ）か、ワイヤの弾性による関節周りの振動の固有振動数が制御帯域以下になると、制御器がワイヤの振動を励起し、制御系が不安定となってしまう。そのため、特許文献１では、制御系を安定化させるために、ワイヤの張力が低下し、ワイヤの固有振動数が低下した場合は、フィードバック制御器の制御帯域を低下させている。また、特許文献１では、剛性の制御を行っていないため、関節剛性が位置フィードバックのための制御入力によって決まってしまう。

特開２００６−３５３２５号公報

朴、他１名、「腱駆動ロボットの作業に適した関節剛性のＧＡによる学習」、日本ロボット学会誌、２００６年５月１５日、Ｖｏｌ．２４、Ｎｏ．４、ｐ．４８２−４８８

しかしながら、非特許文献１では、所望の関節剛性を実現するためのワイヤの張力を学習によって演算している。そのため、関節剛性の目標値を変更するためには再度繰り返し計算を行う必要があり、目標値を容易に変更することができない。また、フィードフォワード制御器のみを用いて目標軌道への追従制御を行っているため、モデル誤差やリンクへの外乱により位置決め精度が大きく低下してしまう。

一方、特許文献１では、ワイヤの張力の低下により制御帯域が低下すると、位置決め精度が劣化してしまう。従って、これらの技術では、高精度な位置決め制御を実現することは難しい。さらに、特許文献１は関節剛性を制御していないため、位置決め制御を行う際に大きな張力指令値が出力されるとワイヤの弾性係数が増加し、関節の柔軟性が失われてしまう。

そこで、本発明は、関節の柔軟性が損なわれずにリンクの高精度な位置決めが可能となるロボット及びロボット制御方法を提供することを目的とするものである。

本発明のロボットは、関節で旋回するリンクと、前記リンクに接続され、互いに拮抗して配置された第１紐状部材及び第２紐状部材と、前記第１紐状部材を進退させる駆動力を発生する第１アクチュエータ、及び前記第２紐状部材を進退させる駆動力を発生する第２アクチュエータを有し、駆動力の差で前記関節まわりにトルクを発生し、駆動力の和で前記関節に剛性を与える駆動部と、前記第１及び第２アクチュエータに発生させる駆動力を制御する制御装置と、を備え、前記制御装置は、外部から入力を受けた目標関節角度に対する関節角度の角度偏差に基づいて、前記関節まわりのトルク指令値を算出する位置制御部と、前記各紐状部材の固有振動数をｆ_ｍｉｎ、前記位置制御部の制御帯域をｆ_ｃ、前記位置制御部のナイキスト周波数をｆ_ｎとしたとき、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす前記固有振動数を求め、前記第１及び第２紐状部材の長さをＬ、前記第１及び第２紐状部材の線密度をμ、前記第１及び第２紐状部材の張力最低値をＴ_ｍｉｎとしたとき、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から前記張力最低値を算出する張力最低値計算部と、前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて、前記第１及び第２アクチュエータの駆動力指令値を算出する駆動力指令値計算部と、前記駆動力指令値計算部により求めた駆動力指令値に対応する駆動力を、前記第１及び第２アクチュエータに発生させる駆動制御部と、を有することを特徴とする。

また、本発明のロボット制御方法は、関節で旋回するリンクと、前記リンクに接続され、互いに拮抗して配置された第１紐状部材及び第２紐状部材と、前記第１紐状部材を進退させる駆動力を発生する第１アクチュエータ、及び前記第２紐状部材を進退させる駆動力を発生する第２アクチュエータを有し、駆動力の差で前記関節まわりにトルクを発生し、駆動力の和で前記関節に剛性を与える駆動部と、を備えたロボット本体を、制御装置により制御するロボット制御方法において、前記制御装置の位置制御部が、外部から入力を受けた目標関節角度に対する関節角度の角度偏差に基づいて、前記リンクの前記関節まわりのトルク指令値を算出する位置制御工程と、前記制御装置の張力最低値計算部が、前記各紐状部材の固有振動数をｆ_ｍｉｎ、前記位置制御部の制御帯域をｆ_ｃ、前記位置制御部のナイキスト周波数をｆ_ｎとしたとき、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす前記固有振動数を求め、前記第１及び第２紐状部材の長さをＬ、前記第１及び第２紐状部材の線密度をμ、前記第１及び第２紐状部材の張力最低値をＴ_ｍｉｎとしたとき、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から前記張力最低値を算出する張力最低値計算工程と、前記制御装置の駆動力指令値計算部が、前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて、前記第１及び第２アクチュエータの駆動力指令値を算出する駆動力指令値計算工程と、前記制御装置の駆動制御部が、前記駆動力指令値計算工程にて求められた駆動力指令値に対応する駆動力を、前記第１及び第２アクチュエータに発生させる駆動制御工程と、を備えたことを特徴とする。

本発明によれば、各紐状部材の固有振動数が位置制御部の制御帯域を超えるように各紐状部材の張力を制御するので、広い制御帯域の位置制御部を用いた場合においても制御系が安定し、リンクの高精度な位置決め制御を行うことが可能となる。また、位置制御部の制御帯域に基づいて制御系の安定化に必要な最低限の張力に制御するため、関節の剛性が過度に増加することがなく、関節の柔軟性が損なわれることがない。

人工筋肉アクチュエータの粘弾性モデルの模式図である。 本発明の第１実施形態に係るロボットの概略構成を示す説明図である。 本発明の第１実施形態に係るロボット本体を示す模式図である。 本発明の第１実施形態に係るロボットの制御装置を示すブロック線図である。 人工筋肉アクチュエータの収縮力とワイヤの張力との関係を示す図である。 本発明の第１実施形態の張力制御部を示すブロック線図である。 本発明の第１実施形態に係るロボットの制御装置によるロボット制御方法を示すフローチャートである。 本発明の第１実施形態の張力補償量演算部における演算処理を示すフローチャートである。 本発明の第１実施形態におけるシミュレーション結果を示す図である。 本発明の第１実施形態におけるシミュレーション結果を示す図である。 本発明の第２実施形態に係るロボットの制御装置を示すブロック線図である。 本発明の第２実施形態の収縮力切り替え部における演算処理を示すフローチャートである。 本発明の第２実施形態におけるシミュレーション結果を示す図である。 本発明の第２実施形態におけるシミュレーション結果を示す図である。 本発明の第２実施形態におけるシミュレーション結果を示す図である。 ３対６筋構造を備える２リンクアームモデルを示す図である。 本発明の第３実施形態に係るロボットの概略構成を示す説明図である。 本発明の第３実施形態に係るロボットの制御装置を示すブロック線図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。 本発明の第３実施形態におけるシミュレーション結果を示す図である。

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

［第１実施形態］
本第１実施形態では、可変剛性機構に人工筋肉アクチュエータを、駆動力の伝達機構にワイヤを用いる１リンクマニピュレータの高精度な位置決めを行う。位置決め精度を向上させるためには広制御帯域なフィードバック制御部を用いる必要があるため、位置制御に加えて、ワイヤの張力を一定値以上に保つための張力制御を行う。

（１．１モデル導出）
本第１実施形態では、可変剛性機構として人工筋肉アクチュエータを用いるマニピュレータの制御を行う。人工筋肉アクチュエータとは、筋の粘弾性と類似する特性を有するアクチュエータであり、図１に示すように、力発生要素２０、弾性要素２１、粘性要素２２を用いてモデル化される。力発生要素２０の収縮力をｕ、収縮方向を正とする筋の収縮量をｘ、収縮速度を、

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

と表わされる。式（１）に示すように、人工筋肉アクチュエータは、弾性力と粘性力とが力発生要素の収縮力ｕに比例する非線形性を有する。

図２は、本発明の第１実施形態に係るロボットの概略構成を示す説明図である。図２に示すロボット１００は、１リンクマニピュレータであるロボット本体１２０と、ロボット本体１２０を制御する制御装置１５０とを備えている。

ロボット本体１２０は、基部であるベース部材１０３及びベース部材１０３から延びるリンク１０４と、リンク１０４の先端を関節１０７として、関節１０７に回動可能に連結されたプーリ１０５とを備えている。また、ロボット本体１２０は、プーリ１０５に固定され、関節１０７で旋回可能にリンク１０４にプーリ１０５を介して連結されたリンク１０６を備えている。

また、ロボット本体１２０は、一端がベース部材１０３に固定された第１アクチュエータであるアクチュエータ１０１を備えている。また、ロボット本体１２０は、一端がアクチュエータ１０１の他端に接続され、他端がプーリ１０５を介してリンク１０６に接続された第１紐状部材であるワイヤ１１１を備えている。

また、ロボット本体１２０は、一端がベース部材１０３に固定された第２アクチュエータであるアクチュエータ１０２を備えている。また、ロボット本体１２０は、一端がアクチュエータ１０２の他端に接続され、他端がプーリ１０５を介してリンク１０６に接続された第２紐状部材であるワイヤ１１２を備えている。なお、ワイヤ１１１とワイヤ１１２とは、別体に構成したが、１つのワイヤで一体に構成してもよい。

これらアクチュエータ１０１，１０２により、駆動部１０８が構成されている。アクチュエータ１０１，１０２は、図１に示す空気圧式の人工筋肉アクチュエータである。人工筋肉アクチュエータは収縮方向にのみ力を発生するため、関節１０７を任意の角度に位置決めするために、アクチュエータ１０１及びワイヤ１１１と、アクチュエータ１０２及びワイヤ１１２とは、図２に示すように拮抗して配置されている。つまり、各アクチュエータ１０１，１０２は、人工筋肉からなる一関節筋アクチュエータであり、収縮することにより、駆動力である収縮力が発生する。制御装置１５０は、これらアクチュエータ１０１，１０２に発生する駆動力である収縮力を制御するものである。

第１人工筋肉アクチュエータであるアクチュエータ１０１は、収縮によりワイヤ１１１を進退させ、リンク１０６を関節１０７まわりに第１旋回方向Ｒ_１（図２中、反時計回り方向）に旋回させる。第２人工筋肉アクチュエータであるアクチュエータ１０２は、収縮によりワイヤ１１２を進退させ、リンク１０６を関節１０７まわりに第１旋回方向Ｒ_１に対して反対の第２旋回方向Ｒ_２（図２中、時計回り方向）に旋回させる。制御装置１５０は、各アクチュエータ１０１，１０２の駆動力指令値である収縮力指令値ｕ_ｆ，ｕ_ｅを演算により求めて、各収縮力指令値ｕ_ｆ，ｕ_ｅに応じて各アクチュエータ１０１，１０２を収縮させる。

アクチュエータ１０１，１０２（弾性要素）の収縮量ｘは、モーメントアーム径ｒと関節角度θを用いて、

と表わせる。このとき、ワイヤ１１１，１１２の張力Ｔ_ｆ，Ｔ_ｅは、関節角速度ω、アクチュエータ１０１，１０２の収縮力指令値ｕ_ｆ，ｕ_ｅを用いて、

と表せる。なお、ワイヤ１１１，１１２は収縮方向にのみ力を伝達できるため、張力Ｔ_ｆ，Ｔ_ｅは正数となる。リンク１０６の慣性モーメントをＪ、関節角加速度をαとし、式（３）及び式（４）を用いてリンク１０６の運動方程式を導出すると、

となる。式（５）より、右辺第１項における収縮力の差が関節１０７まわりの回転トルクを与え、右辺第２項と第三項における収縮力の和が関節１０７まわりの剛性および粘性を与えることがわかる。

つまり、本第１実施形態では、アクチュエータ１０１，１０２からなる駆動部１０８は、アクチュエータ１０１，１０２の駆動力の差で関節１０７まわりにトルクを発生し、アクチュエータ１０１，１０２の駆動力の和で関節１０７に剛性を与える。

次にワイヤ１１１，１１２のモデルを、次の仮定の下で導出する。

仮定（１） ワイヤ１１１，１１２を、１つの質点と、質点に接続した２つのバネで近似する。質点は、ワイヤ１１１，１１２の伸縮方向と伸縮方向に垂直な方向の並進２自由度を備える。

仮定（２） 弦振動の減衰を、ワイヤ１１１，１１２の質点に働くダンパで近似する。

仮定（３） 張力によるワイヤ長の変化を考慮しない。

仮定（４） ワイヤ１１１，１１２の伸縮方向の固有振動数は弦振動の固有振動数に比べて十分に大きい。

仮定（５） ワイヤ１１１，１１２とプーリ１０５との間に働く摩擦を考慮しない。

仮定（１）〜仮定（５）を用いて導出したワイヤ駆動１リンクマニピュレータのモデルを図３に示す。図３において、ｋ_ｗ，ｃ_ｗ，ｍ_ｗはそれぞれ、ワイヤ１１１，１１２の伸縮方向の剛性、弦振動の減衰率、ワイヤ１１１，１１２の質量を表す。仮定（３）及び仮定（４）より、ワイヤ１１１，１１２の剛性ｋ_ｗは筋の弾性に比べて十分大きな値を用いる。

（１．２制御系設計）
図４は、本発明の第１実施形態に係るロボットの制御装置を示すブロック線図である。制御装置１５０は、減算部１５１と、位置制御部１５２と、張力制御部１５３と、駆動制御部１５４と、を有している。

減算部１５１は、上位コントローラ等の外部機器から入力を受けた目標関節角度θ_ｒｅｆから関節角度θを減算することで、角度偏差Δθを算出する。位置制御部１５２は、角度偏差Δθを低減するように、関節１０７まわりのトルク指令値τ_ｒｅｆを算出する。張力制御部１５３は、ワイヤ１１１，１１２の張力が予め設計された張力最低値Ｔ_ｍｉｎ以上になるという条件で、トルク指令値τ_ｒｅｆから収縮力指令値ｕ_ｆ，ｕ_ｅを演算し、駆動制御部１５４に出力する。

駆動制御部１５４は、各収縮力指令値ｕ_ｆ，ｕ_ｅに応じた収縮力で各アクチュエータ１０１，１０２を収縮させる。また、制御対象は、式（５）でモデル化される１リンクアームにおけるロボット本体１２０である。

本節では、まず、張力最低値Ｔ_ｍｉｎの設計方法を示し、次に、収縮力指令値ｕ_ｆ，ｕ_ｅの導出方法を示す。張力最低値Ｔ_ｍｉｎは、制御系を安定化するために必要な弦振動の固有振動数ｆ_ｍｉｎにより導出される。そこで、まず、位置制御部１５２の制御帯域ｆ_ｃに基づいて固有振動数ｆ_ｍｉｎを導出する方法を示す。

（１．２．１制御帯域の導出方法）
本第１実施形態では制御帯域ｆ_ｃを、マニピュレータであるロボット本体１２０の伝達関数Ｐ（ｓ）と位置制御部１５２の伝達関数Ｋ（ｓ）を乗算した一巡伝達関数Ｐ（ｓ）Ｋ（ｓ）を用いて導出する。ここで、伝達関数Ｐ（ｓ）はトルク指令値τ_ｒｅｆから関節角度θまでの伝達関数であり、トルク指令値τ_ｒｅｆは、

と表される。また、収縮力指令値ｕ_ｆ，ｕ_ｅの和が関節１０７の剛性となるので、これをＵと定義すると、

と表される。式（５）より、伝達関数Ｐ（ｓ）は、ラプラス演算子ｓを用いて、

となる。

本第１実施形態では、位置制御部１５２としてＰＩＤ制御器を用いている。このため、位置制御部１５２の伝達関数Ｋ（ｓ）は、微分ゲインＫ_ｄ、比例ゲインＫ_ｐ、積分ゲインＫ_ｉ、時定数Ｔ_ｄを用いて、

と表せる。式（８）及び式（９）より、位置制御系の一巡伝達関数Ｐ（ｓ）Ｋ（ｓ）は、

となる。本第１実施形態では、筋の弾性によって生じるリンク１０６の固有振動数に比べて制御帯域が高くなるように位置制御部１５２を設計する。そこで、式（１０）において、ラプラス演算子ｓに関する一次の項のみを考慮すると、

となる。式（１１）より、一巡伝達関数Ｐ’（ｓ）Ｋ’（ｓ）の零クロス周波数、すなわち制御帯域ｆ_ｃは、

となる。

（１．２．２ 最低張力値の導出方法）
本項では、位置制御部１５２の制御帯域ｆ_ｃに基づいて、位置制御部１５２を安定化するためのワイヤ１１１，１１２の弦振動の固有振動数ｆ_ｍｉｎを導出する。次に、ワイヤ１１１，１１２の弦振動の固有振動数が振動数ｆ_ｍｉｎとなるときのワイヤ１１１，１１２の張力を張力最低値Ｔ_ｍｉｎとして導出する。

固有振動数ｆ_ｍｉｎが制御帯域ｆ_ｃよりも低い場合、位置制御部１５２が弦振動を励起してしまい、制御系が不安定となってしまう。一方、制御帯域ｆ_ｃを位置制御部１５２のナイキスト周波数ｆ_ｎ以上とすることはできない。これらのことから、固有振動数ｆ_ｍｉｎは、

を満たす必要がある。そのため、式（１３）より固有振動数_ｆｍｉｎは、制御帯域ｆ_ｃと、以下の条件、

を満たす増幅率ａを用いて、

と表せる。制御系が安定となる最小の固有振動数は弦振動の減衰率ｃ_ｗに依存するが、減衰率ｃ_ｗから固有振動数ｆ_ｍｉｎを解析的に求めることは難しい。そこで、本第１実施形態では増幅率ａを１から徐々に増加させながら制御系の安定性を調べ、増幅率ａを導出する。

次に、張力最低値Ｔ_ｍｉｎを演算する。弦振動の１次の固有振動数ｆは弦の長さＬ、線密度μ、張力Ｔを用いて、

と表される。そこで、式（１６）を変形し、

とする。これを用いて固有振動数ｆ_ｍｉｎからワイヤ１１１，１１２の張力最低値Ｔ_ｍｉｎを演算する。

（１．２．３収縮力の導出方法）
本第１実施形態では、式（３）及び式（４）に示すワイヤ張力Ｔ_ｆ，Ｔ_ｅの一方を張力最低値Ｔ_ｍｉｎと一致させ、かつ、式（６）を満たすように収縮力指令値ｕ_ｆ，ｕ_ｅを演算する。なお、本第１実施形態では、簡単のため、人工筋肉アクチュエータの粘性要素を考慮せず、力発生要素と弾性要素のみを考慮して収縮力指令値ｕ_ｆ，ｕ_ｅを導出する。

まず、ワイヤの張力Ｔ_ｆ，Ｔ_ｅの大小を判別する。式（３）及び式（４）より張力Ｔ_ｆ，Ｔ_ｅは、

と表せる。ここで、１．１節で述べたように、人工筋肉アクチュエータ１０１，１０２は収縮力のみを発生し、ワイヤ１１１，１１２は収縮方向の力のみを伝達できる。式（１８）及び式（１９）より、張力Ｔ_ｆ，Ｔ_ｅと収縮力指令値ｕ_ｆ，ｕ_ｅが正数となるためには、関節角度θが

を満たす必要がある。そこで、本第１実施形態では、関節角度θが式（２０）を満たすと仮定して収縮力指令値ｕ_ｆ，ｕ_ｅを導出する。収縮力指令値ｕ_ｆ，ｕ_ｅが式（６）を満たす場合、張力Ｔ_ｅは収縮力指令値ｕ_ｆを用いて、

と表わせる。張力Ｔ_ｆ，Ｔ_ｅが式（１８）と式（２１）の２つの式で表される。式（１８）と式（２１）には３つの未知数、即ち関節角度θ、トルク指令値τ_ｒｅｆ、収縮力指令値ｕ_ｆが含まれるため、このままでは収縮力指令値ｕ_ｆを一意に決めることができない。

そこで、本第１実施形態では、ワイヤ１１１，１１２のうち、張力Ｔ_ｆ，Ｔ_ｅの小さい方のワイヤの張力を張力最低値Ｔ_ｍｉｎとして、収縮力指令値ｕ_ｆを導出する。まず、トルク指令値τ_ｒｅｆを正負のどちらか一方に固定する。次に、関節角度θが正の場合、負の場合、それぞれについて、収縮力指令値ｕ_ｆと張力Ｔ_ｆ，Ｔ_ｅの関係を表すグラフを描画する。そして、グラフ用いて張力Ｔ_ｆ，Ｔ_ｅの大小を判別する。

（ｉ） トルク指令値τ_ｒｅｆが正の場合
収縮力指令値ｕ_ｆと張力Ｔ_ｆ，Ｔ_ｅの関係を、関節角度θが正の場合と負の場合において、それぞれ、図５（ａ）と図５（ｂ）に示す。図５において、実線は収縮力指令値ｕ_ｆと張力Ｔ_ｅの関係を、破線は収縮力指令値ｕ_ｆと張力Ｔ_ｆの関係をそれぞれ表す。図５（ａ）と式（１８）及び式（２１）より、関節角度θが正の場合、張力Ｔ_ｆを表す直線の傾きが張力Ｔ_ｅを表す直線の傾きに比べて小さくなることがわかる。さらに、式（２０）及び式（２１）より、張力Ｔ_ｅを表す直線とＸ軸との切片は正となることがわかる。これより、張力Ｔ_ｆと張力Ｔ_ｅが等しくなる収縮力指令値ｕ_０が存在し、ｕ_０は式（１８）と式（２１）より、

と表わせる。収縮力指令値ｕ_０が式（２２）で与えられる場合、ワイヤ張力Ｔ_０は式（１８）より、

となる。従って、張力最低値Ｔ_ｍｉｎが、

を満たす場合、図５（ａ）より、張力Ｔ_ｆが張力Ｔ_ｅより小さくなるため、

とすればよい。張力最低値Ｔ_ｍｉｎが、

を満たす場合には、張力Ｔ_ｅが張力Ｔ_ｆより小さくなるため、

とすればよい。

一方、関節角度θが負の場合、図５（ｂ）と式（１８）及び式（２１）より、張力Ｔ_ｅを表す直線の傾きは張力Ｔ_ｆを表す直線の傾きに比べて小さくなることがわかる。さらに、式（２０）及び式（２１）より張力Ｔ_ｅを表す直線とＸ軸との切片は正となることがわかる。これより、張力Ｔ_ｅは張力Ｔ_ｆより小さくなるため、張力Ｔ_ｅが式（２７）を満たすようにすればよい。

（ｉｉ） トルク指令値τ_ｒｅｆが負の場合
収縮力指令値ｕ_ｆと張力Ｔ_ｆ，Ｔ_ｅの関係を、関節角度θが正の場合と負の場合において、それぞれ、図５（ｃ）と図５（ｄ）に示す。

図５（ｃ）と式（１８）及び式（２１）より、関節角度θが正の場合には張力Ｔ_ｆを表す直線の傾きが張力Ｔ_ｅを表す直線の傾きに比べて小さくなることがわかる。また、式（２０）及び式（２１）より、張力Ｔ_ｅを表す直線とＹ軸との切片は正となることがわかる。これより、張力Ｔ_ｆは張力Ｔ_ｅより小さくなるため、張力Ｔ_ｆが式（２５）を満たすようにすればよい。

一方、図５（ｄ）と式（１８）及び式（２１）より、関節角度θが負の場合、張力Ｔ_ｅを表す直線の傾きは張力Ｔ_ｆを表す直線の傾きに比べて小さくなることがわかる。また、式（２０）及び式（２１）より、張力Ｔ_ｅを表す直線とＹ軸との切片は正となることがわかる。これより、張力Ｔ_ｆと張力Ｔ_ｅは式（２２）で表される収縮力指令値ｕ_０において等しくなり、このときの張力Ｔ_０は式（２３）で表される。従って、張力最低値Ｔ_ｍｉｎが、式（２４）を満たす場合、図５（ｄ）に示すように張力Ｔ_ｆは張力Ｔ_ｅより小さくなるため、張力Ｔ_ｆを式（２５）とすればよい。また、張力最低値Ｔ_ｍｉｎが式（２６）を満たす場合、張力Ｔ_ｅは張力Ｔ_ｆより小さくなるため、張力Ｔ_ｅを式（２７）とすればよい。

（ｉｉｉ） 収縮力指令値ｕ_ｆ，ｕ_ｅの演算方法
張力Ｔ_ｆが張力Ｔ_ｅよりも小さくなる場合は、張力Ｔ_ｆを張力最低値Ｔ_ｍｉｎと一致させればよいため、式（１８）及び式（２１）より、収縮力指令値ｕ_ｆ，ｕ_ｅは、

と求まる。一方、張力Ｔ_ｅが張力Ｔ_ｆよりも小さくなる場合は、張力Ｔ_ｅを張力最低値Ｔ_ｍｉｎと一致させればよいため、式（１８）及び式（２１）を用いて収縮力指令値ｕ_ｆ，ｕ_ｅは、

と求まる。

（１．２．４張力制御部のブロック線図）
張力制御部１５３のブロック線図を図６に示す。張力制御部１５３は、張力最低値計算部１８１と、駆動力指令値計算部（収縮力指令値計算部）としての張力補償量演算部１７３と、ローパスフィルタ１７４，１７５とを有して構成される。

張力最低値計算部１８１は、増幅部１７１，１７２からなる。増幅部１７１は、式（１４）を用いて、制御帯域ｆ_ｃよりも大きく、ナイキスト周波数ｆ_ｎよりも小さい値の固有振動数ｆ_ｍｉｎを演算する。また、増幅部１７２は、式（１６）を用いて、固有振動数ｆ_ｍｉｎから張力最低値Ｔ_ｍｉｎを演算する。

張力補償量演算部１７３は、張力最低値Ｔ_ｍｉｎ、トルク指令値τ_ｒｅｆ、及び関節角度θを入力とし、収縮力指令値ｕ_ｆ，ｕ_ｅを演算する。

以下、制御装置１５０の各部の動作について詳細に説明する。図７は、本発明の第１実施形態に係るロボットの制御装置によるロボット制御方法を示すフローチャートである。

まず、位置制御部１５２は、外部から入力を受けた目標関節角度θ_ｒｅｆに対する関節角度θの角度偏差Δθに基づいて、関節１０７まわりのトルク指令値τ_ｒｅｆを算出する（Ｓ１：位置制御工程）。

次に、張力最低値計算部１８１は、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす固有振動数ｆ_ｍｉｎを求め、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から張力最低値Ｔ_ｍｉｎを算出する（Ｓ２：張力最低値計算工程）。張力補償量演算部１７３は、ワイヤ１１１，１１２のうち張力が小さい方のワイヤの張力が張力最低値Ｔ_ｍｉｎとなるように、張力最低値Ｔ_ｍｉｎ、関節角度θ及びトルク指令値τ_ｒｅｆを用いて収縮力指令値ｕ_ｆ，ｕ_ｅを算出する（Ｓ３：収縮力指令値計算工程）。

駆動制御部１５４は、ステップＳ３（駆動力指令値計算工程としての収縮力指令値計算工程）にて求められた収縮力指令値ｕ_ｆ，ｕ_ｅに対応する収縮力を、アクチュエータ１０１，１０２に発生させる（Ｓ４：駆動制御工程）。

図８は、張力補償量演算部における演算処理を示すフローチャートである。以下、ステップＳ３（図７）における張力補償量演算部１７３の演算処理について具体的に説明する。まず、張力補償量演算部１７３は、τ_ｒｅｆ＞０であるか否かを判断する（Ｓ１１）。張力補償量演算部１７３は、τ_ｒｅｆ＞０である場合（Ｓ１１：Ｙｅｓ）、θ＞０であるか否かを判断する（Ｓ１２）。張力補償量演算部１７３は、θ＞０である場合（Ｓ１２：Ｙｅｓ）、式（２３）により張力Ｔ_０を算出する（Ｓ１３）。次に、張力補償量演算部１７３は、Ｔ_ｍｉｎ＞Ｔ_０であるか否かを判断する（Ｓ１４）。張力補償量演算部１７３は、Ｔ_ｍｉｎ＞Ｔ_０である場合（Ｓ１４：Ｙｅｓ）、式（２８）及び式（２９）により収縮力指令値ｕ_ｆ，ｕ_ｅを算出する（Ｓ１５）。

また、張力補償量演算部１７３は、θ＞０ではない場合（Ｓ１２：Ｎｏ）、或いはＴ_ｍｉｎ＞Ｔ_０ではない場合（Ｓ１４：Ｎｏ）、式（３０）及び式（３１）により収縮力指令値ｕ_ｆ，ｕ_ｅを算出する（Ｓ１６）。

また、張力補償量演算部１７３は、τ_ｒｅｆ＞０ではない場合（Ｓ１１：Ｎｏ）、θ＞０であるか否かを判断する（Ｓ１７）。張力補償量演算部１７３は、θ＞０ではない場合（Ｓ１７：Ｎｏ）、式（２３）により張力Ｔ_０を算出する（Ｓ１８）。次に、張力補償量演算部１７３は、Ｔ_ｍｉｎ＜Ｔ_０であるか否かを判断する（Ｓ１９）。

張力補償量演算部１７３は、Ｔ_ｍｉｎ＜Ｔ_０である場合（Ｓ１９：Ｙｅｓ）、或いはθ＞０である場合（Ｓ１７：Ｙｅｓ）、式（２８）及び式（２９）により収縮力指令値ｕ_ｆ，ｕ_ｅを算出する（Ｓ２０）。

また、張力補償量演算部１７３は、Ｔ_ｍｉｎ＜Ｔ_０ではない場合（Ｓ１９：Ｎｏ）、式（３０）及び式（３１）により収縮力指令値ｕ_ｆ，ｕ_ｅを算出する（Ｓ２１）。

ここで、張力Ｔ_ｆ，Ｔ_ｅが張力最低値Ｔ_ｍｉｎの近傍となる場合、式（２８）及び式（２９）に示す収縮力指令値と、式（３０）及び式（３１）に示す収縮力指令値とが頻繁に切り替わる。これにより、振動的な制御入力を発生するチャタリング現象が生じる可能性がある。

そこで、本第１実施形態では、ローパスフィルタ１７４，１７５として一次遅れフィルタを用いて収縮力指令値ｕ_ｆ，ｕ_ｅの高周波成分を低減する。ローパスフィルタ１７４，１７５の伝達関数Ｌ_１（ｓ）を、時定数Ｔ_ｌ１を用いて、

と表す。

（１．３ シミュレーション）
前節で導出した制御系を用いて位置決めシミュレーションを行う。本第１実施形態では、制御帯域ｆ_ｃを２５Ｈｚ、増幅率ａを２とし、固有振動数ｆ_ｍｉｎを５０Ｈｚとする。そして、線密度μを２．５×１０^−４ｋｇ／ｍ、ワイヤ長Ｌを０．２ｍとして張力最低値Ｔ_ｍｉｎを４Ｎとする。また、ローパスフィルタ１７４，１７５の時定数Ｔ_ｌ１を４．５×１０^−３ｓｅｃとする。目標関節角度θ_ｒｅｆとして、０．２ｓｅｃから０．７ｓｅｃの間に０ｒａｄから０．８５ｒａｄまで増加するランプ状の信号を与える。また、弦振動が生じる場合にも制御系が安定となることを確かめるために、時刻０．５ｓｅｃから０．５１ｓｅｃの間に、１．５Ｎのパルス外乱をワイヤ１１１，１１２に印加する。

図９（ａ）と図９（ｂ）はそれぞれ、ワイヤ張力Ｔ_ｆとワイヤ張力Ｔ_ｅの応答を、図１０は角度偏差Δθの応答を表す。図９及び図１０において、実線は一定ゲインの位置制御部１５２と張力制御部１５３を有する制御装置１５０による応答を、破線はワイヤ１１１，１１２の張力と制御帯域が比例する、上記特許文献１のゲイン可変制御器による応答を、それぞれ表す。なお、張力制御を行わずに一定ゲインの位置制御のみを行うシミュレーションでは、張力の低下によって弦振動の固有振動数が低下し、制御系が不安定となるため、応答を図示していない。

図９（ａ）及び図９（ｂ）より、本第１実施形態の張力制御では、張力Ｔ_ｆ，Ｔ_ｅがどちらも張力最低値４Ｎ以上となっていることがわかる。これは、張力制御がワイヤ１１１，１１２の張力を張力最低値Ｔ_ｍｉｎ以上となるように制御しているためである。これにより、弦振動の固有振動数が固有振動数ｆ_ｍｉｎ以上に保たれるため、弦振動が生じる０．５ｓｅｃ以降においても位置制御系が安定となる。さらに、制御帯域ｆ_ｃが保たれるため、高精度な位置決めが可能となる。一方、ゲイン可変制御はワイヤの張力を考慮して収縮力指令値ｕ_ｆ，ｕ_ｅを制御していないため、０．７ｓｅｃ以降では張力が２Ｎ以下に低下している。ゲイン可変制御の制御帯域は張力に比例するため、０．７ｓｅｃ以降でのゲイン可変制御の制御帯域は、制御帯域ｆ_ｃに比べて低下する。そのため、図１０に示すように、ゲイン可変制御による角度偏差Δθは、張力制御に比べて増加している。これより、本第１実施形態では、張力制御部１５３は張力の低下を防止し、制御系を安定化することで、広制御帯域な位置制御部１５２による高精度な位置決めを実現していることがわかる。

以上、本第１実施形態では、弦振動の固有振動数が位置制御部１５２の制御帯域ｆ_ｃを超えるようにワイヤ１１１，１１２の張力を制御する。これにより、外乱に強い広制御帯域な位置制御部１５２を用いた場合においても制御系が安定となるため、高精度な位置決め制御を行うことが可能となる。

また張力制御は、ワイヤ振動の固有振動数を向上させるためにワイヤ１１１，１１２の張力を増加させる。そのため、従来のように、関節剛性がワイヤ１１１，１１２の張力に比例する可変剛性要素を用いた場合、張力制御によって関節剛性が増加してしまう可能性がある。しかし、本第１実施形態では、位置制御部１５２の制御帯域に基づいて、制御系の安定化に必要な最低限の張力に制御するため、関節剛性が過度に増加することがなく、関節の柔軟性が損なわれることがない。

［第２実施形態］
次に、本発明の第２実施形態に係るロボットについて説明する。なお、本第２実施形態においてロボット本体の構成は上記第１実施形態と同様であり、以下の説明において、ロボット本体の各部の構成については、上記第１実施形態の図２と同じ符号を用い、詳細な説明を省略する。

上記第１実施形態の張力制御では、張力最低値Ｔ_ｍｉｎ、関節角度θ及びトルク指令値τ_ｒｅｆに基づいて収縮力指令値ｕ_ｆ，ｕ_ｅを一意に演算している。そのため、収縮力指令値の和Ｕ、すなわち関節の剛性と張力Ｔ_ｆ，Ｔ_ｅを独立に制御できない。一方、関節角度θやトルク指令値τ_ｒｅｆの大きさによって、張力制御を行わなくとも張力Ｔ_ｆ，Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回ることがある。

そこで、本第２実施形態では、張力Ｔ_ｆ，Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回る場合は所望の関節剛性を実現するように収縮力指令値ｕ_ｆ，ｕ_ｅを制御する剛性制御を行い、張力Ｔ_ｆ，Ｔ_ｅが張力最低値Ｔ_ｍｉｎを下回るときにのみ張力制御を行う。

（２．１ 制御系設計）
上記第１実施形態の式（５）に示すように、関節剛性Ｕは、収縮力指令値ｕ_ｆ，ｕ_ｅの和に比例する。そこで、剛性制御では、収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’の和分が剛性指令値Ｕ_ｒｅｆに、差分とモーメントアーム径ｒの積がトルク指令値τ_ｒｅｆになるという条件で収縮力を導出する。このとき、剛性指令値Ｕ_ｒｅｆとトルク指令値τ_ｒｅｆは、

と表されるため、式（３３）と式（３４）を収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’について解き、

を得る。式（３５）及び式（３６）より、本第２実施形態の剛性制御では簡便な演算によって所望の関節剛性を実現するための収縮力を導出できることがわかる。

剛性制御と張力制御を切り替えるための切り替え制御では、ワイヤ１１１の張力Ｔ_ｆとワイヤ１１２の張力Ｔ_ｅの少なくとも一方が張力最低値Ｔ_ｍｉｎを下回る場合は、張力制御による収縮力指令値ｕ_ｆ’，ｕ_ｅ’を出力する。一方、張力Ｔ_ｆと張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回る場合は剛性制御による収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’を出力する。なお、本第２実施形態では、切り替え制御に用いるワイヤ張力Ｔ_ｆ，Ｔ_ｅを、張力センサを用いて直接計測するが、関節角度θと１サンプル前の収縮力指令値ｕ_ｆ，ｕ_ｅを用いて、式（３）及び式（４）より推定してもよい。

図１１は、本発明の第２実施形態に係るロボットの制御装置を示すブロック線図である。本第２実施形態の制御装置２５０は、上記第１実施形態と同様、減算部１５１、位置制御部１５２及び駆動制御部１５４を備えている。また、制御装置２５０は、張力・剛性制御部２０８を備えている。

張力・剛性制御部２０８は、上記第１実施形態と同様の張力最低値計算部１８１を備えると共に、駆動力指令値計算部２８２及びローパスフィルタ２１１，２１２を備えている。

駆動力指令値計算部２８２は、上記第１実施形態と同様の張力補償量演算部１７３と、剛性制御部２０９と、収縮力切り替え部２１０とを有している。なお、張力最低値計算部１８１と張力補償量演算部１７３とで張力制御部２５３が構成されている。

剛性制御部２０９は、外部機器から剛性指令値Ｕ_ｒｅｆの入力を受けると共に位置制御部１５２からトルク指令値τ_ｒｅｆの入力を受ける。そして、剛性制御部２０９は、関節１０７（図２）の剛性が、剛性指令値Ｕ_ｒｅｆとなるように、剛性指令値Ｕ_ｒｅｆ及びトルク指令値τ_ｒｅｆを用いて仮の収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’を計算する。

なお、張力補償量演算部１７３は、上記第１実施形態と同様の計算処理により、仮の収縮力指令値ｕ_ｆ’，ｕ_ｅ’を計算する。

図１２は、収縮力切り替え部における演算処理を示すフローチャートである。収縮力切り替え部２１０は、ワイヤ１１１の張力Ｔ_ｆが張力最低値Ｔ_ｍｉｎを上回る（Ｔ_ｆ＞Ｔ_ｍｉｎである）か否かを判断する（Ｓ３１）。

収縮力切り替え部２１０は、ワイヤ１１１の張力Ｔ_ｆが張力最低値Ｔ_ｍｉｎを上回ると判断した場合（Ｓ３１：Ｙｅｓ）、ワイヤ１１２の張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回る（Ｔ_ｅ＞Ｔ_ｍｉｎである）か否かを判断する（Ｓ３２）。

収縮力切り替え部２１０は、ワイヤ１１２の張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回ると判断した場合（Ｓ３２：Ｙｅｓ）、ｕ_ｆ＝ｕ_ｆ’’，ｕ_ｅ＝ｕ_ｅ’’とする（Ｓ３３）。

収縮力切り替え部２１０は、ワイヤ１１１の張力Ｔ_ｆが張力最低値Ｔ_ｍｉｎを上回らない（Ｓ３１：Ｎｏ）、又はワイヤ１１２の張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回らないと判断した場合（Ｓ３２：Ｎｏ）、ｕ_ｆ＝ｕ_ｆ’，ｕ_ｅ＝ｕ_ｅ’とする（Ｓ３４）。

つまり、駆動力指令値計算部２８２は、ワイヤ１１１，１１２のうち少なくとも一方のワイヤの張力が張力最低値Ｔ_ｍｉｎを下回る場合には、ワイヤ１１１，１１２のうち張力が小さい方のワイヤの張力が張力最低値Ｔ_ｍｉｎとなる収縮力指令値を計算する。換言すると、駆動力指令値計算部２８２は、ワイヤ１１１，１１２のうち少なくとも一方のワイヤの張力が張力最低値Ｔ_ｍｉｎを下回る場合には、仮の収縮力指令値ｕ_ｆ’，ｕ_ｅ’を収縮力指令値ｕ_ｆ，ｕ_ｅとする。

また、駆動力指令値計算部２８２は、両方のワイヤ１１１，１１２の張力が張力最低値Ｔ_ｍｉｎを上回る場合には、関節の剛性が剛性指令値Ｕ_ｒｅｆとなるように、剛性指令値Ｕ_ｒｅｆ及びトルク指令値τ_ｒｅｆを用いて収縮力指令値を計算する。換言すると、駆動力指令値計算部２８２は、両方のワイヤ１１１，１１２の張力が張力最低値Ｔ_ｍｉｎを上回る場合には、仮の収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’を収縮力指令値ｕ_ｆ，ｕ_ｅとする。

なお、本第２実施形態では、Ｔ_ｆ＝Ｔ_ｍｉｎ又はＴ_ｅ＝Ｔ_ｍｉｎである場合には、仮の収縮力指令値ｕ_ｆ’，ｕ_ｅ’を収縮力指令値ｕ_ｆ，ｕ_ｅとしているが、仮の収縮力指令値ｕ_ｆ’’，ｕ_ｅ’’を収縮力指令値ｕ_ｆ，ｕ_ｅとしてもよい。

この切り替え制御では、張力Ｔ_ｆと張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎの近傍となる場合、張力制御と剛性制御の制御入力が頻繁に切り替わる。これにより、制御入力が振動的となるチャタリング現象が生じるため、ローパスフィルタ２１１，２１２として一次遅れフィルタを用いて、制御入力の高周波成分を低減する。ローパスフィルタ２１１，２１２の伝達関数Ｌ_２（ｓ）を、時定数Ｔ_ｌ２を用いて、

と表す。なお、本第２実施形態の制御装置２５０では、上記第１実施形態と異なり、ローパスフィルタ１７４，１７５を備えていない。これは、収縮力指令値ｕ_ｆ’，ｕ_ｅ’の高周波数成分は、ローパスフィルタ２１１，２１２によって低減可能なためである。

（２．２ シミュレーション）
前節で導出した制御系を用いて位置決めシミュレーションを行う。ワイヤ１１１，１１２の物理パラメータと位置制御部１５２及び張力制御部２５３の制御パラメータを上記第１実施形態と同一とする。

また、ローパスフィルタ２１１，２１２の時定数Ｔ_ｌ２を４．５×１０^−３ｓｅｃとし、剛性指令値Ｕ_ｒｅｆを１２．５Ｎとする。目標関節角度θ_ｒｅｆとして０．２ｓｅｃから０．７ｓｅｃの間に０ｒａｄから０．６ｒａｄまで増加するランプ状の信号を与える。

図１３（ａ）と図１３（ｂ）はそれぞれ、ワイヤ１１１の張力Ｔ_ｆとワイヤ１１２の張力Ｔ_ｅの応答を、図１４は収縮力の和Ｕの応答を表す。図１３と図１４において、実線は本第２実施形態の張力・剛性制御による応答を、破線は張力制御のみを行う上記第１実施形態の制御系の応答を表す。また、図１５は、本第２実施形態の張力・剛性制御による角度偏差Δθの応答を表す。

図１３（ａ）と図１３（ｂ）に示すように、時刻０．６ｓｅｃまでの間は張力Ｔ_ｆと張力Ｔ_ｅが張力最低値４Ｎを上回っている。張力・剛性制御では、張力Ｔ_ｆと張力Ｔ_ｅが張力最低値Ｔ_ｍｉｎを上回る場合、収縮力の和Ｕを剛性指令値Ｕ_ｒｅｆとする剛性制御を行う。そのため、図１４に示すように時刻０．６ｓｅｃまでの間は、関節剛性を剛性指令値Ｕ_ｒｅｆと一致させるように制御可能であることがわかる。一方、図１３（ａ）と図１３（ｂ）に示すように、張力・剛性制御では、０．６ｓｅｃ以降において張力Ｔ_ｆと張力Ｔ_ｅが張力最低値４Ｎと一致していることがわかる。これは、張力・剛性制御は張力Ｔ_ｆと張力Ｔ_ｅの一方が張力最低値Ｔ_ｍｉｎを下回る場合、張力Ｔ_ｆと張力Ｔ_ｅの低い方を張力最低値Ｔ_ｍｉｎとする張力制御を行うためである。図１４に示すように、０．６ｓｅｃ以降では関節剛性の制御から張力制御に切り替わるため、収縮力の和Ｕが剛性指令値Ｕ_ｒｅｆに比べて大きくなってしまっている。しかし、図１５より、張力制御によって制御系が不安定となるのを防いでいるため、角度偏差が０ｒａｄに収束していることがわかる。一方、張力制御のみを行う第１実施形態の制御系では、関節剛性を考慮して収縮力を演算していない。そのため、０．６ｓｅｃ以降だけでなく、０ｓｅｃから０．６ｓｅｃの間においても剛性指令値Ｕ_ｒｅｆと一致するように収縮力の和Ｕを制御できない。これより、本第２実施形態の張力・剛性制御によって、張力Ｔ_ｆ，Ｔ_ｅが張力最低値Ｔ_ｍｉｎより大きい場合には、剛性指令値と一致するように関節の剛性を制御できることがわかる。

即ち、本第２実施形態では、ワイヤ１１１，１１２の張力が低下して制御系が不安定となる場合にのみ上記第１実施形態と同様の張力制御を行い、制御系が安定となる場合は所望の関節剛性を実現するように張力を制御する。これらのことから、本第２実施形態の張力制御方法では広制御帯域な位置制御部１５２を用いた場合においても、関節の柔軟性が大きく損なわれることがない。さらに、所望の関節剛性を実現するための制御入力を簡便な剛性制御部２０９によって演算するため、関節剛性の目標値を容易に変更できる。

［第３実施形態］
次に、本発明の第３実施形態に係るロボットについて説明する。本第３実施形態では、ワイヤと、人工筋肉アクチュエータを有する駆動部とを備えるロボット本体である２リンクマニピュレータにおいて、ワイヤの張力と関節剛性を制御する。

（３．１ モデルの導出）
人間の上肢上腕部や下肢大腿部は、二関節筋と呼ばれる２関節同時駆動マニピュレータを有しており、二関節筋が手先の剛性制御に重要な役割を果たすことが知られている。人間の上肢をモデルとしたロボットは図１６に示すように第１関節３１３を駆動するアクチュエータｆ_１，ｅ_１、第２関節３１４を駆動するアクチュエータｆ_２，ｅ_２、二関節を同時に駆動するアクチュエータｆ_３，ｅ_３をそれぞれ拮抗配置した３対６筋構造である。これらアクチュエータｆ_１，ｅ_１，ｆ_２，ｅ_２，ｆ_３，ｅ_３は、人工筋肉アクチュエータである。

（３．１．１ シミュレーションモデルの導出）
図１７は、本発明の第３実施形態に係るロボットの概略構成を示す説明図である。本第３実施形態のロボット３００は、３対６筋２リンクアームであるロボット本体３２０と、制御装置３５０とを備えている。

ロボット本体３２０は、第１リンク３０１と、第２リンク３０２と、基部であるプーリ３０３とを有している。第１リンク３０１は、第１関節３１３でプーリ３０３に対して旋回可能に連結されている。第２リンク３０２は、第２関節３１４で第１リンク３０１に対して旋回可能に連結されている。また、ロボット本体３２０は、アクチュエータｆ_１，ｅ_１，ｆ_２，ｅ_２，ｆ_３，ｅ_３と、これらに駆動される紐状部材としてのワイヤ３４１〜３４６とを備えている。ワイヤ３４１，３４３，３４５と、ワイヤ３４２，３４４，３４６とは、リンク３０１，３０２を挟んで互いに拮抗して配置されている。

ワイヤ３４１及びワイヤ３４２は、第１リンク３０１を旋回させるようにプーリ３０３を介して第１リンク３０１に接続されている。ワイヤ３４３及びワイヤ３４４は、第１リンク３０１に対して第２リンク３０２を旋回させるようにプーリ３０３を介して第２リンク３０２に接続されている。ワイヤ３４５及びワイヤ３４６は、第１及び第２リンク３０１，３０２を同時に旋回させるように、第１及び第２リンク３０１，３０２に接続されている。

本第３実施形態では、上記第１実施形態と同様に仮定（１）〜仮定（５）を用いて、ワイヤの弦振動を１つの質点と２つのバネでモデル化する。

各人工筋肉アクチュエータｆ_１，ｆ_２，ｆ_３，ｅ_１，ｅ_２，ｅ_３の収縮力をｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３，ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３、各関節３１３，３１４の関節角度をθ_１，θ_２とする。また、各リンク３０１，３０２の慣性モーメントをＪ_１，Ｊ_２、各リンク３０１，３０２の長さをＬ_１，Ｌ_２、各トルク指令値をτ_ｒｅｆ１，τ_ｒｅｆ２とする。また、本第３実施形態では各アクチュエータのモーメントアーム径、弾性力定数、粘性力定数をそれぞれ等しくｒ、ｋ、ｃとする。３対６筋構造を備える２リンクマニピュレータの運動方程式を導出すると、

となる。ただし、式（３８）の関節角度θ、トルク指令値τ_ｒｅｆ、慣性行列Ｍ（θ）、コリオリ力・遠心力、弾性係数行列Ｋ_ｍ、減衰係数行列Ｃ_ｍをそれぞれ、

とする。本第３実施形態では、式（３８）のモデルをシミュレーションモデルと呼ぶ。

（３．１．２ 近似モデルの導出）
本項では、位置制御部の制御帯域を導出する際に用いる２リンクマニピュレータの近似モデルを導出する。式（３８）より、慣性行列Ｍ（θ）、弾性係数行列Ｋ_ｍ、減衰係数行列Ｃ_ｍには非対角成分が存在するため、２リンクマニピュレータは各関節の運動が干渉する多入出力系となることがわかる。さらに、式（３８）より、慣性行列Ｍ（θ）、コリオリ力・遠心力に非線形項を有することがわかる。一方、上記第１実施形態の一巡伝達関数による制御帯域の導出方法は、干渉や非線形性を考慮していないため、単入出力系・線形系以外には適用できない。そこで、シミュレーションモデルを非干渉化、線形化し、近似モデルを導出する。そして、近似モデルと制御器を乗算した一巡伝達関数によって制御帯域を導出する。

まず、仮定（６）及び仮定（７）を用いてシミュレーションモデルを非干渉化する。

仮定（６） コリオリ力と遠心力を考慮しない。

仮定（７） 慣性行列Ｍ（θ）、弾性係数行列Ｋ_ｍ、減衰係数行列Ｃ_ｍ対角成分のみを考慮する。

また、シミュレーションモデルを、位置決め終端の関節角度θ_ｆｉｎ１，θ_ｆｉｎ２近傍で線形化する。

仮定（６）及び仮定（７）と式（４５）より、近似モデルの運動方程式は、

となる。ただし、式（４６）において、慣性行列Ｍ’、弾性係数行列Ｋ_ｍ’、減衰係数行列Ｃ_ｍ’はそれぞれ、

となる。

（３．２ 制御系設計）
本節では、人工筋肉アクチュエータｆ_１，ｆ_２，ｆ_３，ｅ_１，ｅ_２，ｅ_３の収縮力を制御する制御系の構成を示す。また、前節で導出した近似モデルとシミュレーションモデルを用いた、張力最低値の導出方法を示す。

（３．２．１制御系の構成）
本第３実施形態では、第１関節３１３を駆動するアクチュエータｆ_１，ｅ_１によって、関節角度θ_１とワイヤ張力Ｔ_ｆ１，Ｔ_ｅ１を制御する。同様に、第２関節３１４を駆動するアクチュエータｆ_２，ｅ_２によって、関節角度θ_２とワイヤ張力Ｔ_ｆ２，Ｔ_ｅ２を制御する。

これら制御系では、上記第２実施形態と同様に張力制御と剛性制御を、ワイヤ張力に応じて切り替える。一方、二関節同時駆動アクチュエータｆ_３，ｅ_３は関節剛性のみを制御し、関節角度の制御に用いない。それは、位置制御部の設計と制御帯域の導出が複雑となるためである。このとき、アクチュエータｆ_３，ｅ_３へのトルク指令値は０となるため、上記第２実施形態の式（３５）及び式（３６）より、剛性制御による収縮力指令値ｕ_ｆ３，ｕ_ｅ３は二関節同時駆動アクチュエータｆ_３，ｅ_３の剛性指令値Ｕ_ｒｅｆ３を用いて、

と表せる。

図１８は、本発明の第３実施形態に係るロボットの制御装置を示すブロック線図である。制御装置３５０は、減算部３５１，３５２と、位置制御部３６１，３６２と、張力・剛性制御部３８１，３８２と、剛性制御部３９１と、駆動制御部３９２とを備えている。

これら減算部３５１，３５２及び位置制御部３６１，３６２は、上記第１実施形態の減算部１５１及び位置制御部１５２と同様の構成である。

また、張力・剛性制御部３８１，３８２は、上記第２実施形態の張力・剛性制御部２０８と同様の構成である。

剛性制御部３９１は式（５０）に示す収縮力指令値ｕ_ｆ３，ｕ_ｅ３を出力するフィードフォワード制御部である。駆動制御部３９２は、収縮力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３，ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３の入力を受けて、収縮力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３，ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３に対応する収縮力を、各アクチュエータｆ_１，ｆ_２，ｆ_３，ｅ_１，ｅ_２，ｅ_３に発生させる。制御対象であるロボット本体３２０は、式（３８）でモデル化される２リンクアームである。

（３．２．２ 制御帯域の導出）
位置制御部３６１の制御帯域ｆ_ｃ１を、近似モデルの伝達関数Ｐ_１（ｓ）と、位置制御部３６１の伝達関数Ｋ_１（ｓ）とを乗算した一巡伝達関数Ｐ_１（ｓ）Ｋ_１（ｓ）を用いて導出する。また、位置制御部３６２の制御帯域ｆ_ｃ２を、近似モデルの伝達関数Ｐ_２（ｓ）と、位置制御部３６２の伝達関数Ｋ_２（ｓ）とを乗算した一巡伝達関数Ｐ_２（ｓ）Ｋ_２（ｓ）を用いて導出する。

式（３８）より、近似モデルにおけるトルク指令値τ_ｒｅｆ１，τ_ｒｅｆ２から関節角度θ_１，θ_２までの伝達関数Ｐ_１（ｓ），Ｐ_２（ｓ）は、収縮力指令値ｕ_ｆ１，ｕ_ｆ２，ｕ_ｆ３，ｕ_ｅ１，ｕ_ｅ２，ｕ_ｅ３の和Ｕ_１，Ｕ_２，Ｕ_３を用いて、それぞれ、

と表される。また、本第３実施形態では、位置制御部３６１，３６２をＰＩＤ制御部とするため、伝達関数Ｋ_１（ｓ），Ｋ_２（ｓ）は、微分ゲインＫ_ｄ１，Ｋ_ｄ２、比例ゲインＫ_ｐ１，Ｋ_ｐ２、積分ゲインＫ_ｉ１，Ｋ_ｉ２、時定数Ｔ_ｄ１，Ｔ_ｄ２を用いて、

と表される。本第３実施形態では、筋の弾性によるリンクの固有振動数に比べて制御帯域が高くなるように位置制御部３６１，３６２を設計する。そこで、一巡伝達関数Ｐ_１（ｓ）Ｋ_１（ｓ），Ｐ_２（ｓ）Ｋ_２（ｓ）において、ラプラス演算子ｓに関する一次の項のみを考慮すると、

となる。式（５５）及び式（５６）より、制御帯域ｆ_ｃ１，ｆ_ｃ２は、

と表される。

（３．２．３ 張力最低値の導出）
近似モデルは線形化と非干渉化によるモデル誤差を含む。そのため、制御帯域ｆ_ｃ１，ｆ_ｃ２を用いて導出する張力最低値が、実際の制御系の安定化に必要な張力よりも小さくなり、制御系が不安定となる恐れがある。そこで、本第３実施形態では、シミュレーションよって張力最低値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２を修正し、適当な張力最低値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２を導出する。

まず、増幅率ａ_１，ａ_２を１から徐々に増加させながら近似モデルにおける制御系の安定性を調べ、増幅率ａ_１，ａ_２の初期値を導出する。次に、増幅率ａ_１，ａ_２の初期値と式（１５）及び式（１７）より、張力最低値の初期値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２を演算し、シミュレーションモデルを用いて制御系の安定性を調べる。そして、制御系が安定であれば増幅率ａ_１，ａ_２を減少させ、不安定であれば増幅率ａ_１，ａ_２を増加させる。さらに、新たな増幅率ａ_１，ａ_２を用いて張力最低値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２を演算し、シミュレーションモデルを用いて安定性の確認と増幅率ａ_１，ａ_２の更新を行う。これを繰り返すことにより、張力最低値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２を導出する。

（３．３ シミュレーション結果）
本節では前節で導出した制御系を用いて２リンクアームの位置決めシミュレーションを行う。各ワイヤの物理パラメータを上記第１実施形態と同一とする。また、制御帯域ｆ_ｃ１，ｆ_ｃ２を２５Ｈｚ、増幅率ａを２として、張力最低値Ｔ_ｍｉｎ１，Ｔ_ｍｉｎ２をそれぞれ４Ｎとする。さらに、剛性指令値Ｕ_ｒｅｆ１，Ｕ_ｒｅｆ２，Ｕ_ｒｅｆ３をそれぞれ、１２．５Ｎ、１２．５Ｎ、３Ｎとする。０．２ｓｅｃから０．７ｓｅｃの間に０ｒａｄから０．５ｒａｄまで増加するランプ状の信号を目標関節角度θ_ｒｅｆ１，θ_ｒｅｆ２として与える。そして、弦振動が生じる場合にも制御系が安定となることを確かめるために、０．５ｓｅｃから０．５１ｓｅｃの間に０．６Ｎのパルス外乱を各ワイヤに印加する。

図１９（ａ）と図１９（ｂ）は張力Ｔ_ｅ１と張力Ｔ_ｅ２の応答を、図２０（ａ）と図２０（ｂ）は収縮力の和Ｕ_１と和Ｕ_２の応答を、それぞれ表す。また、図２１（ａ）と図２１（ｂ）はそれぞれ、角度偏差Δθ_１と角度偏差Δθ_２の応答を表す。図１９（ａ）及び図１９（ｂ）より、０ｓｅｃから０．５ｓｅｃの間は、張力Ｔ_ｅ１と張力Ｔ_ｅ２が張力最低値４Ｎ以上となることがわかる。張力・剛性制御部３８１，３８２は、ワイヤの張力が張力最低値を上回る場合は、所望の関節剛性を実現する剛性制御を行う。そのため、図２０（ａ）及び図２０（ｂ）に示すように、０ｓｅｃから０．５ｓｅｃまでの間は、関節剛性Ｕ_１，Ｕ_２が剛性指令値Ｕ_ｒｅｆ１，Ｕ_ｒｅｆ２と、それぞれ一致していることがわかる。

一方、図１９（ａ）及び図１９（ｂ）より、０．５ｓｅｃ以降では張力Ｔ_ｅ１と張力Ｔ_ｅ２が張力最低値４Ｎと一致することがわかる。これは、張力・剛性制御部３８１，３８２が、ワイヤ張力が張力最低値以上となるように制御しているためである。

図２０（ａ）及び図２０（ｂ）に示すように、０．５ｓｅｃ以降では関節剛性の制御から張力制御に切り替わるため、剛性指令値Ｕ_ｒｅｆ１，Ｕ_ｒｅｆ２と比べて関節剛性Ｕ_１，Ｕ_２が増加している。しかし、図２１（ａ）及び図２１（ｂ）に示すように、張力制御によって制御系が安定となるため、弦振動が生じた場合にも角度偏差Δθ_１と角度偏差Δθ_２が０ｒａｄに収束していることがわかる。このことから、本第３実施形態の張力・剛性制御部３８１，３８２を多関節マニピュレータに適用した場合もワイヤの張力を張力最低値以上に保ち、制御系を安定化できることがわかる。また、張力制御と剛性制御の切り替え制御を行うことにより、ワイヤの張力が張力最低値を上回る場合には所望の関節剛性を実現できることがわかる。

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

上記第１〜第３実施形態では、紐状部材としてワイヤを用いた場合について説明したが、紐状部材はこれに限定するものではなく、例えばベルトであってもよい。

また、上記第１〜第３実施形態では、駆動部を構成するアクチュエータが人工筋肉アクチュエータである場合について説明したが、これに限定するものではない。人工筋肉アクチュエータと同等、即ち図１に示すような粘弾性モデルに近似されるアクチュエータであれば、いかなるアクチュエータであってもよい。例えば、回転モータとバネとを組み合わせたアクチュエータであってもよい。

１００…ロボット、１０１…アクチュエータ（第１アクチュエータ）、１０２…アクチュエータ（第２アクチュエータ）、１０６…リンク、１０７…関節、１０８…駆動部、１１１…ワイヤ（第１紐状部材）、１１２…ワイヤ（第２紐状部材）、１２０…ロボット本体、１５０…制御装置、１５２…位置制御部、１５４…駆動制御部、１７３…張力補償量演算部（駆動力指令値計算部）、１８１…張力最低値計算部、ｆ_ｃ…制御帯域、ｆ_ｍｉｎ…固有振動数、ｆ_ｎ…ナイキスト周波数、Ｔ_ｍｉｎ…張力最低値、ｕ_ｆ，ｕ_ｅ…収縮力指令値

Claims (4)

1. 関節で旋回するリンクと、
   前記リンクに接続され、互いに拮抗して配置された第１紐状部材及び第２紐状部材と、
   前記第１紐状部材を進退させる駆動力を発生する第１アクチュエータ、及び前記第２紐状部材を進退させる駆動力を発生する第２アクチュエータを有し、駆動力の差で前記関節まわりにトルクを発生し、駆動力の和で前記関節に剛性を与える駆動部と、
   前記第１及び第２アクチュエータに発生させる駆動力を制御する制御装置と、を備え、
   前記制御装置は、
   外部から入力を受けた目標関節角度に対する関節角度の角度偏差に基づいて、前記関節まわりのトルク指令値を算出する位置制御部と、
   前記各紐状部材の固有振動数をｆ_ｍｉｎ、前記位置制御部の制御帯域をｆ_ｃ、前記位置制御部のナイキスト周波数をｆ_ｎとしたとき、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす前記固有振動数を求め、前記第１及び第２紐状部材の長さをＬ、前記第１及び第２紐状部材の線密度をμ、前記第１及び第２紐状部材の張力最低値をＴ_ｍｉｎとしたとき、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から前記張力最低値を算出する張力最低値計算部と、
   前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて、前記第１及び第２アクチュエータの駆動力指令値を算出する駆動力指令値計算部と、
   前記駆動力指令値計算部により求めた駆動力指令値に対応する駆動力を、前記第１及び第２アクチュエータに発生させる駆動制御部と、を有することを特徴とするロボット。
2. 前記駆動力指令値計算部は、
   前記第１及び第２紐状部材のうち少なくとも一方の紐状部材の張力が、前記張力最低値を下回る場合には、前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて前記第１及び第２アクチュエータの駆動力指令値を計算し、
   前記第１及び第２紐状部材の両方の紐状部材の張力が、前記張力最低値を上回る場合には、前記関節の剛性が外部から入力を受けた剛性指令値となるように、前記剛性指令値及び前記トルク指令値を用いて前記第１及び第２アクチュエータの駆動力指令値を計算することを特徴とする請求項１に記載のロボット。
3. 関節で旋回するリンクと、前記リンクに接続され、互いに拮抗して配置された第１紐状部材及び第２紐状部材と、前記第１紐状部材を進退させる駆動力を発生する第１アクチュエータ、及び前記第２紐状部材を進退させる駆動力を発生する第２アクチュエータを有し、駆動力の差で前記関節まわりにトルクを発生し、駆動力の和で前記関節に剛性を与える駆動部と、を備えたロボット本体を、制御装置により制御するロボット制御方法において、
   前記制御装置の位置制御部が、外部から入力を受けた目標関節角度に対する関節角度の角度偏差に基づいて、前記リンクの前記関節まわりのトルク指令値を算出する位置制御工程と、
   前記制御装置の張力最低値計算部が、前記各紐状部材の固有振動数をｆ_ｍｉｎ、前記位置制御部の制御帯域をｆ_ｃ、前記位置制御部のナイキスト周波数をｆ_ｎとしたとき、ｆ_ｎ＞ｆ_ｍｉｎ＞ｆ_ｃを満たす前記固有振動数を求め、前記第１及び第２紐状部材の長さをＬ、前記第１及び第２紐状部材の線密度をμ、前記第１及び第２紐状部材の張力最低値をＴ_ｍｉｎとしたとき、Ｔ_ｍｉｎ＝４×Ｌ^２×μ×ｆ_ｍｉｎ ^２の関係式から前記張力最低値を算出する張力最低値計算工程と、
   前記制御装置の駆動力指令値計算部が、前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて、前記第１及び第２アクチュエータの駆動力指令値を算出する駆動力指令値計算工程と、
   前記制御装置の駆動制御部が、前記駆動力指令値計算工程にて求められた駆動力指令値に対応する駆動力を、前記第１及び第２アクチュエータに発生させる駆動制御工程と、を備えたことを特徴とするロボット制御方法。
4. 前記駆動力指令値計算工程では、
   前記駆動力指令値計算部が、前記第１及び第２紐状部材のうち少なくとも一方の紐状部材の張力が、前記張力最低値を下回る場合には、前記第１及び第２紐状部材のうち張力が小さい方の紐状部材の張力が前記張力最低値となるように、前記張力最低値、前記関節角度及び前記トルク指令値を用いて前記第１及び第２アクチュエータの駆動力指令値を計算し、
   前記第１及び第２紐状部材の両方の紐状部材の張力が、前記張力最低値を上回る場合には、前記関節の剛性が外部から入力を受けた剛性指令値となるように、前記剛性指令値及び前記トルク指令値を用いて前記第１及び第２アクチュエータの駆動力指令値を計算することを特徴とする請求項３に記載のロボット制御方法。

Images