JP2006055927A

JP2006055927A - 関節駆動装置および関節駆動装置の制御方法

Info

Publication number: JP2006055927A
Application number: JP2004238572A
Authority: JP
Inventors: Hironori Yamataka; 大乗山高
Original assignee: Sharp Corp
Current assignee: Sharp Corp
Priority date: 2004-08-18
Filing date: 2004-08-18
Publication date: 2006-03-02

Abstract

【課題】関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る関節駆動装置を提供する。
【解決手段】装置のモータ側にエンコーダを接続する一方、関節部側にポテンショメータを接続し、ポテンショメータおよびエンコーダの値を読み込んでメモリに記憶した後（Ｓ２０１、Ｓ２０２）、記憶したポテンショメータの値を関節角度θ_Ｐに、記憶したエンコーダ３の値を関節角度θ_Ｅにそれぞれ変換し、その角度差θ_Ｐ−θ_Ｅを計算する(Ｓ２０３)。角度差と関節部の負荷トルクとの関係を示す負荷トルクデータを予め記憶部に記憶し、その負荷トルクデータと、Ｓ２０３で算出された角度差とに基づいて、関節部の負荷トルクを推定する（Ｓ２０４）。
【選択図】図６

Description

本発明は、関節駆動装置および関節駆動装置の制御方法に関し、特に、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材とを備える関節駆動装置および関節駆動装置の制御方法に関する。

関節駆動装置として、たとえば、ロボットハンドを例に挙げることができる。人の手に合わせて作られた道具類を人の手が利用するように器用に利用するために、人の手のように多関節フィンガを複数組み合わせたロボットハンドの開発が進められている。

実際に人の手のような器用な動作をロボットハンドに行なわせるためには、複数の関節部を独立して駆動するための機構が必要となる。しかしながら、たとえばモータ等で構成されるアクチュエータ本体を直接関節部に設置した場合、フィンガのリンクの重量が増加するとともに、フィンガ自体のサイズが大きくなる等の問題が生じる。

このため、現在では、人の手のサイズに近いコンパクトなロボットハンドを構成するために掌部分にモータ等のアクチュエータを集中配置し、アクチュエータ本体から各関節部に対してリンク機構により動力を伝達する方法や、アクチュエータと関節部とのそれぞれにプーリを取付け、プーリ同士をワイヤ、タイミングベルト等の弾性を有する動力伝達部材で連結して用いて動力を伝達する方法が考えられている。

前者の方法を採用した場合には、フィンガの各関節角度を十分にとる必要性があるために機構が複雑になる傾向がある。また、リンク機構の性質上、トルクを平均して伝達することが難しい等の問題がある。

一方、後者の方法であれば、タイミングベルトの歯飛びを起こさせないための工夫や、ワイヤテンションを一定に保つためのメンテナンス作業が必要になるものの、前者の方法に比較して動力伝達を単純化することができる。このため、設計が容易である。さらに、トルク伝達の異方性等がないため、取り扱いも容易である。

このような理由から、ロボットハンドに代表される小型の関節駆動装置の駆動系としては、後者の方法、すなわち、アクチュエータと、関節部側に取付けたプーリとの間をワイヤあるいはタイミングベルトで結んで動力を伝達する方法を用いることが多い。

ロボットハンドが物体を安定に把持し続けるためには、各指の接触力のバランスをとる必要がある。各指の接触力を調整するために、一般的には、各フィンガの関節部のトルクを所定の値に制御するフィードバック系あるいはサーボ系を使用する。

ところが、ロボットハンド等の関節駆動装置の場合には、アクチュエータを構成するギア等の動力駆動機構部分のがたつきでバックラッシュが生じる。したがって、関節部を目標角度位置に正確に回動させるためには、関節部側の状態、およびバックラッシュの大きさを把握する必要がある。

また、ワイヤあるいはタイミングベルト駆動型の関節駆動装置の場合、フレームの撓みの他、動力伝達部材が弛むことによるコンプライアンス（compliance）性を有するため、アクチュエータ側のトルク出力を求めることと同時に、実際の関節部側のトルク情報を取得することが重要である。

特許文献１および非特許文献１には、関節部のトルクをワイヤ張力から推定する方法が開示されている。前者にはプーリ両端のワイヤに押し当てたカンチレバーの撓み量からワイヤ張力を測定する方法が開示されており、後者には張力差動型トルクセンサを用いてワイヤ張力を測定する方法が開示されている。いずれもプーリ両端のワイヤ張力の差分から関節トルクを推定する方法であるといえる。

特許文献１に開示された関節駆動装置の概略構成を図８（Ａ）に示す。また、非特許文献１に開示された関節駆動装置の概略構成を図８（Ｂ）に示す。

（Ａ）、（Ｂ）のいずれも、モータ軸Ｓ１に連結された主動側プーリＰ１と、関節部Ｊの関節軸Ｓ２に連結された従動側プーリＰ２との間に、ワイヤＷが掛け渡されており、主動側プーリＰ１の回転がワイヤＷによって従動側プーリＰ２に伝達されて、リンクＬが連結された関節軸Ｓ２がメカリミットＭＬ１およびメカリミットＭＬ２によって規制される範囲で回動する。

（Ａ）に示される関節駆動装置の場合には、板ばね状の２つのカンチレバーＣＬがフレームＦに取付けられている。また、カンチレバーＣＬの先端にはワイヤＷに張力を与えるテンションプーリＰ３が設けられている。さらに、カンチレバーＣＬの両側には図示しない歪ゲージが設けられる。（Ａ）に示される関節駆動装置では、双方の歪ゲージの出力に基づいて、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力を測定することにより関節部Ｊのトルクを推定する。

一方、（Ｂ）に示される関節駆動装置の場合には、ワイヤＷを挟んでテンションを与えているテンションプーリＰ４が張力計測用梁Ｂを介してフレームＦに取付けられている。（Ｂ）に示される関節駆動装置では、張力計測用梁Ｂに取付けた歪ゲージにより、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力の差分を測定し、その測定結果に基づいて、関節部Ｊのトルクを推定する。

なお、（Ａ）および（Ｂ）に示されたトルクの推定手法は原理的には動力伝達部材がベルトであっても同様に利用することが可能である。
特開平６−８１７８号公報金子真人，横井一仁，鈴木夏夫，谷江和雄，「プーリ・ワイヤ駆動系におけるトルクセンシングとトルク制御」，日本ロボット学会誌，１９８９年２月，第７巻，第１号，ｐ．６２−ｐ．７０

関節駆動装置をデジタル制御するためには、アナログ情報をデジタル変換してデジタル量として処理する必要がある。しかしながら、一般に、アナログ情報をデジタル変換するＡ／Ｄ変換回路は純粋なデジタル回路に比較して大きいために、装置へ組み込んだときの占有スペースが大きくなる。また、アナログ入力に応じてＡ／Ｄ変換回路を設計する必要があり、設計および製作が完了した後にアナログ入力を増やすことは困難になるので、回路設計の柔軟性が乏しくなる。

したがって、関節駆動装置をコンパクト化し、かつ、回路設計の柔軟性を高めるためには、極力、アナログ情報の入力を避ける必要がある。

ところが、従来においては、たとえば、図８に示されるように、歪ゲージからのアナログ入力を必要としていた。特に、図８（Ａ）の場合には、それぞれの関節についてプーリ両側のワイヤに押し付けたカンチレバーにそれぞれ２枚の歪みゲージが必要であるため、ブリッジ回路を組んで２個のアナログ入力が必要になる。また、張力作動型トルクセンサを用いる図８（Ｂ）の場合であっても、２枚の歪ゲージを用いるために、ブリッジ回路を組んで１個のアナログ入力が各関節に必要になる。

本発明は、上記のような課題を解決するためになされたものであって、歪ゲージ等からの情報の入力を必要とすることなく、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る関節駆動装置の提供を目的とする。

この発明のある局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備える。

この発明の他の局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備える。

この発明の他の局面に係る関節駆動装置は、関節部を正逆両方向に回動させるための関節側回転部材と、前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、前記関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備える。

本発明によれば、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。これは制御回路を小型化、省略化することに寄与するので、コンパクトかつ高性能な関節部駆動装置を容易かつ安価に構成することができるようになる。

本発明の実施の形態について図面を参照しながら詳細に説明する。図１は本発明に係る関節駆動装置１の平面図である。また、図２は、関節駆動装置１を図１の矢印Ｌ方向から見た図である。なお、図１および図２では、関節駆動装置１の制御を行なう制御回路等、電気的回路の図示は省略している。

関節駆動装置１は、ＤＣモータであるモータ４を駆動源として、関節部１１に連結されたリンク１４を回動させる。関節駆動装置１のフレーム１３には、遊星ギアから成るギア５によって回転速度が１／２５６に減速されたモータ４の駆動力を伝達するモータ軸９が挿通されている。モータ軸９には、主動側プーリ６が連結されている。モータ４と、ギア５と、モータ軸９と、主動側プーリ６とにより、動力伝達機構の一例となる回転型アクチュエータが構成される。

関節部１１の関節軸１０には、従動側プーリ７が連結されている。従動側プーリ７はリンク１４に固定されており、関節軸１０を軸として回転する。

主動側プーリ６と従動側プーリ７との間には、弾性を有する動力伝達部材の一例となるワイヤ１２が８の字状に掛け渡されている。なお、弾性を有する動力伝達部材として、ワイヤに代えて、タイミングベルトを採用してもよい。

フレーム１３には、リンク１４の回動角を規制する規制部材の一例となるメカリミット３０，４０が取付けられている（図２では取付け構造の図示を省略）。リンク１４が矢印Ｒ１の方向（正方向）、すなわち時計回りに所定角度回動すると、リンク１４がメカリミット３０に当たって、それ以上の回動が規制される。同様に、リンク１４が矢印Ｒ２の方向（逆方向）、すなわち反時計回りに回ると、リンク１４がメカリミット４０に当たってそれ以上の回動が規制される。

モータ４には、モータ４の回転角を検出するためのエンコーダ３が接続されている。このエンコーダ３は、たとえば、インクリメンタル型の相対角度検出センサを構成するパルスエンコーダである。エンコーダ３は、モータ４が１回転する間に所定数のパルスを出力する。関節部１１には、関節軸１０においてフレーム１３とリンク１４とのなす角度を検出するためのポテンショメータ８が接続されている。以下、関節軸１０においてフレーム１３とリンク１４とのなす角度を“関節角度”という。ポテンショメータ８は、たとえば、関節角度が０度〜３２０度変化するのに応じて、出力電圧が０Ｖ〜２．５Ｖまで変化するように電気的に接続されている。

図３は、関節駆動装置１の電気的構成要素をブロック図として表したものである。関節駆動装置１は、図１および図２では図示が省略されている制御回路２、モータサーボ回路１５、モータドライバ１６、およびＡ／Ｄ変換回路１７をさらに備えている。

制御回路２は、ＣＰＵ２０、ＲＡＭ２２、ＲＯＭ２１を備えるマイクロコンピュータを含んでいる。ＲＯＭ２１には、ＣＰＵ２０を動作させる制御プログラムを記憶する領域とは別に、推定用データ記憶領域２３が設けられている。この推定用データ記憶領域２３には、後述する関節部１１への負荷トルクを推定するためのデータや、ギア５等の動力機構部分のがたつきで生じるバックラッシュの大きさを推定するためのデータが記憶されている。

制御回路２からモータサーボ回路１５へは、エンコーダ３の制御目標値が入力される。さらに、モータサーボ回路１５には、エンコーダ３から出力されるパルス信号が入力される。モータサーボ回路１５は、エンコーダ３のパルス信号をカウントしてモータ４の回転状況を特定し、モータドライバ１６に入力するＰＷＭ（Pulse Width Modulation）波形のデューティー比を変化させて、制御回路２から指令を受けたエンコーダ３の制御目標値相当の角度だけモータ４を駆動する。また、モータサーボ回路１５は、エンコーダ３のパルス信号を制御回路２へ入力する。モータ４は、モータドライバ１６からの駆動信号に応答して回転する。すなわち、モータ４、モータサーボ回路１５、およびモータドライバ１６は、モータ４の位置制御サーボ系を構成している。

モータ４の回転は、図１および図２に示したギア５、モータ軸９、主動側プーリ６、および動力伝達部材であるワイヤ１２により、関節部１１に伝達される。これにより、関節部１１において、リンク１４（図１、図２参照）が回動する。また、リンク１４の回動による関節角度の変化は、ポテンショメータ８の出力電圧を変化させる。この出力電圧は、Ａ／Ｄ変換回路１７によりアナログ情報からデジタル情報に変換されて制御回路２に入力される。制御回路２は、このデジタル情報に基づいて、関節角度を特定する。

次に、関節角度の算出手順について説明する。ここでは、関節駆動装置１の構成が、たとえば、以下のとおりであるものとする。

・プーリの直径比：主動側プーリ６（直径１５ｍｍ）／従動側プーリ７＝３／２
・ワイヤ長：５０ｍｍ
・Ａ／Ｄ変換回路１７の分解能：１２ビット
・ギア５による減速割合：１／２５６
・エンコーダ３のパルス発生割合：１６パルス／１回転
［エンコーダ３のカウント値から関節角度を算出する手順］
主動側プーリ６と従動側プーリ７との直径比が３：２であり、ギア５によるモータ４の回転速度の減速割合が１／２５６であるため、ワイヤ１２およびギア５に弾性やバックラッシュがない理想的な回転伝達状態においては、モータ４が１回転するごとに、関節軸１０は１／３８４回転することになる（（２／３）×（１／２５６））。

モータ４に接続されたエンコーダ３は、モータ４が１回転する間に１６パルスをカウントアップするので、関節角度を基準とすれば、エンコーダ３の分解能は、５．８６×１０^−２度となる（（１／３８４）×（１／１６）×３６０度）。

関節部１１の原点位置（関節角度＝０度）におけるエンコーダ３のカウント値をｎ_Ｅ０とし、ある時点でのエンコーダ３のカウント値をｎ_Ｅとすると、式１を用いることによって、エンコーダ３のカウント値ｎ_Ｅから関節角度θ_Ｅを算出できる。なお、式１においてＣ_Ｅはエンコーダ３のカウント値を関節角度に変換する係数である。この係数は、エンコーダ３の分解能により定まるため、たとえば、５．８６×１０^−２度/パルスである。

（式１） θ_Ｅ＝Ｃ_Ｅ(ｎ_Ｅ−ｎ_Ｅ０)
［ポテンショメータ８の出力値から関節角度を算出する手順］
ポテンショメータ８に接続されるＡ／Ｄ変換回路１７の分解能が１２ビットであるため、ポテンショメータ８による関節角度の分解能は８．７８×１０^−２度である（３６０度／２^１２）。

ポテンショメータ８のＡ／Ｄ変換後の値をｎ_Ｐ、関節部１１の原点位置におけるポテンショメータ８のカウント値をｎ_Ｐ０とすると、式２を用いることによって、ポテンショメータ８の出力値から関節角度θ_Ｐを計算できる。なお、式２において、Ｃ_ＰはＡ／Ｄ変換回路１７から出力されるパルス数を関節角度に変換する係数である。この係数は、ポテンショメータ８による関節角度の分解能により定まるため、たとえば、８．７８×１０^−２度/パルスである。

（式２） θ_Ｐ＝Ｃ_Ｐ(ｎ_Ｐ−ｎ_Ｐ０)
次に、関節駆動装置１の制御回路２が実行する各種の制御について、図４〜図６を参照して説明する。

図４は、初期化処理を説明するためのフローチャートである。この初期化処理は、エンコーダ３およびポテンショメータ８の出力値が正常値であるか否かをチェックするとともに、ポテンショメータ８に基づく式２の演算式のパラメータを補正し、さらには、関節駆動装置１の動力駆動機構のバックラッシュを推定するための処理である。

まず、内側に関節が回る方向、すなわち、リンク１４が図２に示したＲ１方向に回転するように、モータ４を駆動させる（Ｓ１）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもメカリミット３０にリンク１４が当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。

次に、ポテンショメータ８の出力値をデジタル変換した値ｎ_Ｐ＋を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ２)。

次に、前記式１を用いて、入力値ｎ_Ｐ＋に基づいた関節角度θ_Ｐ＋を算出する（Ｓ３）。次に、算出した関節角度θ_Ｐ＋が予め定めた正常範囲に収まっているか否かを判断する（Ｓ４）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θ_Ｐ＋が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θ_Ｐ＋の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ４の判断を行なう。

算出した関節角度θ_Ｐ＋が正常な範囲に収まっていないならば（Ｓ４でＮＯ）、初期化処理を中断する。このような減少が生じる原因としては、ポテンショメータ８に繋がる電気系統の切断あるいは故障、ポテンショメータ８の取付け位置の機械的なずれ、あるいは、モータ４の故障等が考えられる。

Ｓ４において、算出した関節角度θ_Ｐ＋が正常な範囲に収まっているならば（Ｓ４でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎ_Ｅ＋を入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ５)。

次に、外側に関節が回る方向、すなわち、リンク１４が図２に示したＲ２方向に回転するように、Ｓ１と逆方向にモータ４を駆動させる（Ｓ６）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもリンク１４がメカリミット４０に当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。

次に、ポテンショメータ８の出力値をデジタル変換した値ｎ_Ｐ−を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ７)。

次に、前記式２を用いて、入力値ｎ_Ｐ−に基づいた関節角度θ_Ｐ−を算出する（Ｓ８）。次に、算出した関節角度θ_Ｐ−が予め定めた正常範囲に収まっているか否かを判断する（Ｓ９）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θ_Ｐ−が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θ_Ｐ−の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ９の判断を行なう。

算出した関節角度θ_Ｐ−が正常な範囲に収まっていないならば（Ｓ９でＮＯ）、初期化処理を中断する。一方、算出した関節角度θ_Ｐ−が正常な範囲に収まっているならば（Ｓ９でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎ_Ｅ−を入力し（図３参照）、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ１０)。

次に、Ｓ５およびＳ１０において入力されたエンコーダ３の値に基づいて、エンコーダ３の値が正常に変化しているか否かを判断する（Ｓ１１）。具体的には、Ｓ５において取得したエンコーダ３のパルス数ｎ_Ｅ＋とＳ１０において取得したエンコーダ３のパルス数ｎ_Ｅ−との差分ｎ_Ｅ＋−ｎ_Ｅ−が、リンク１４がメカリミット３０とメカリミット４０との間を回動する間にエンコーダ３によってカウントすることが期待されるパルス数に対して一定の範囲内に収まっているか否かを判別する。たとえば、制御回路２のＲＯＭ２３内には、エンコーダ３の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ１１の判断を行なう。

エンコーダ３のパルス数の差分(ｎ_Ｅ＋−ｎ_Ｅ−)が、期待される値の範囲内に無い場合（Ｓ１１でＮＯ）には、初期化処理を異常終了する。このような現象が起きる原因としてはエンコーダ３に繋がる電気系系統の切断、あるいはエンコーダ３やそれに関わる回路の故障が考えられる。

エンコーダ３のパルス数の差分が期待される値の範囲内にある場合（Ｓ１１でＹＥＳ）には、ギア等の動力駆動機構部分のがたつきによるバックラッシュ成分とワイヤ１２のコンプライアンス成分とによって生じる、アクチュエータ側と関節部側との間の角度差αを算出する(Ｓ１２)。

このバックラッシュ成分およびコンプライアンス成分が存在することによって、関節部１１が固定されている場合であってもモータ４が若干回転し、逆にモータ４の駆動が停止している場合であっても関節部１１が若干回転してしまう。そのため、アクチュエータ側の制御上の角度から計算で求められる関節角度と、関節部側の実際の関節角度との間で角度差αが生じる。この角度差αの大きさを特定することは、動力伝達系の制御の正確性を高めるために重要である。角度差αのうち、特にバックラッシュ成分に起因するものを、無負荷時のバックラッシュα_０という。

ここで、Ｓ１２において制御回路２が実行する処理を詳細に説明する。

まず、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれでエンコーダ３から入力された値ｎ_Ｅ＋(Ｓ５で入力)，ｎ_Ｅ−(Ｓ１０で入力)に基づいて、前記式１を用いて関節角度を算出する。ｎ_Ｅ＋に基づいて算出された関節角度をθ_Ｅ＋とし、ｎ_Ｅ−に基づいて算出された関節角度をθ_Ｅ−とする。

次に、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれのポテンショメータ８の出力値（デジタル値）ｎ_Ｐ＋(Ｓ２で入力)，ｎ_Ｐ−(Ｓ７で入力)に基づいてＳ３およびＳ８ですでに算出されている関節角度θ_Ｐ＋およびθ_Ｐ−を読み出す。

最後に、「（θ_Ｅ＋−θ_Ｅ−）−(θ_Ｐ＋−θ_Ｐ−)」を実行することによって、角度差αを算出する。すなわち、角度差α＝（θ_Ｅ＋−θ_Ｅ−）−(θ_Ｐ＋−θ_Ｐ−)である。

ところで、図４に示される初期化処理中のエンコーダ３による関節角度換算値の変化量（θ_Ｅ＋−θ_Ｅ−）と、ポテンショメータ８による関節角度換算値の変化量(θ_Ｐ＋−θ_Ｐ−)との角度差αは、式３に示すように、バックラッシュα_０の関数と考えることができる。

（式３） α(α_０)＝（θ_Ｅ＋−θ_Ｅ−）−(θ_Ｐ＋−θ_Ｐ−)
モータ４に一定電圧Ｖ１を連続的に印加して、リンク１４をメカリミット３０または４０に押し付ける場合、関節角度換算値の変化量の差αはバックラッシュα_０について単調増加になる。したがって電圧Ｖ１下におけるα_０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα_０を算出できる。

関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたα_０とαとの関係を示すバックラッシュデータが記憶されている。初期化処理のＳ１２では、さらに、推定用データ記憶領域２３からバックラッシュデータが読出されて、算出した角度差αに対応するバックラッシュα_０が算出される。

Ｓ１２でバックラッシュの推定処理が終了した後、θ_Ｐ＋，θ_Ｐ−と、メカリミット３０，４０にリンク１４が接している時の機械的にあるべき角度θ_＋，θ₋との比較により、ポテンショメータ入力値を関節角度に変換するために用いる前記式２のパラメータＣ_Ｐ，ｎ_Ｐ０の補正を行なう（Ｓ１３）。

具体的には、以下の式４および式５に従う演算をして、パラメータＣ_Ｐ，ｎ_Ｐ０を補正する。

（式４）Ｃ_Ｐ＝（θ_＋−θ₋）／（ｎ_Ｐ＋−ｎ_Ｐ−）
（式５）ｎ_Ｐ０＝ｎ_Ｐ＋−（θ_＋／Ｃ_Ｐ）
以降、パラメータＣ_Ｐおよびｎ_Ｐ０は、変更せず、固定値とする。

なお、式４および式５は、既知の（θ_＋，ｎ_Ｐ＋）および（θ₋，ｎ_Ｐ−）の各々を、前記式２「θ_Ｐ＝Ｃ_Ｐ(ｎ_Ｐ−ｎ_Ｐ０)」に代入して、
θ_＋＝Ｃ_Ｐ(ｎ_Ｐ＋−ｎ_Ｐ０)
θ₋＝Ｃ_Ｐ(ｎ_Ｐ−−ｎ_Ｐ０)
とし、これを連立方程式として、Ｃ_Ｐおよびｎ_Ｐ０を導出するものである。

たとえば、伝達機構がマニプレータやロボットフィンガに搭載され機構の周囲をケース等でシールドされた状態であっても本実施の形態の手法は適用可能である。つまり、上記のような初期化処理を制御回路２が実行することにより、ワイヤ１２を直接観測できない条件下であっても簡易に伝達系のバックラッシュを推定することができるようになるので、メンテナンスが容易である。また、本実施の形態によれば、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、伝達系のバックラッシュを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。

次に、図５を用いて、位置制御サーボ補正処理を説明する。この処理は、関節角度が所望の値になるように装置を制御するための処理である。

まず、ポテンショメータ８の値ｎ_Ｐを入力し、メモリ（ＲＡＭ２２）に記憶し、入力値に基づいて、前記式２によって関節角度θ_Ｐを算出する（Ｓ１０１）。関節部１１とポテンショメータ８は直接接続してあるので、θ_Ｐは実際の関節角度θを正確に反映している。なお、Ｓ１０１では、図４の初期化処理のＳ１３において補正されたパラメータＣ_Ｐおよびｎ_Ｐ０が用いられる。

次に、エンコーダ３のカウント値ｎ_Ｅを入力し、メモリ（ＲＡＭ２２）に記憶する（Ｓ１０２）。エンコーダ３と関節部１１との間には動力伝達系(ギア５およびワイヤ１２)が存在することから、エンコーダ３のカウント値ｎ_Ｅから前記式１を用いて算出した関節角度θ_Ｅは、実際の関節角度θを正確に反映していない。したがって、単にエンコーダ３のパルスカウントのみに基づいてサーボ形を構成した場合、リンク１４を目標の関節角度θの位置に正確に制御することができない。

そこで式６の計算により、エンコーダ３のパルスカウント原点を、初期値ｎ_Ｅ０から関節角度制御の回毎（ｉ回毎）に逐次変更して、ｎ_Ｅ０ ^ｉに補正する(Ｓ１０３)。

（式６）ｎ_Ｅ０ ^ｉ＝（１−ｋ）ｎ_Ｅ０ ^ｉ−１＋ｋ（ｎ_Ｅ−θ_Ｐ／Ｃ_Ｅ）
（ただし、ｉは自然数であり、ｎ_Ｅ０ ^０＝ｎ_Ｅ０である）
式６において、θ_Ｐ／Ｃ_Ｅは、実際の関節角度θ_Ｐに対応するエンコーダ３のパルスカウント値である。したがって、ｎ_Ｅ−θ_Ｐ／Ｃ_Ｅは、前回（ｉ−１回目）のエンコーダ３のパルスカウント値ｎ_Ｅと、実際にカウントされるべきエンコーダ３のパルスカウント値θ_Ｐ／Ｃ_Ｅとの差、すなわち、今回（ｉ回目）の制御に際して、補正すべき原点の誤差である。

理論上は、この誤差（ｎ_Ｅ−θ_Ｐ／Ｃ_Ｅ）を、前回のエンコーダ３のパルスカウント原点ｎ_Ｅ０ ^ｉ−１に加算することによって、次回の制御に用いるパルスカウント原点を求めることができる。

しかしながら、このようなパルスカウント原点の補正処理が毎回行なわれることで処理Ｓ１０３の実行周期が大きくなると、一度の変更によるパルスカウント原点ｎ_Ｅ０の変化が大きくなり、制御系の振動が生じやすくなる。そこで、本実施の形態では、１以下の比例ゲインであるｋを式６に示すように採用し、ゲインｋを適切に設定して振動を抑制している。

以上の計算が終わった後、式７によって、次回の関節角度目標値θ^ｉに制御するためのエンコーダ制御目標値ｎ_Ｅ ^ｉを算出する(Ｓ１０４)。

（式７）ｎ_Ｅ ^ｉ＝ｎ_Ｅ０ ^ｉ＋θ^ｉ／Ｃ_Ｅ
この式７は、前記式１の「θ_Ｅ＝Ｃ_Ｅ(ｎ_Ｅ−ｎ_Ｅ０)」から変形した式「ｎ_Ｅ＝ｎ_Ｅ０＋θ_Ｅ／Ｃ_Ｅ」を応用したものである。

最後に、Ｓ１０４において算出したエンコーダ制御目標値ｎ_Ｅ ^ｉをモータサーボ回路１５に設定する（Ｓ１０５）。なお、モータサーボ回路１５は、モータドライバ１６を制御して、設定されたエンコーダ制御目標値に従う回転量だけモータ４を駆動させる。

以上のような制御を行なうことにより、ワイヤやギア等バックラッシュあるいはコンプライアンスをもった動力伝達形を持った装置であっても、振動を生じさせることなく正確に関節部１１を目標関節角度に制御することが可能になる。また、極力、アナログ入力を必要とせずに、正確に関節部１１を目標関節角度に制御することができるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。

次に、図６を用いて、関節トルク推定処理を説明する。この処理は、図４の初期化処理が実行された後に、ポテンショメータ８およびエンコーダ３の出力値に基づいて、関節部１１のトルクを推定するための処理である。なお、この処理は、図５を用いて説明を行った関節角度の制御が実行されていないときに行なわれる。したがってパルスカウント原点を補正するＳ１０３の処理も実行しておらず、エンコーダ３のパルスカウント原点は一定である。

まず、ポテンショメータ８の値ｎ_Ｐを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０１)。次に、エンコーダ３の値ｎ_Ｅを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０２)。

次に、記憶したポテンショメータ８の値ｎ_Ｐを前記式２により関節角度θ_Ｐに、記憶したエンコーダ３の値ｎ_Ｅを前記式１により関節角度θ_Ｅに、それぞれ変換し、その角度差θ_Ｐ−θ_Ｅを計算する(Ｓ２０３)。

ポテンショメータ８およびエンコーダ３の値に基づいて算出した関節角度の角度差αは、ワイヤ１２およびギア５からなる動力伝達系のコンプライアンス性およびバックラッシュによって生じるものと考えてよい。これを図示すると、図７のようになる。

図７は、関節部１１の負荷トルクＴと角度差α（θ_Ｐ−θ_Ｅ）との関係を示している。理想的に動力伝達系のバックラッシュα_０が存在しない場合には、両者の関係は、図中の直線Ｃ２で表される。しかしながら、実際にはバックラッシュα_０が存在するために、両者の関係は直線では表されず、角度差軸部分で断続する図中のＣ１のような関係をなす。

バックラッシュα_０の角度差軸方向の中央値は、エンコーダ３の原点ｎ_Ｅ０をどのタイミングで初期化したかによって変化する。エンコーダ３のパルスカウント原点は、前記式６によって、逐次、補正されるが、初期化したときの原点の初期値ｎ_Ｅ０は、以下のようにして求められる。

前記式２「θ_Ｐ＝Ｃ_Ｐ(ｎ_Ｐ−ｎ_Ｐ０)」を用いて、ポテンショメータ値ｎ_Ｐから角度θ_Ｐを算出し、θ_Ｐをθとする。

前記式１「θ_Ｅ＝Ｃ_Ｅ(ｎ_Ｅ−ｎ_Ｅ０)」を「ｎ_Ｅ０＝(ｎ_Ｅ−θ_Ｅ／Ｃ_Ｅ)」と変形した式に、エンコーダ値ｎ_Ｅを代入し、さらに、θ_Ｅに算出したθを代入し、初期値ｎ_Ｅ０を求める。

このように、エンコーダ３のパルスカウント原点を初期化したときに、バックラッシュα_０が、初期化されたパルスカウント原点に対していずれの側に偏っているかを判別できないために、Ｃ１はグラフ原点を中心として、角度差軸方向にバックラッシュα_０分の不確定性を持つことになる。

Ｃ１が角度差軸方向に確定していれば、関節部１１の負荷トルクが０の場合以外は、角度差θ_Ｐ−θ_Ｅと関節部１１の負荷トルクＴとの関係が一意に決まるので、Ｓ２０３によって算出した角度差θ_Ｐ−θ_Ｅから関節部１１の負荷トルクＴを推定することができるようになる。

関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたＣ１に従う、角度差θ_Ｐ−θ_Ｅと関節部１１の負荷トルクＴとの関係を示す負荷トルクデータが記憶されている。Ｓ２０４では、推定用データ記憶領域２３から負荷トルクデータを読出して、Ｓ２０３で算出した角度差θ_Ｐ−θ_Ｅに対応する関節部１１の負荷トルクＴを算出する。エンコーダ３およびポテンショメータ８それぞれの停止が推定された後、処理Ｓ２０１〜Ｓ２０４によりトルクの推定を行ない、モータ４の電圧の調整を行なう。

なお、図６に示される関節トルク推定処理の前に、図４に示される初期化処理が実行されるために、図４に示されるフローチャートにおいて、モータ４に発生させる動力を可能な限り低減させた状態で角度差αが推定された後（図４のＳ１２）、関節部１１にかかるトルクが図６に示されるフローチャートによって推定されることになる。

このように、本実施の形態の場合には、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、関節部１１の負荷トルクＴを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。

図７のグラフ中のＣ１は、たとえば、モータ４の出力トルクを複数段階に変化させながら角度差θ_Ｐ−θ_Ｅを計測することにより特定することが可能である。しかしながら、モータ４の出力トルクを変更してからその値が安定するのを待ってからデータを取得するという手順では、作業に時間がかかるという問題がある。実用的な手法として以下のような方法がある。

まず、バックラッシュα_０を算出する。バックラッシュα_０は、前記式３を利用して算出できる。すなわち、前述したように、電圧Ｖ１下におけるα_０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα_０を算出できる。これにより、角度差軸上の正の値と負の値との２点を特定できる。

次に、トルクを計測する直前までは関節部１１を駆動する最低限のトルクＴ２でモータ４を回転させる。やがて、ポテンショメータ８の値θ_Ｐの変化がなくなり、エンコーダ３の値θ_Ｅの変化がなくなった時点での角度差θ_Ｐ−θ_Ｅを計算する。そして、計算した角度差θ_Ｐ−θ_ＥとトルクＴ２とに基づいて、図７のグラフに点をプロットする（たとえば、図７のＰＴ）。これにより、Ｃ１を確定させることができる。

以上、説明した本実施の形態では、制御に必要なアナログ入力数は、関節角度検出用センサがポテンショメータであれば１であり、従来技術として説明した図８の構成に比較すると、極力、アナログ入力数を減少させることができる。

同様に、アクチュエータ側および関節部側のうちのいずれか一方の回転角度センサを、ポテンショメータ等アナログ出力を持ったセンサで構成し、もう一方の回転角度センサをロータリエンコーダによって構成した場合であっても、アナログ入力はアクチュエータごとに１個となるので、従来の構成以下のアナログ入力数で、バックラッシュ推定、関節部側トルク推定等が可能になり、制御の正確性を高めることができる。

また、本実施の形態において、ポテンショメータ８に代えて、アブソルート型エンコーダのようなデジタル的な絶対角度検出手段を用いてもよく、この場合には、制御に必要な入力をすべてデジタル入力とすることが可能になる。

同様に、アクチュエータ側および関節側の回転角度センサを両方ともロータリエンコーダのようなデジタル出力を持ったセンサによって構成すれば、ハンドやマニプレータの制御回路に代表される関節駆動装置は、アナログ入力を必要としない。

その結果、Ａ／Ｄ変換回路を減少あるいは不要とすることができ、コンパクトかつ高性能な関節駆動装置を容易かつ安価に構成することができる。

また、本実施の形態では、ワイヤやベルト等弾性を持った動力伝達機構で接続されたアクチュエータおよび関節部それぞれに角度センサを取付ける。そして、関節部を所望の角度まで回転させるために、アクチュエータのサーボ系をアクチュエータ本体と、アクチュエータの回転角度を計測する角度センサにより構成している。これにより、動力伝達機構に弾性があっても剛性の高い制御を行なうことができる。

さらに、本実施の形態では、上記サーボ系の目標値を、関節部側に取付けた角度センサの読み値と回転型アクチュエータ側に取付けた角度センサの読み値の差分に比較的低いゲインをかけたもので逐次修正することにより、関節部が目標角度に到達することを保証している。そして、上記のような構成をとることにより、動力伝達部材の弾性の影響下であっても振動を生じさせることなく正確に関節部の角度位置決めを行なうことができるようになる。

本発明の関節駆動装置１をロボットフィンガに用いる場合、無負荷状態に置いて回転型アクチュエータの回転角度の読み値と関節部の回転角度の読み値とを一致させておけば、回転型アクチュエータ側の回転角度センサ読み値と、関節側の回転角度センサ読み値との差は、その中間にある弾性を持った動力伝達部材の変形によって生じたものと考えられるようになる。

したがって、回転型アクチュエータ側の回転角度センサの読み値と、関節側の回転角度センサの読み値との差は、関節部に働くトルクを反映して生じているものと捉えられるので、関節部に働くトルクと回転角度センサの読み値の差との関係を事前に測定しておけば回転角度センサの読み値の差に基づいて関節部に働くトルクを検出することができる。

以上をまとめると、ワイヤあるいはベルト等弾性を持った動力伝達部材により動力伝達を行なうハンドやマニプレータに本発明を適用することにより、振動を発生させることなく確実に関節角度の位置決めを行なう位置制御が可能になり、また、関節部に働くトルクの制御をすることが可能になる。

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

関節駆動装置の平面図である。関節駆動装置を図１の矢印Ｌ方向から見た図である。関節駆動装置の構成を示すブロック図である。初期化処理を説明するためのフローチャートである。位置制御サーボ補正処理を説明するためのフローチャートである。関節トルク推定処理を説明するためのフローチャートである。関節部の負荷トルクと角度差との関係を示す図である。従来の関節駆動装置を示す図である。

符号の説明

１関節駆動装置、２制御回路、２０ＣＰＵ、２１ＲＯＭ、２２ＲＡＭ、３エンコーダ、４モータ、５ギア、６主動側プーリ、７従動側プ−リ、８ポテンショメータ、９モータ軸、１０関節軸、１１関節部、１２ワイヤ、１３フレーム、１４リンク、１５モータサーボ回路、１６モータドライバ、１７Ａ／Ｄ変換回路、３０メカリミット（内側）、４０メカリミット（外側）。

Claims

関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、
前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、
前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備えることを特徴とする、関節駆動装置。
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、
該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、
該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備えることを特徴とする、関節駆動装置。
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備えることを特徴とする、関節駆動装置。
前記動力側回転角度検出手段と前記関節側回転角度検出手段とのうち少なくとも一方は、検出値として出力される情報がデジタル情報である回転角度センサであることを特徴とする、請求項１〜請求項３のいずれかに記載の関節駆動装置。
前記動力側回転角度検出手段は、前記回転型アクチュエータの一部を構成するモータに取付けた相対角度検出センサであり、
前記関節側回転角度検出手段は、絶対角度検出型センサであることを特徴とする、請求項１〜請求項４のいずれかに記載の関節駆動装置。
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材とをさらに備え、
前記制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とをさらに備え、
前記負荷トルク特定手段は、前記角度差算出手段が前記動力伝達前後角度差を算出した後に、前記負荷トルクを特定することを特徴とする、請求項１に記載の関節駆動装置。
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出ステップと、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出ステップと、
前記第１算出ステップの算出値と前記第２算出ステップの算出値との角度差を算出する角度差算出ステップと、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する手段であって前記関節駆動装置が備える記憶手段に記憶されている負荷トルクデータと、前記角度差算出ステップにより算出された角度差とから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定ステップとからなることを特徴とする、関節駆動装置の制御方法。
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正ステップと、
該基準値補正ステップにより補正された基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出ステップと、
該制御量算出ステップにより算出された制御量に基づいて前記回転型アクチュエータを駆動する駆動制御ステップとからなることを特徴とする、関節駆動装置の制御方法。
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段とを備える関節駆動装置の制御方法であって、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出ステップと、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出ステップと、
前記第１正方向最大角算出ステップの算出値と前記第１逆方向最大角算出ステップの算出値との差数と、前記第２正方向最大角算出ステップの算出値と前記第２逆方向最大角算出ステップの算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段ステップとからなることを特徴とする、関節駆動装置の制御方法。