(57)【要約】【課題】ロボットアームのあるタスク位置（例えば、手先位置）の剛性を要求される剛性となるように、各関節の回転方向に対する剛性を決定する駆動装置を提供する。【解決手段】駆動装置１０は、複数の関節１６と複数の関節１６を駆動する複数のモータ３２との間にそれぞれ設けられた、関節１６の回転方向における剛性を可変する可変剛性要素５０を含む柔軟伝達部材３８を有する駆動部１２と、駆動部１２を駆動制御する制御部１４とを備え、制御部１４は、リンク２０のうち予め定められたタスク位置に要求される剛性と、複数の関節１６を駆動する複数のモータの回転角度によって決定される係数行列とに基づいて、複数の関節１６の剛性閾値を算出する剛性閾値算出部８２を備える。【選択図】図２
2014046417.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節を有するリンクと、前記複数の関節を駆動する複数の回転駆動源と、前記関節と前記回転駆動源との間にそれぞれ設けられた、前記関節の回転方向における剛性を可変する可変剛性要素を含む柔軟伝達部材とを有する駆動部と、
前記駆動部を駆動制御する制御部と、
を備える駆動装置において、
前記制御部は、
前記リンクのうち予め定められたタスク位置に要求される剛性と、前記複数の関節を駆動する前記複数の回転駆動源の回転角度によって決定される係数行列とに基づいて、前記複数の関節の剛性閾値を算出する剛性閾値算出部を備える
ことを特徴とする駆動装置。
【請求項２】
請求項１に記載の駆動装置において、
前記剛性閾値算出部は、前記係数行列の転置行列と、前記タスク位置の剛性を表す行列と、前記係数行列との積を算出して得られた行列の対角要素を、前記複数の関節の前記剛性閾値として用いる
ことを特徴とする駆動装置。
【請求項３】
請求項１又は２に記載の駆動装置において、
前記複数の関節の前記剛性閾値以上で前記複数の関節の剛性を決定する剛性決定部と、
前記複数の関節の剛性が前記剛性決定部によって決定された前記複数の関節の剛性となるように、前記複数の関節の前記可変剛性要素の剛性を変更する剛性変更制御部と、
を備えることを特徴とする駆動装置。
【請求項４】
請求項３に記載の駆動装置において、
前記柔軟伝達部材は、前記関節の回転方向における粘性係数を可変する可変粘性係数要素を含み、
前記剛性変更制御部が変更した前記複数の関節の剛性を表す行列と、前記複数の回転駆動源の回転角度によって決定された前記駆動部の慣性行列の逆行列との積を算出して得られた行列の対角要素の平方根を前記複数の関節の自己固有振動成分として用いる固有振動数算出部と、
前記複数の関節の前記自己固有振動成分と、前記慣性行列の対角要素とを用いて、前記複数の関節の粘性係数を算出する粘性係数算出部と、
前記複数の関節の粘性係数が前記粘性係数算出部によって算出された前記複数の関節の粘性係数となるように、前記複数の関節の前記可変粘性係数要素の粘性係数を変更する粘性変更制御部と、
を備えることを特徴とする駆動装置。
【請求項５】
複数の関節を有するリンクと、前記複数の関節を駆動する複数の回転駆動源と、前記関節と前記回転駆動源との間にそれぞれ設けられた、前記関節の回転方向における粘性係数を可変する可変粘性係数要素を含む柔軟伝達部材とを有する駆動部と、
前記駆動部を駆動制御する制御部と、
を備える駆動装置において、
前記複数の関節の剛性を表す行列と、前記複数の回転駆動源の回転角度によって決定された前記駆動部の慣性行列の逆行列との積を算出して得られた行列の対角要素の平方根を前記複数の関節の自己固有振動成分として用いる固有振動数算出部と、
前記複数の関節の前記自己固有振動成分と、前記慣性行列の対角要素とを用いて、前記複数の関節の粘性係数を算出する粘性係数算出部と、
前記複数の関節の粘性係数が前記粘性係数算出部によって算出された前記複数の関節の粘性係数となるように、前記複数の関節の前記可変粘性係数要素の粘性係数を変更する粘性変更制御部と、
を備えることを特徴とする駆動装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの関節を駆動させる駆動装置に関する。
【背景技術】
【０００２】
下記特許文献１には、モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置が記載されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−３４９５５５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記特許文献１に記載の技術では、関節の回転方向に対して剛性（例えば、弾性）を持たせることができるが、ロボットアームのあるタスク位置（例えば、手先位置）の剛性を要求される剛性となるように、各関節の回転方向に対する剛性を決定することはできない。
【０００５】
そこで本発明は、ロボットアームのあるタスク位置（例えば、手先位置）の剛性を要求される剛性となるように、各関節の回転方向に対する剛性を決定する駆動装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、複数の関節を有するリンクと、前記複数の関節を駆動する複数の回転駆動源と、前記関節と前記回転駆動源との間にそれぞれ設けられた、前記関節の回転方向における剛性を可変する可変剛性要素を含む柔軟伝達部材とを有する駆動部と、前記駆動部を駆動制御する制御部と、を備える駆動装置において、前記制御部は、前記リンクのうち予め定められたタスク位置に要求される剛性と、前記複数の関節を駆動する前記複数の回転駆動源の回転角度によって決定される係数行列とに基づいて、前記複数の関節の剛性閾値を算出する剛性閾値算出部を備えることを特徴とする。
【０００７】
前記駆動装置において、前記剛性閾値算出部は、前記係数行列の転置行列と、前記タスク位置の剛性を表す行列と、前記係数行列との積を算出して得られた行列の対角要素を、前記複数の関節の前記剛性閾値として用いてもよい。
【０００８】
前記駆動装置において、前記複数の関節の前記剛性閾値以上で前記複数の関節の剛性を決定する剛性決定部と、前記複数の関節の剛性が前記剛性決定部によって決定された前記複数の関節の剛性となるように、前記複数の関節の前記可変剛性要素の剛性を変更する剛性変更制御部と、を備えてもよい。
【０００９】
前記駆動装置において、前記柔軟伝達部材は、前記関節の回転方向における粘性係数を可変する可変粘性係数要素を含み、前記剛性変更制御部が変更した前記複数の関節の剛性を表す行列と、前記複数の回転駆動源の回転角度によって決定された前記駆動部の慣性行列の逆行列との積を算出して得られた行列の対角要素の平方根を前記複数の関節の自己固有振動成分として用いる固有振動数算出部と、前記複数の関節の前記自己固有振動成分と、前記慣性行列の対角要素とを用いて、前記複数の関節の粘性係数を算出する粘性係数算出部と、前記複数の関節の粘性係数が前記粘性係数算出部によって算出された前記複数の関節の粘性係数となるように、前記複数の関節の前記可変粘性係数要素の粘性係数を変更する粘性変更制御部と、を備えてもよい。
【００１０】
本発明は、複数の関節を有するリンクと、前記複数の関節を駆動する複数の回転駆動源と、前記関節と前記回転駆動源との間にそれぞれ設けられた、前記関節の回転方向における粘性係数を可変する可変粘性係数要素を含む柔軟伝達部材とを有する駆動部と、前記駆動部を駆動制御する制御部と、を備える駆動装置において、前記複数の関節の剛性を表す行列と、前記複数の回転駆動源の回転角度によって決定された前記駆動部の慣性行列の逆行列との積を算出して得られた行列の対角要素の平方根を前記複数の関節の自己固有振動成分として用いる固有振動数算出部と、前記複数の関節の前記自己固有振動成分と、前記慣性行列の対角要素とを用いて、前記複数の関節の粘性係数を算出する粘性係数算出部と、前記複数の関節の粘性係数が前記粘性係数算出部によって算出された前記複数の関節の粘性係数となるように、前記複数の関節の前記可変粘性係数要素の粘性係数を変更する粘性変更制御部と、を備えることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、リンクのうち予め定められたタスク位置に要求される剛性と、複数の関節を駆動する複数の回転駆動源の回転角度によって決定される係数行列とに基づいて、複数の関節の剛性閾値を算出するので、タスク位置に要求される剛性を満たすように、各関節の剛性閾値を求めることができる。
【００１２】
また、本発明によれば、複数の関節の剛性を表す行列と、複数の回転駆動源の回転角度によって決定された駆動部の慣性行列の逆行列との積を算出して得られた行列の対角要素の平方根を複数の関節の自己固有振動成分として用い、複数の関節の自己固有振動成分と、慣性行列の対角要素とを用いて、複数の関節の粘性係数を算出するので、関節の振動を抑制することができる。
【図面の簡単な説明】
【００１３】
【図１】駆動装置を示す図である。
【図２】本実施の形態の駆動部の関節駆動系の構成を示す模式図である。
【図３】制御部の機能ブロック図である。
【発明を実施するための形態】
【００１４】
本発明に係る駆動装置について、好適な実施の形態を掲げて添付の図面を参照しながら以下詳細に説明する。
【００１５】
図１は、駆動装置１０を示す図である。駆動装置１０は、駆動部１２と、駆動部１２を駆動する制御する制御部１４とを備え、駆動部１２は、複数の関節１６（１６1、１６2）と複数の連結部１８（１８1、１８2）を有するリンク２０を有する。複数の連結部１８は、関節１６を介して互いに接続されている。リンク２０の根本の関節１６1は基体１９側に設けられている。
【００１６】
図１に示すように、リンク２０のうち予め定められたタスク位置（本実施の形態では手先位置Ａとする）が移動した量をΔｘで表し、関節１６1が回転した回転角度をΔθ1で、関節１６2が回転した回転角度をΔθ2で表している。また、手先位置Ａの剛性をＫｘで表している。
【００１７】
なお、図１では、関節１６及び連結部１８を２個としたが、以下、本実施の形態では、関節１６及び連結部１８がｎ個として説明する。関節１６及び連結部１８がｎ個の場合は、各関節１６を１６iで表し、各連結部１８を１８iで表す。また、各関節１６iの回転角度をΔθiで表す。本実施の形態では、ｉは、ｉ＝１、２、・・・、ｎ、であり、関節１６の番号を表している。
【００１８】
図２は、本実施の形態の駆動部１２の関節駆動系３０の構成を示す模式図である。関節駆動系３０は、モータ（回転駆動源）３２と、駆動ギア３４と、従動ギア３６と、柔軟伝達部材３８とを有し、柔軟伝達部材３８を介して、モータ３２に関節１６が接続されている。
【００１９】
モータ３２は、いわゆる電動機であり、図示しない電源から電力が供給されることで、モータ３２に接続された出力軸３２ａを軸周りに回転させるトルクを発生する。この出力軸３２ａには駆動ギア３４が接続されている。なお、モータ３２には、モータ３２の回転角度を検出するロータリエンコーダ４０が接続されている。
【００２０】
従動ギア３６は、駆動ギア３４と噛み合うように構成され、駆動ギア３４の回転により従動ギア３６が回転する。このとき、従動ギア３６は、モータ３２の回転を減速して回転する。即ち、駆動ギア３４及び従動ギア３６は、減速機として機能する。
【００２１】
柔軟伝達部材３８は、可変剛性要素５０と、可変粘性係数要素５２と、剛性変更部５４と、粘性係数変更部５６と、ねじ軸５８、６０と、ナット６２、６４とを備える。このとき、可変剛性要素５０は、変位によって関節１６の回転方向の剛性が変化する非線形ばねであり、可変粘性係数要素５２は、変位によって関節１６の回転方向の粘性係数が変化する非線形ダンパである。
【００２２】
剛性変更部５４は、印加される電圧に応じて、ねじ軸５８を回転させる駆動源であり、粘性係数変更部５６は、印加される電圧に応じて、ねじ軸６０を回転させる駆動源である。なお、剛性変更部５４及び粘性係数変更部５６には、駆動するための図示しない電源から電力が供給されている。ナット６２は、ねじ軸５８が剛性変更部５４の駆動により回転することで、ねじ軸５８の軸方向に沿って図２の左右方向に移動し、ナット６４は、ねじ軸６０が粘性係数変更部５６の駆動により回転することで、ねじ軸６０の軸方向に沿って図２の左右方向に移動する。
【００２３】
このとき、剛性変更部５４及び粘性係数変更部５６は、印加される電圧が増加するとき、ナット６２、６４を右方向に移動させ、該電圧が減少する時、ナット６２、６４を左方向に移動させ、電圧が一定である時、ナット６２、６４の移動を停止させる。また、剛性変更部５４及び粘性係数変更部５６に電圧が印加されていないときは、ナット６２、６４は最も左側に移動される。
【００２４】
ナット６２が図２の右方向に移動することで、可変剛性要素５０を押圧して、当該移動した分だけ右方向に変位し、ナット６２が図２の左方向に移動することで可変剛性要素５０への押圧が軽減され、当該移動した分だけ左方向に変位する。また、ナット６４が図２の右方向に移動することで、可変粘性係数要素５２を押圧して、当該移動した分だけ右方向に変位し、ナット６４が図２の左方向に移動することで可変粘性係数要素５２への押圧が軽減され、当該移動した分だけ左方向に変位する。即ち、剛性変更部５４及び粘性係数変更部５６に印加される電圧に応じて、可変剛性要素５０の剛性及び可変粘性係数要素５２の粘性係数が変化する。
【００２５】
図２においては、「剛性」及び左右方向の「変位」に応じて可変剛性要素５０が力を作用し、「粘性係数」及び左右方向の「速度」に応じて可変粘性係数要素５２が力を作用するかのように図示されているが、実際には、可変剛性要素５０及び可変粘性係数要素５２は、関節１６の回転方向に対して各々の力を作用する。従って、可変剛性要素５０の剛性は、関節１６の回転方向における剛性を示し、可変粘性係数要素５２の粘性係数は、関節１６の回転方向における粘性係数を示している。剛性変更部５４に印加される電圧が高くなると、可変剛性要素５０の関節１６の回転方向における剛性は高くなり、粘性係数変更部５６に印加される電圧が高くなると、可変粘性係数要素５２の関節１６の回転方向における粘性係数が高くなる。この可変剛性要素５０及び可変粘性係数要素５２により、従動ギア３６から出力されるトルクが関節１６に伝達されて、関節１６が回転する。
【００２６】
ここで、図１に示すように、リンク２０がある姿勢で、複数の関節１６の回転方向における剛性を表す行列Ｋｒ（Ｎ・ｍ／ｒａｄ）によって支持されている場合であって、駆動部１２のリンク２０の手先位置Ａの剛性を表す行列をＫｘとし、手先位置Ａの変位量をΔｘとすると、手先位置Ａにかかる力Ｆは、以下の式で表すことができる。
【００２７】
【数１】
2014046417.tif 000003
【００２８】
ここで、ｆ1、ｆ2、・・・、ｆn、等は、手先位置Ａにかかる力Ｆの各方向の成分を表しており、例えば、ｘ軸方向、ｙ軸方向、ｘ軸の回転方向、ｙ軸の回転方向の成分等である。また、Δｘ1、Δｘ2、・・・、Δｘn、等は、手先位置Ａの変化量Δｘの各方向の成分を表しており、例えば、ｘ軸方向、ｙ軸方向の成分等である。この力Ｆの成分の数、手先位置Ａの変化量Δｘの成分の数は、任意に設定することができる。なお、行列Ｋｘの各要素をｋｘgh（ｇ、ｈ＝１、２、・・・、ｎ）とする。
【００２９】
また、関節１６の回転角度をΔθとすると、関節１６にかかるトルクτは、以下の関係式で表すことができる。この関節１６の回転角度Δθは、ロータリエンコーダ４０が検出した回転角度によって得ることができる。
【００３０】
【数２】
2014046417.tif 000004
【００３１】
ここで、τは、各関節１６iにかかるトルクτiをベクトル表示したものであり、Δθは、各関節１６iの回転角度Δθiをベクトル表示したものである。なお、行列Ｋｒの各要素をｋｒgh（ｇ、ｈ＝１、２、・・・、ｎ）とする。
【００３２】
また、Δｘと、Δθとは、ヤコビ行列（係数行列）Ｊを用いて、以下の関係式で表すことができる。このヤコビ行列Ｊは、手先位置Ａと、関節バネ変位平衡点である各関節１６を駆動する各モータ３２の回転角度に応じて決定されたものである。
【００３３】
【数３】
2014046417.tif 000005
【００３４】
また、関節１６にかかるトルクτと手先位置Ａにかかる力Ｆとは、ヤコビ行列Ｊの転置行列を用いて、以下の関係式で表すことができる。
【００３５】
【数４】
2014046417.tif 000006
【００３６】
よって、数１〜数４に示す関係式を用いて、以下の関係式を導くことができる。
【００３７】
【数５】
2014046417.tif 000007
【００３８】
このように、リンク２０の手先位置Ａに要求される剛性を表す行列Ｋｘから複数の関節１６に要求される必要最小限の剛性を表す行列Ｋｒを求めることができる。従って、リンク２０の手先位置Ａの剛性を、要求される剛性Ｋｘ以上とするためには、少なくとも、複数の関節１６の剛性を表す行列Ｋｍは、Ｋｍ≧Ｋｒでなければならない。なお、Ｋｍ≧Ｋｒとは、Ｋｍの各要素（剛性）ｋｍghが、対応するＫｒの各要素（剛性閾値）ｋｒgh以上であることを示す。つまり、各要素がＫｍgh≧Ｋｒghであればよい。
【００３９】
複数の関節１６の剛性を表す行列をＫｍとしたときに、複数の関節１６が回転すると、複数の関節１６にかかるトルクτは、以下の関係式で表すことができる。
【００４０】
【数６】
2014046417.tif 000008
【００４１】
数６から、関節１６1にかかるトルクτ1は、
τ1＝ｋｍ11×Δθ1＋ｋｍ12×Δθ2＋・・・＋ｋｍ1n×Δθn
となり、関節１６1にかかるトルクτ1は、関節１６1以外の他の関節１６iの回転角度Δθiで表される成分（２項目以降の成分）の影響を受けることになる。
【００４２】
また、同様に、関節１６2にかかるトルクτ2は、
τ2＝ｋｍ21×Δθ1＋ｋｍ22×Δθ2＋・・・＋ｋｍ2n×Δθn
となり、関節１６2にかかるトルクτ2は、関節１６2以外の関節１６iの回転角度Δθiで表される成分（１項目と３項目以降の成分）の影響を受けることになる。
【００４３】
このトルクτiにおいて、関節１６iの回転角度Δθi以外の成分を外乱と見做し、行列Ｋｍの対角要素ｋｍiiを抽出することで（行列Ｋｍの対角要素以外の要素を０にすることで）、該外乱を除去する。
【００４４】
従って、行列Ｋｍの対角要素であるｋｍ11が関節１６1の剛性となり、ｋｍ22が関節１６2の剛性となるというように、剛性Ｋｍの対角要素ｋｍiiが、関節１６iの剛性となる。この場合は、関節１６iにかかるトルクτiは、ｋｍii×Δθiで表される。
【００４５】
このように、各関節１６iの剛性を、剛性ｋｍiiとすることで、手先位置Ａに要求される剛性を満たすことができる。
【００４６】
なお、各関節１６iの剛性ｋｍiiは、関節１６iの機械剛性ｋiより高く設定することはできないので、各関節１６iの剛性ｋｍiiは、ｋi≧ｋｍii≧ｋｒiiの関係式を満たしていなければならない。この機械剛性ｋiは、各関節１６iの可変剛性要素５０の回転方向における最大剛性である。
【００４７】
また、関節１６の運動方程式は、下記の関係式で表すことができる。
【００４８】
【数７】
2014046417.tif 000009
【００４９】
Ｍは、慣性行列であり、駆動部１２の慣性モーメント（イナーシャ）を表したものであり、ｄｄΔθは、Δθを２階微分したものであり、回転角加速度を示す。数７は、以下のように変形することができる。なお、Ｍ-1・Ｋｍの各要素をｓgh（ｇ、ｈ＝１、２、・・・、ｎ）とする。なお、慣性行列Ｍは、各関節１６を駆動する各モータ３２の回転角度によって決定される。
【００５０】
【数８】
2014046417.tif 000010
【００５１】
数８から、関節１６iの回転角加速度ｄｄΔθ1は、
ｄｄΔθ1＝−（ｓ11×Δθ1＋ｓ12×Δθ2＋・・・＋ｓ1n×Δθn）
となり、関節１６1の回転角加速度ｄｄΔθ1は、関節１６i以外の他の関節１６iの回転角加速度ｄｄΔθnで表される成分（２項目以降の成分）の影響を受けることになる。
【００５２】
また、同様に、関節１６2の回転角加速度ｄｄΔθ2は、
ｄｄΔθ2＝−（ｓ21×Δθ1＋ｓ22×Δθ2＋・・・＋ｓ2n×Δθn）
となり、関節１６2の回転角加速度ｄｄΔθ2は、関節１６2以外の他の関節１６2の回転角加速度ｄｄΔθnで表される成分（１項目と３項目以降の成分）の影響を受けることになる。
【００５３】
このｄｄΔθiにおいて、関節１６iの回転角度Δθi以外の成分を外乱と見做し、Ｍ-1・Ｋｍの対角要素ｓiiを抽出することで、該外乱を除去する。従って、各関節１６iの自己固有振動成分（自己固有振動数）ωiは、以下の関係式で表すことができる。
【００５４】
【数９】
2014046417.tif 000011
【００５５】
従って、各関節１６iの粘性係数ｄiは、以下の数式で表すことができる。なお、ｍiiは、慣性行列Ｍの対角要素であり、ζは、減衰比である。
【００５６】
【数１０】
2014046417.tif 000012
【００５７】
従って、粘性係数ｄ1が各関節１６1の粘性係数となり、粘性係数ｄ2が各関節１６2の粘性係数となるというように、粘性係数ｄiが各関節１６iの粘性係数となる。
【００５８】
このように、各関節１６iの粘性係数を、粘性係数ｄiとすることで、決定された剛性ｋｍiiを有する関節１６iの振動を抑制することができる。
【００５９】
図３は、制御部１４の機能ブロック図である。制御部１４は、ヤコビ行列決定部８０、剛性閾値算出部８２、剛性決定部８４、剛性変更制御部８６、慣性行列決定部８８、固有振動数算出部９０、粘性係数算出部９２、及び粘性変更制御部９４を備える。
【００６０】
ヤコビ行列決定部８０は、各関節１６iのロータリエンコーダ４０が算出したモータ３２の回転角度Δθiと、手先位置Ａとに応じてヤコビ行列Ｊを決定する。手先位置Ａは、手先位置Ａにジャイロセンサ等を設けて手先位置Ａを検出してもよいし、各関節１６iのΔθiから手先位置Ａを算出してもよい。このヤコビ行列Ｊの算出については周知技術なので説明を割愛する。ヤコビ行列決定部８０は、決定したヤコビ行列Ｊを剛性閾値算出部８２に出力する。
【００６１】
剛性閾値算出部８２は、手先位置Ａに要求される剛性を表すＫｘと、ヤコビ行列決定部８０が決定したヤコビ行列Ｊとに基づいて、複数の関節１６の剛性閾値を表すＫｒを算出する。詳しくは、剛性閾値算出部８２は、数５を用いて、複数の関節１６の剛性を表すＫｒを算出する。行列Ｋｒの各要素Ｋｒghが剛性閾値となる。
【００６２】
剛性決定部８４は、行列Ｋｒ以上の行列Ｋｍを決定する。つまり、剛性決定部８４は、行列Ｋｍの各要素ｋｍghが、対応する行列Ｋｒの各要素ｋｒgh以上となるように、行列Ｋｍを決定する。剛性決定部８４は、決定した行列Ｋｍを、剛性変更制御部８６及び固有振動数算出部９０に出力する。
【００６３】
剛性変更制御部８６は、複数の関節１６の剛性変更部５４に、決定した行列Ｋｍに対応する電圧を印加することで、複数の関節１６の可変剛性要素５０の剛性を変更する。本実施の形態では、行列Ｋｍの対角成分以外の要素ｋｍghは、外乱要素と見做すので、詳しくは、剛性変更制御部８６は、行列Ｋｍの対角要素ｋｍiiを抽出し、各関節１６iの剛性が、決定した行列Ｋｍの対角要素ｋｍiiとなるように、各関節１６iの剛性変更部５４に電圧を印加する。
【００６４】
なお、決定した行列Ｋｍの対角要素ｋｍiiは、ｋi≧ｋｍii≧ｋｒii、の関係式を満たすように設定される。但し、ｋiは、各関節１６iの機械剛性、つまり、各関節１６iの可変剛性要素５０の最大剛性である。
【００６５】
また、慣性行列決定部８８は、各関節１６iのロータリエンコーダ４０が算出したモータ３２の回転角度Δθiに基づいて、駆動部１２の慣性モーメントを表す慣性行列Ｍを算出する。この慣性行列Ｍの算出については周知技術なので説明を割愛する。慣性行列決定部８８は、決定した慣性行列Ｍを固有振動数算出部９０及び粘性係数算出部９２に出力する。
【００６６】
固有振動数算出部９０は、慣性行列Ｍの逆行列Ｍ-1と行列Ｋｍの積を算出して得られた行列の対角要素ｓiiの平方根を各関節１６iの自己固有振動成分ωiとして用いる。詳しくは、固有振動数算出部９０は、決定した複数の関節１６の剛性を表す行列Ｋｍと、慣性行列Ｍとに基づいて、数９を用いて各関節１６iの自己固有振動成分ωiを算出する。固有振動数算出部９０は、算出した各関節１６iの自己固有振動成分ωiを粘性係数算出部９２に出力する。
【００６７】
なお、固有振動数算出部９０は、行列Ｋｍのうち、対角要素ｋｍii以外の要素を０にした行列と慣性行列Ｍの逆行列Ｍ-1との積を算出して得られた行列を求めるようにしてもよい。この場合であっても、得られる行列の対角要素は、行列Ｋｍと逆行列Ｍ-1との積を算出して得られた行列の対角要素ｓiiと同じ値になるからである。
【００６８】
粘性係数算出部９２は、各関節１６iの自己固有振動成分ωiと、慣性行列Ｍの対角要素Ｍiiとを用いて、各関節１６iの粘性係数ｄiを算出する。詳しくは、数１０を用いて、各関節１６iの粘性係数ｄiを算出する。粘性係数算出部９２は、算出した各関節１６iの粘性係数ｄiを粘性変更制御部９４に出力する。
【００６９】
粘性変更制御部９４は、各関節１６iの粘性係数変更部５６に、算出した粘性係数ｄiに対応する電圧を印加することで、複数の関節１６の可変粘性係数要素５２の粘性係数を変更する。詳しくは、粘性変更制御部９４は、各関節１６iの粘性係数が、算出した粘性係数ｄiとなるように、各関節１６iの粘性係数変更部５６に電圧を印加する。
【００７０】
このように、リンク２０のうち予め定められた手先位置Ａに要求される剛性Ｋｘと、複数の関節１６を駆動する複数のモータ３２の回転角度Δθによって決定される慣性行列Ｍとに基づいて、各関節１６iの剛性閾値ｋｒiiを算出するので、手先位置Ａに要求される剛性を満たすように、各関節１６iの剛性閾値ｋｒiiを求めることができる。
【００７１】
また、複数の関節１６の剛性を表す行列Ｋｍと、複数のモータ３２の回転角度Δθによって決定された駆動部１２の慣性行列Ｍの逆行列との積を算出して得られた行列の対角要素ｓiiの平方根を各関節１６iの自己固有振動成分ωiとして用い、各関節１６iの自己固有振動成分ωiと、慣性行列Ｍの対角要素ｍiiとを用いて、各関節１６iの粘性係数ｄiを算出するので、関節１６の振動を抑制することができる。
【００７２】
以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に、多様な変更又は改良を加えることが可能であることが当業者に明らかである。その様な変更又は改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。
【符号の説明】
【００７３】
１０…駆動装置                          １２…駆動部
１４…制御部                            １６…関節
１８…連結部                            ２０…リンク
３０…関節駆動系                        ３２…モータ
３４…駆動ギア                          ３６…従動ギア
３８…柔軟伝達部材                      ４０…ロータリエンコーダ
５０…可変剛性要素                      ５２…可変粘性係数要素
５４…剛性変更部                        ５６…粘性係数変更部
８０…ヤコビ行列決定部                  ８２…剛性閾値算出部
８４…剛性決定部                        ８６…剛性変更制御部
８８…慣性行列決定部                    ９０…固有振動数算出部
９２…粘性係数算出部                    ９４…粘性変更制御部
【図１】
2014046417.tif 000013
【図２】
2014046417.tif 000014
【図３】
2014046417.tif 000015