関節駆動装置および関節駆動装置の制御方法
【課題】 関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る関節駆動装置を提供する。
【解決手段】 装置のモータ側にエンコーダを接続する一方、関節部側にポテンショメータを接続し、ポテンショメータおよびエンコーダの値を読み込んでメモリに記憶した後（Ｓ２０１、Ｓ２０２）、記憶したポテンショメータの値を関節角度θＰに、記憶したエンコーダ３の値を関節角度θＥにそれぞれ変換し、その角度差θＰ−θＥを計算する(Ｓ２０３)。角度差と関節部の負荷トルクとの関係を示す負荷トルクデータを予め記憶部に記憶し、その負荷トルクデータと、Ｓ２０３で算出された角度差とに基づいて、関節部の負荷トルクを推定する（Ｓ２０４）。
【解決手段】 装置のモータ側にエンコーダを接続する一方、関節部側にポテンショメータを接続し、ポテンショメータおよびエンコーダの値を読み込んでメモリに記憶した後（Ｓ２０１、Ｓ２０２）、記憶したポテンショメータの値を関節角度θＰに、記憶したエンコーダ３の値を関節角度θＥにそれぞれ変換し、その角度差θＰ−θＥを計算する(Ｓ２０３)。角度差と関節部の負荷トルクとの関係を示す負荷トルクデータを予め記憶部に記憶し、その負荷トルクデータと、Ｓ２０３で算出された角度差とに基づいて、関節部の負荷トルクを推定する（Ｓ２０４）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、関節駆動装置および関節駆動装置の制御方法に関し、特に、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材とを備える関節駆動装置および関節駆動装置の制御方法に関する。
【背景技術】
【０００２】
関節駆動装置として、たとえば、ロボットハンドを例に挙げることができる。人の手に合わせて作られた道具類を人の手が利用するように器用に利用するために、人の手のように多関節フィンガを複数組み合わせたロボットハンドの開発が進められている。
【０００３】
実際に人の手のような器用な動作をロボットハンドに行なわせるためには、複数の関節部を独立して駆動するための機構が必要となる。しかしながら、たとえばモータ等で構成されるアクチュエータ本体を直接関節部に設置した場合、フィンガのリンクの重量が増加するとともに、フィンガ自体のサイズが大きくなる等の問題が生じる。
【０００４】
このため、現在では、人の手のサイズに近いコンパクトなロボットハンドを構成するために掌部分にモータ等のアクチュエータを集中配置し、アクチュエータ本体から各関節部に対してリンク機構により動力を伝達する方法や、アクチュエータと関節部とのそれぞれにプーリを取付け、プーリ同士をワイヤ、タイミングベルト等の弾性を有する動力伝達部材で連結して用いて動力を伝達する方法が考えられている。
【０００５】
前者の方法を採用した場合には、フィンガの各関節角度を十分にとる必要性があるために機構が複雑になる傾向がある。また、リンク機構の性質上、トルクを平均して伝達することが難しい等の問題がある。
【０００６】
一方、後者の方法であれば、タイミングベルトの歯飛びを起こさせないための工夫や、ワイヤテンションを一定に保つためのメンテナンス作業が必要になるものの、前者の方法に比較して動力伝達を単純化することができる。このため、設計が容易である。さらに、トルク伝達の異方性等がないため、取り扱いも容易である。
【０００７】
このような理由から、ロボットハンドに代表される小型の関節駆動装置の駆動系としては、後者の方法、すなわち、アクチュエータと、関節部側に取付けたプーリとの間をワイヤあるいはタイミングベルトで結んで動力を伝達する方法を用いることが多い。
【０００８】
ロボットハンドが物体を安定に把持し続けるためには、各指の接触力のバランスをとる必要がある。各指の接触力を調整するために、一般的には、各フィンガの関節部のトルクを所定の値に制御するフィードバック系あるいはサーボ系を使用する。
【０００９】
ところが、ロボットハンド等の関節駆動装置の場合には、アクチュエータを構成するギア等の動力駆動機構部分のがたつきでバックラッシュが生じる。したがって、関節部を目標角度位置に正確に回動させるためには、関節部側の状態、およびバックラッシュの大きさを把握する必要がある。
【００１０】
また、ワイヤあるいはタイミングベルト駆動型の関節駆動装置の場合、フレームの撓みの他、動力伝達部材が弛むことによるコンプライアンス（compliance）性を有するため、アクチュエータ側のトルク出力を求めることと同時に、実際の関節部側のトルク情報を取得することが重要である。
【００１１】
特許文献１および非特許文献１には、関節部のトルクをワイヤ張力から推定する方法が開示されている。前者にはプーリ両端のワイヤに押し当てたカンチレバーの撓み量からワイヤ張力を測定する方法が開示されており、後者には張力差動型トルクセンサを用いてワイヤ張力を測定する方法が開示されている。いずれもプーリ両端のワイヤ張力の差分から関節トルクを推定する方法であるといえる。
【００１２】
特許文献１に開示された関節駆動装置の概略構成を図８（Ａ）に示す。また、非特許文献１に開示された関節駆動装置の概略構成を図８（Ｂ）に示す。
【００１３】
（Ａ）、（Ｂ）のいずれも、モータ軸Ｓ１に連結された主動側プーリＰ１と、関節部Ｊの関節軸Ｓ２に連結された従動側プーリＰ２との間に、ワイヤＷが掛け渡されており、主動側プーリＰ１の回転がワイヤＷによって従動側プーリＰ２に伝達されて、リンクＬが連結された関節軸Ｓ２がメカリミットＭＬ１およびメカリミットＭＬ２によって規制される範囲で回動する。
【００１４】
（Ａ）に示される関節駆動装置の場合には、板ばね状の２つのカンチレバーＣＬがフレームＦに取付けられている。また、カンチレバーＣＬの先端にはワイヤＷに張力を与えるテンションプーリＰ３が設けられている。さらに、カンチレバーＣＬの両側には図示しない歪ゲージが設けられる。（Ａ）に示される関節駆動装置では、双方の歪ゲージの出力に基づいて、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力を測定することにより関節部Ｊのトルクを推定する。
【００１５】
一方、（Ｂ）に示される関節駆動装置の場合には、ワイヤＷを挟んでテンションを与えているテンションプーリＰ４が張力計測用梁Ｂを介してフレームＦに取付けられている。（Ｂ）に示される関節駆動装置では、張力計測用梁Ｂに取付けた歪ゲージにより、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力の差分を測定し、その測定結果に基づいて、関節部Ｊのトルクを推定する。
【００１６】
なお、（Ａ）および（Ｂ）に示されたトルクの推定手法は原理的には動力伝達部材がベルトであっても同様に利用することが可能である。
【特許文献１】特開平６−８１７８号公報
【非特許文献１】金子真人，横井一仁，鈴木夏夫，谷江和雄，「プーリ・ワイヤ駆動系におけるトルクセンシングとトルク制御」，日本ロボット学会誌，１９８９年２月，第７巻，第１号，ｐ．６２−ｐ．７０
【発明の開示】
【発明が解決しようとする課題】
【００１７】
関節駆動装置をデジタル制御するためには、アナログ情報をデジタル変換してデジタル量として処理する必要がある。しかしながら、一般に、アナログ情報をデジタル変換するＡ／Ｄ変換回路は純粋なデジタル回路に比較して大きいために、装置へ組み込んだときの占有スペースが大きくなる。また、アナログ入力に応じてＡ／Ｄ変換回路を設計する必要があり、設計および製作が完了した後にアナログ入力を増やすことは困難になるので、回路設計の柔軟性が乏しくなる。
【００１８】
したがって、関節駆動装置をコンパクト化し、かつ、回路設計の柔軟性を高めるためには、極力、アナログ情報の入力を避ける必要がある。
【００１９】
ところが、従来においては、たとえば、図８に示されるように、歪ゲージからのアナログ入力を必要としていた。特に、図８（Ａ）の場合には、それぞれの関節についてプーリ両側のワイヤに押し付けたカンチレバーにそれぞれ２枚の歪みゲージが必要であるため、ブリッジ回路を組んで２個のアナログ入力が必要になる。また、張力作動型トルクセンサを用いる図８（Ｂ）の場合であっても、２枚の歪ゲージを用いるために、ブリッジ回路を組んで１個のアナログ入力が各関節に必要になる。
【００２０】
本発明は、上記のような課題を解決するためになされたものであって、歪ゲージ等からの情報の入力を必要とすることなく、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る関節駆動装置の提供を目的とする。
【課題を解決するための手段】
【００２１】
この発明のある局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備える。
【００２２】
この発明の他の局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備える。
【００２３】
この発明の他の局面に係る関節駆動装置は、関節部を正逆両方向に回動させるための関節側回転部材と、前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、前記関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備える。
【発明の効果】
【００２４】
本発明によれば、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。これは制御回路を小型化、省略化することに寄与するので、コンパクトかつ高性能な関節部駆動装置を容易かつ安価に構成することができるようになる。
【発明を実施するための最良の形態】
【００２５】
本発明の実施の形態について図面を参照しながら詳細に説明する。図１は本発明に係る関節駆動装置１の平面図である。また、図２は、関節駆動装置１を図１の矢印Ｌ方向から見た図である。なお、図１および図２では、関節駆動装置１の制御を行なう制御回路等、電気的回路の図示は省略している。
【００２６】
関節駆動装置１は、ＤＣモータであるモータ４を駆動源として、関節部１１に連結されたリンク１４を回動させる。関節駆動装置１のフレーム１３には、遊星ギアから成るギア５によって回転速度が１／２５６に減速されたモータ４の駆動力を伝達するモータ軸９が挿通されている。モータ軸９には、主動側プーリ６が連結されている。モータ４と、ギア５と、モータ軸９と、主動側プーリ６とにより、動力伝達機構の一例となる回転型アクチュエータが構成される。
【００２７】
関節部１１の関節軸１０には、従動側プーリ７が連結されている。従動側プーリ７はリンク１４に固定されており、関節軸１０を軸として回転する。
【００２８】
主動側プーリ６と従動側プーリ７との間には、弾性を有する動力伝達部材の一例となるワイヤ１２が８の字状に掛け渡されている。なお、弾性を有する動力伝達部材として、ワイヤに代えて、タイミングベルトを採用してもよい。
【００２９】
フレーム１３には、リンク１４の回動角を規制する規制部材の一例となるメカリミット３０，４０が取付けられている（図２では取付け構造の図示を省略）。リンク１４が矢印Ｒ１の方向（正方向）、すなわち時計回りに所定角度回動すると、リンク１４がメカリミット３０に当たって、それ以上の回動が規制される。同様に、リンク１４が矢印Ｒ２の方向（逆方向）、すなわち反時計回りに回ると、リンク１４がメカリミット４０に当たってそれ以上の回動が規制される。
【００３０】
モータ４には、モータ４の回転角を検出するためのエンコーダ３が接続されている。このエンコーダ３は、たとえば、インクリメンタル型の相対角度検出センサを構成するパルスエンコーダである。エンコーダ３は、モータ４が１回転する間に所定数のパルスを出力する。関節部１１には、関節軸１０においてフレーム１３とリンク１４とのなす角度を検出するためのポテンショメータ８が接続されている。以下、関節軸１０においてフレーム１３とリンク１４とのなす角度を“関節角度”という。ポテンショメータ８は、たとえば、関節角度が０度〜３２０度変化するのに応じて、出力電圧が０Ｖ〜２．５Ｖまで変化するように電気的に接続されている。
【００３１】
図３は、関節駆動装置１の電気的構成要素をブロック図として表したものである。関節駆動装置１は、図１および図２では図示が省略されている制御回路２、モータサーボ回路１５、モータドライバ１６、およびＡ／Ｄ変換回路１７をさらに備えている。
【００３２】
制御回路２は、ＣＰＵ２０、ＲＡＭ２２、ＲＯＭ２１を備えるマイクロコンピュータを含んでいる。ＲＯＭ２１には、ＣＰＵ２０を動作させる制御プログラムを記憶する領域とは別に、推定用データ記憶領域２３が設けられている。この推定用データ記憶領域２３には、後述する関節部１１への負荷トルクを推定するためのデータや、ギア５等の動力機構部分のがたつきで生じるバックラッシュの大きさを推定するためのデータが記憶されている。
【００３３】
制御回路２からモータサーボ回路１５へは、エンコーダ３の制御目標値が入力される。さらに、モータサーボ回路１５には、エンコーダ３から出力されるパルス信号が入力される。モータサーボ回路１５は、エンコーダ３のパルス信号をカウントしてモータ４の回転状況を特定し、モータドライバ１６に入力するＰＷＭ（Pulse Width Modulation）波形のデューティー比を変化させて、制御回路２から指令を受けたエンコーダ３の制御目標値相当の角度だけモータ４を駆動する。また、モータサーボ回路１５は、エンコーダ３のパルス信号を制御回路２へ入力する。モータ４は、モータドライバ１６からの駆動信号に応答して回転する。すなわち、モータ４、モータサーボ回路１５、およびモータドライバ１６は、モータ４の位置制御サーボ系を構成している。
【００３４】
モータ４の回転は、図１および図２に示したギア５、モータ軸９、主動側プーリ６、および動力伝達部材であるワイヤ１２により、関節部１１に伝達される。これにより、関節部１１において、リンク１４（図１、図２参照）が回動する。また、リンク１４の回動による関節角度の変化は、ポテンショメータ８の出力電圧を変化させる。この出力電圧は、Ａ／Ｄ変換回路１７によりアナログ情報からデジタル情報に変換されて制御回路２に入力される。制御回路２は、このデジタル情報に基づいて、関節角度を特定する。
【００３５】
次に、関節角度の算出手順について説明する。ここでは、関節駆動装置１の構成が、たとえば、以下のとおりであるものとする。
【００３６】
・プーリの直径比：主動側プーリ６（直径１５ｍｍ）／従動側プーリ７＝３／２
・ワイヤ長：５０ｍｍ
・Ａ／Ｄ変換回路１７の分解能：１２ビット
・ギア５による減速割合：１／２５６
・エンコーダ３のパルス発生割合：１６パルス／１回転
［エンコーダ３のカウント値から関節角度を算出する手順］
主動側プーリ６と従動側プーリ７との直径比が３：２であり、ギア５によるモータ４の回転速度の減速割合が１／２５６であるため、ワイヤ１２およびギア５に弾性やバックラッシュがない理想的な回転伝達状態においては、モータ４が１回転するごとに、関節軸１０は１／３８４回転することになる（（２／３）×（１／２５６））。
【００３７】
モータ４に接続されたエンコーダ３は、モータ４が１回転する間に１６パルスをカウントアップするので、関節角度を基準とすれば、エンコーダ３の分解能は、５．８６×１０−２度となる（（１／３８４）×（１／１６）×３６０度）。
【００３８】
関節部１１の原点位置（関節角度＝０度）におけるエンコーダ３のカウント値をｎＥ０とし、ある時点でのエンコーダ３のカウント値をｎＥとすると、式１を用いることによって、エンコーダ３のカウント値ｎＥから関節角度θＥを算出できる。なお、式１においてＣＥはエンコーダ３のカウント値を関節角度に変換する係数である。この係数は、エンコーダ３の分解能により定まるため、たとえば、５．８６×１０−２度/パルスである。
【００３９】
（式１） θＥ＝ＣＥ(ｎＥ−ｎＥ０)
［ポテンショメータ８の出力値から関節角度を算出する手順］
ポテンショメータ８に接続されるＡ／Ｄ変換回路１７の分解能が１２ビットであるため、ポテンショメータ８による関節角度の分解能は８．７８×１０−２度である（３６０度／２１２）。
【００４０】
ポテンショメータ８のＡ／Ｄ変換後の値をｎＰ、関節部１１の原点位置におけるポテンショメータ８のカウント値をｎＰ０とすると、式２を用いることによって、ポテンショメータ８の出力値から関節角度θＰを計算できる。なお、式２において、ＣＰはＡ／Ｄ変換回路１７から出力されるパルス数を関節角度に変換する係数である。この係数は、ポテンショメータ８による関節角度の分解能により定まるため、たとえば、８．７８×１０−２度/パルスである。
【００４１】
（式２） θＰ＝ＣＰ(ｎＰ−ｎＰ０)
次に、関節駆動装置１の制御回路２が実行する各種の制御について、図４〜図６を参照して説明する。
【００４２】
図４は、初期化処理を説明するためのフローチャートである。この初期化処理は、エンコーダ３およびポテンショメータ８の出力値が正常値であるか否かをチェックするとともに、ポテンショメータ８に基づく式２の演算式のパラメータを補正し、さらには、関節駆動装置１の動力駆動機構のバックラッシュを推定するための処理である。
【００４３】
まず、内側に関節が回る方向、すなわち、リンク１４が図２に示したＲ１方向に回転するように、モータ４を駆動させる（Ｓ１）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもメカリミット３０にリンク１４が当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。
【００４４】
次に、ポテンショメータ８の出力値をデジタル変換した値ｎＰ＋を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ２)。
【００４５】
次に、前記式１を用いて、入力値ｎＰ＋に基づいた関節角度θＰ＋を算出する（Ｓ３）。次に、算出した関節角度θＰ＋が予め定めた正常範囲に収まっているか否かを判断する（Ｓ４）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θＰ＋が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θＰ＋の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ４の判断を行なう。
【００４６】
算出した関節角度θＰ＋が正常な範囲に収まっていないならば（Ｓ４でＮＯ）、初期化処理を中断する。このような減少が生じる原因としては、ポテンショメータ８に繋がる電気系統の切断あるいは故障、ポテンショメータ８の取付け位置の機械的なずれ、あるいは、モータ４の故障等が考えられる。
【００４７】
Ｓ４において、算出した関節角度θＰ＋が正常な範囲に収まっているならば（Ｓ４でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎＥ＋を入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ５)。
【００４８】
次に、外側に関節が回る方向、すなわち、リンク１４が図２に示したＲ２方向に回転するように、Ｓ１と逆方向にモータ４を駆動させる（Ｓ６）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもリンク１４がメカリミット４０に当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。
【００４９】
次に、ポテンショメータ８の出力値をデジタル変換した値ｎＰ−を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ７)。
【００５０】
次に、前記式２を用いて、入力値ｎＰ−に基づいた関節角度θＰ−を算出する（Ｓ８）。次に、算出した関節角度θＰ−が予め定めた正常範囲に収まっているか否かを判断する（Ｓ９）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θＰ−が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θＰ−の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ９の判断を行なう。
【００５１】
算出した関節角度θＰ−が正常な範囲に収まっていないならば（Ｓ９でＮＯ）、初期化処理を中断する。一方、算出した関節角度θＰ−が正常な範囲に収まっているならば（Ｓ９でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎＥ−を入力し（図３参照）、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ１０)。
【００５２】
次に、Ｓ５およびＳ１０において入力されたエンコーダ３の値に基づいて、エンコーダ３の値が正常に変化しているか否かを判断する（Ｓ１１）。具体的には、Ｓ５において取得したエンコーダ３のパルス数ｎＥ＋とＳ１０において取得したエンコーダ３のパルス数ｎＥ−との差分ｎＥ＋−ｎＥ−が、リンク１４がメカリミット３０とメカリミット４０との間を回動する間にエンコーダ３によってカウントすることが期待されるパルス数に対して一定の範囲内に収まっているか否かを判別する。たとえば、制御回路２のＲＯＭ２３内には、エンコーダ３の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ１１の判断を行なう。
【００５３】
エンコーダ３のパルス数の差分(ｎＥ＋−ｎＥ−)が、期待される値の範囲内に無い場合（Ｓ１１でＮＯ）には、初期化処理を異常終了する。このような現象が起きる原因としてはエンコーダ３に繋がる電気系系統の切断、あるいはエンコーダ３やそれに関わる回路の故障が考えられる。
【００５４】
エンコーダ３のパルス数の差分が期待される値の範囲内にある場合（Ｓ１１でＹＥＳ）には、ギア等の動力駆動機構部分のがたつきによるバックラッシュ成分とワイヤ１２のコンプライアンス成分とによって生じる、アクチュエータ側と関節部側との間の角度差αを算出する(Ｓ１２)。
【００５５】
このバックラッシュ成分およびコンプライアンス成分が存在することによって、関節部１１が固定されている場合であってもモータ４が若干回転し、逆にモータ４の駆動が停止している場合であっても関節部１１が若干回転してしまう。そのため、アクチュエータ側の制御上の角度から計算で求められる関節角度と、関節部側の実際の関節角度との間で角度差αが生じる。この角度差αの大きさを特定することは、動力伝達系の制御の正確性を高めるために重要である。角度差αのうち、特にバックラッシュ成分に起因するものを、無負荷時のバックラッシュα０という。
【００５６】
ここで、Ｓ１２において制御回路２が実行する処理を詳細に説明する。
【００５７】
まず、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれでエンコーダ３から入力された値ｎＥ＋(Ｓ５で入力)，ｎＥ−(Ｓ１０で入力)に基づいて、前記式１を用いて関節角度を算出する。ｎＥ＋に基づいて算出された関節角度をθＥ＋とし、ｎＥ−に基づいて算出された関節角度をθＥ−とする。
【００５８】
次に、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれのポテンショメータ８の出力値（デジタル値）ｎＰ＋(Ｓ２で入力)，ｎＰ−(Ｓ７で入力)に基づいてＳ３およびＳ８ですでに算出されている関節角度θＰ＋およびθＰ−を読み出す。
【００５９】
最後に、「（θＥ＋−θＥ−）−(θＰ＋−θＰ−)」を実行することによって、角度差αを算出する。すなわち、角度差α＝（θＥ＋−θＥ−）−(θＰ＋−θＰ−)である。
【００６０】
ところで、図４に示される初期化処理中のエンコーダ３による関節角度換算値の変化量（θＥ＋−θＥ−）と、ポテンショメータ８による関節角度換算値の変化量(θＰ＋−θＰ−)との角度差αは、式３に示すように、バックラッシュα０の関数と考えることができる。
【００６１】
（式３） α(α０)＝（θＥ＋−θＥ−）−(θＰ＋−θＰ−)
モータ４に一定電圧Ｖ１を連続的に印加して、リンク１４をメカリミット３０または４０に押し付ける場合、関節角度換算値の変化量の差αはバックラッシュα０について単調増加になる。したがって電圧Ｖ１下におけるα０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα０を算出できる。
【００６２】
関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたα０とαとの関係を示すバックラッシュデータが記憶されている。初期化処理のＳ１２では、さらに、推定用データ記憶領域２３からバックラッシュデータが読出されて、算出した角度差αに対応するバックラッシュα０が算出される。
【００６３】
Ｓ１２でバックラッシュの推定処理が終了した後、θＰ＋，θＰ−と、メカリミット３０，４０にリンク１４が接している時の機械的にあるべき角度θ＋，θ−との比較により、ポテンショメータ入力値を関節角度に変換するために用いる前記式２のパラメータＣＰ，ｎＰ０の補正を行なう（Ｓ１３）。
【００６４】
具体的には、以下の式４および式５に従う演算をして、パラメータＣＰ，ｎＰ０を補正する。
【００６５】
（式４） ＣＰ＝（θ＋−θ−）／（ｎＰ＋−ｎＰ−）
（式５） ｎＰ０＝ｎＰ＋−（θ＋／ＣＰ）
以降、パラメータＣＰおよびｎＰ０は、変更せず、固定値とする。
【００６６】
なお、式４および式５は、既知の（θ＋，ｎＰ＋）および（θ−，ｎＰ−）の各々を、前記式２「θＰ＝ＣＰ(ｎＰ−ｎＰ０)」に代入して、
θ＋＝ＣＰ(ｎＰ＋−ｎＰ０)
θ−＝ＣＰ(ｎＰ−−ｎＰ０)
とし、これを連立方程式として、ＣＰおよびｎＰ０を導出するものである。
【００６７】
たとえば、伝達機構がマニプレータやロボットフィンガに搭載され機構の周囲をケース等でシールドされた状態であっても本実施の形態の手法は適用可能である。つまり、上記のような初期化処理を制御回路２が実行することにより、ワイヤ１２を直接観測できない条件下であっても簡易に伝達系のバックラッシュを推定することができるようになるので、メンテナンスが容易である。また、本実施の形態によれば、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、伝達系のバックラッシュを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００６８】
次に、図５を用いて、位置制御サーボ補正処理を説明する。この処理は、関節角度が所望の値になるように装置を制御するための処理である。
【００６９】
まず、ポテンショメータ８の値ｎＰを入力し、メモリ（ＲＡＭ２２）に記憶し、入力値に基づいて、前記式２によって関節角度θＰを算出する（Ｓ１０１）。関節部１１とポテンショメータ８は直接接続してあるので、θＰは実際の関節角度θを正確に反映している。なお、Ｓ１０１では、図４の初期化処理のＳ１３において補正されたパラメータＣＰおよびｎＰ０が用いられる。
【００７０】
次に、エンコーダ３のカウント値ｎＥを入力し、メモリ（ＲＡＭ２２）に記憶する（Ｓ１０２）。エンコーダ３と関節部１１との間には動力伝達系(ギア５およびワイヤ１２)が存在することから、エンコーダ３のカウント値ｎＥから前記式１を用いて算出した関節角度θＥは、実際の関節角度θを正確に反映していない。したがって、単にエンコーダ３のパルスカウントのみに基づいてサーボ形を構成した場合、リンク１４を目標の関節角度θの位置に正確に制御することができない。
【００７１】
そこで式６の計算により、エンコーダ３のパルスカウント原点を、初期値ｎＥ０から関節角度制御の回毎（ｉ回毎）に逐次変更して、ｎＥ０ｉに補正する(Ｓ１０３)。
【００７２】
（式６） ｎＥ０ｉ＝（１−ｋ）ｎＥ０ｉ−１＋ｋ（ｎＥ−θＰ／ＣＥ）
（ただし、ｉは自然数であり、ｎＥ００＝ｎＥ０である）
式６において、θＰ／ＣＥは、実際の関節角度θＰに対応するエンコーダ３のパルスカウント値である。したがって、ｎＥ−θＰ／ＣＥは、前回（ｉ−１回目）のエンコーダ３のパルスカウント値ｎＥと、実際にカウントされるべきエンコーダ３のパルスカウント値θＰ／ＣＥとの差、すなわち、今回（ｉ回目）の制御に際して、補正すべき原点の誤差である。
【００７３】
理論上は、この誤差（ｎＥ−θＰ／ＣＥ）を、前回のエンコーダ３のパルスカウント原点ｎＥ０ｉ−１に加算することによって、次回の制御に用いるパルスカウント原点を求めることができる。
【００７４】
しかしながら、このようなパルスカウント原点の補正処理が毎回行なわれることで処理Ｓ１０３の実行周期が大きくなると、一度の変更によるパルスカウント原点ｎＥ０の変化が大きくなり、制御系の振動が生じやすくなる。そこで、本実施の形態では、１以下の比例ゲインであるｋを式６に示すように採用し、ゲインｋを適切に設定して振動を抑制している。
【００７５】
以上の計算が終わった後、式７によって、次回の関節角度目標値θｉに制御するためのエンコーダ制御目標値ｎＥｉを算出する(Ｓ１０４)。
【００７６】
（式７） ｎＥｉ＝ｎＥ０ｉ＋θｉ／ＣＥ
この式７は、前記式１の「θＥ＝ＣＥ(ｎＥ−ｎＥ０)」から変形した式「ｎＥ＝ｎＥ０＋θＥ／ＣＥ」を応用したものである。
【００７７】
最後に、Ｓ１０４において算出したエンコーダ制御目標値ｎＥｉをモータサーボ回路１５に設定する（Ｓ１０５）。なお、モータサーボ回路１５は、モータドライバ１６を制御して、設定されたエンコーダ制御目標値に従う回転量だけモータ４を駆動させる。
【００７８】
以上のような制御を行なうことにより、ワイヤやギア等バックラッシュあるいはコンプライアンスをもった動力伝達形を持った装置であっても、振動を生じさせることなく正確に関節部１１を目標関節角度に制御することが可能になる。また、極力、アナログ入力を必要とせずに、正確に関節部１１を目標関節角度に制御することができるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００７９】
次に、図６を用いて、関節トルク推定処理を説明する。この処理は、図４の初期化処理が実行された後に、ポテンショメータ８およびエンコーダ３の出力値に基づいて、関節部１１のトルクを推定するための処理である。なお、この処理は、図５を用いて説明を行った関節角度の制御が実行されていないときに行なわれる。したがってパルスカウント原点を補正するＳ１０３の処理も実行しておらず、エンコーダ３のパルスカウント原点は一定である。
【００８０】
まず、ポテンショメータ８の値ｎＰを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０１)。次に、エンコーダ３の値ｎＥを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０２)。
【００８１】
次に、記憶したポテンショメータ８の値ｎＰを前記式２により関節角度θＰに、記憶したエンコーダ３の値ｎＥを前記式１により関節角度θＥに、それぞれ変換し、その角度差θＰ−θＥを計算する(Ｓ２０３)。
【００８２】
ポテンショメータ８およびエンコーダ３の値に基づいて算出した関節角度の角度差αは、ワイヤ１２およびギア５からなる動力伝達系のコンプライアンス性およびバックラッシュによって生じるものと考えてよい。これを図示すると、図７のようになる。
【００８３】
図７は、関節部１１の負荷トルクＴと角度差α（θＰ−θＥ）との関係を示している。理想的に動力伝達系のバックラッシュα０が存在しない場合には、両者の関係は、図中の直線Ｃ２で表される。しかしながら、実際にはバックラッシュα０が存在するために、両者の関係は直線では表されず、角度差軸部分で断続する図中のＣ１のような関係をなす。
【００８４】
バックラッシュα０の角度差軸方向の中央値は、エンコーダ３の原点ｎＥ０をどのタイミングで初期化したかによって変化する。エンコーダ３のパルスカウント原点は、前記式６によって、逐次、補正されるが、初期化したときの原点の初期値ｎＥ０は、以下のようにして求められる。
【００８５】
前記式２「θＰ＝ＣＰ(ｎＰ−ｎＰ０)」を用いて、ポテンショメータ値ｎＰから角度θＰを算出し、θＰをθとする。
【００８６】
前記式１「θＥ＝ＣＥ(ｎＥ−ｎＥ０)」を「ｎＥ０＝(ｎＥ−θＥ／ＣＥ)」と変形した式に、エンコーダ値ｎＥを代入し、さらに、θＥに算出したθを代入し、初期値ｎＥ０を求める。
【００８７】
このように、エンコーダ３のパルスカウント原点を初期化したときに、バックラッシュα０が、初期化されたパルスカウント原点に対していずれの側に偏っているかを判別できないために、Ｃ１はグラフ原点を中心として、角度差軸方向にバックラッシュα０分の不確定性を持つことになる。
【００８８】
Ｃ１が角度差軸方向に確定していれば、関節部１１の負荷トルクが０の場合以外は、角度差θＰ−θＥと関節部１１の負荷トルクＴとの関係が一意に決まるので、Ｓ２０３によって算出した角度差θＰ−θＥから関節部１１の負荷トルクＴを推定することができるようになる。
【００８９】
関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたＣ１に従う、角度差θＰ−θＥと関節部１１の負荷トルクＴとの関係を示す負荷トルクデータが記憶されている。Ｓ２０４では、推定用データ記憶領域２３から負荷トルクデータを読出して、Ｓ２０３で算出した角度差θＰ−θＥに対応する関節部１１の負荷トルクＴを算出する。エンコーダ３およびポテンショメータ８それぞれの停止が推定された後、処理Ｓ２０１〜Ｓ２０４によりトルクの推定を行ない、モータ４の電圧の調整を行なう。
【００９０】
なお、図６に示される関節トルク推定処理の前に、図４に示される初期化処理が実行されるために、図４に示されるフローチャートにおいて、モータ４に発生させる動力を可能な限り低減させた状態で角度差αが推定された後（図４のＳ１２）、関節部１１にかかるトルクが図６に示されるフローチャートによって推定されることになる。
【００９１】
このように、本実施の形態の場合には、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、関節部１１の負荷トルクＴを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００９２】
図７のグラフ中のＣ１は、たとえば、モータ４の出力トルクを複数段階に変化させながら角度差θＰ−θＥを計測することにより特定することが可能である。しかしながら、モータ４の出力トルクを変更してからその値が安定するのを待ってからデータを取得するという手順では、作業に時間がかかるという問題がある。実用的な手法として以下のような方法がある。
【００９３】
まず、バックラッシュα０を算出する。バックラッシュα０は、前記式３を利用して算出できる。すなわち、前述したように、電圧Ｖ１下におけるα０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα０を算出できる。これにより、角度差軸上の正の値と負の値との２点を特定できる。
【００９４】
次に、トルクを計測する直前までは関節部１１を駆動する最低限のトルクＴ２でモータ４を回転させる。やがて、ポテンショメータ８の値θＰの変化がなくなり、エンコーダ３の値θＥの変化がなくなった時点での角度差θＰ−θＥを計算する。そして、計算した角度差θＰ−θＥとトルクＴ２とに基づいて、図７のグラフに点をプロットする（たとえば、図７のＰＴ）。これにより、Ｃ１を確定させることができる。
【００９５】
以上、説明した本実施の形態では、制御に必要なアナログ入力数は、関節角度検出用センサがポテンショメータであれば１であり、従来技術として説明した図８の構成に比較すると、極力、アナログ入力数を減少させることができる。
【００９６】
同様に、アクチュエータ側および関節部側のうちのいずれか一方の回転角度センサを、ポテンショメータ等アナログ出力を持ったセンサで構成し、もう一方の回転角度センサをロータリエンコーダによって構成した場合であっても、アナログ入力はアクチュエータごとに１個となるので、従来の構成以下のアナログ入力数で、バックラッシュ推定、関節部側トルク推定等が可能になり、制御の正確性を高めることができる。
【００９７】
また、本実施の形態において、ポテンショメータ８に代えて、アブソルート型エンコーダのようなデジタル的な絶対角度検出手段を用いてもよく、この場合には、制御に必要な入力をすべてデジタル入力とすることが可能になる。
【００９８】
同様に、アクチュエータ側および関節側の回転角度センサを両方ともロータリエンコーダのようなデジタル出力を持ったセンサによって構成すれば、ハンドやマニプレータの制御回路に代表される関節駆動装置は、アナログ入力を必要としない。
【００９９】
その結果、Ａ／Ｄ変換回路を減少あるいは不要とすることができ、コンパクトかつ高性能な関節駆動装置を容易かつ安価に構成することができる。
【０１００】
また、本実施の形態では、ワイヤやベルト等弾性を持った動力伝達機構で接続されたアクチュエータおよび関節部それぞれに角度センサを取付ける。そして、関節部を所望の角度まで回転させるために、アクチュエータのサーボ系をアクチュエータ本体と、アクチュエータの回転角度を計測する角度センサにより構成している。これにより、動力伝達機構に弾性があっても剛性の高い制御を行なうことができる。
【０１０１】
さらに、本実施の形態では、上記サーボ系の目標値を、関節部側に取付けた角度センサの読み値と回転型アクチュエータ側に取付けた角度センサの読み値の差分に比較的低いゲインをかけたもので逐次修正することにより、関節部が目標角度に到達することを保証している。そして、上記のような構成をとることにより、動力伝達部材の弾性の影響下であっても振動を生じさせることなく正確に関節部の角度位置決めを行なうことができるようになる。
【０１０２】
本発明の関節駆動装置１をロボットフィンガに用いる場合、無負荷状態に置いて回転型アクチュエータの回転角度の読み値と関節部の回転角度の読み値とを一致させておけば、回転型アクチュエータ側の回転角度センサ読み値と、関節側の回転角度センサ読み値との差は、その中間にある弾性を持った動力伝達部材の変形によって生じたものと考えられるようになる。
【０１０３】
したがって、回転型アクチュエータ側の回転角度センサの読み値と、関節側の回転角度センサの読み値との差は、関節部に働くトルクを反映して生じているものと捉えられるので、関節部に働くトルクと回転角度センサの読み値の差との関係を事前に測定しておけば回転角度センサの読み値の差に基づいて関節部に働くトルクを検出することができる。
【０１０４】
以上をまとめると、ワイヤあるいはベルト等弾性を持った動力伝達部材により動力伝達を行なうハンドやマニプレータに本発明を適用することにより、振動を発生させることなく確実に関節角度の位置決めを行なう位置制御が可能になり、また、関節部に働くトルクの制御をすることが可能になる。
【０１０５】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【図面の簡単な説明】
【０１０６】
【図１】関節駆動装置の平面図である。
【図２】関節駆動装置を図１の矢印Ｌ方向から見た図である。
【図３】関節駆動装置の構成を示すブロック図である。
【図４】初期化処理を説明するためのフローチャートである。
【図５】位置制御サーボ補正処理を説明するためのフローチャートである。
【図６】関節トルク推定処理を説明するためのフローチャートである。
【図７】関節部の負荷トルクと角度差との関係を示す図である。
【図８】従来の関節駆動装置を示す図である。
【符号の説明】
【０１０７】
１ 関節駆動装置、２ 制御回路、 ２０ ＣＰＵ、２１ ＲＯＭ、２２ ＲＡＭ、３ エンコーダ、４ モータ、５ ギア、６ 主動側プーリ、７ 従動側プ−リ、８ ポテンショメータ、９ モータ軸、１０ 関節軸、１１ 関節部、１２ ワイヤ、１３ フレーム、１４ リンク、１５ モータサーボ回路、１６ モータドライバ、１７ Ａ／Ｄ変換回路、３０ メカリミット（内側）、４０ メカリミット（外側）。
【技術分野】
【０００１】
本発明は、関節駆動装置および関節駆動装置の制御方法に関し、特に、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材とを備える関節駆動装置および関節駆動装置の制御方法に関する。
【背景技術】
【０００２】
関節駆動装置として、たとえば、ロボットハンドを例に挙げることができる。人の手に合わせて作られた道具類を人の手が利用するように器用に利用するために、人の手のように多関節フィンガを複数組み合わせたロボットハンドの開発が進められている。
【０００３】
実際に人の手のような器用な動作をロボットハンドに行なわせるためには、複数の関節部を独立して駆動するための機構が必要となる。しかしながら、たとえばモータ等で構成されるアクチュエータ本体を直接関節部に設置した場合、フィンガのリンクの重量が増加するとともに、フィンガ自体のサイズが大きくなる等の問題が生じる。
【０００４】
このため、現在では、人の手のサイズに近いコンパクトなロボットハンドを構成するために掌部分にモータ等のアクチュエータを集中配置し、アクチュエータ本体から各関節部に対してリンク機構により動力を伝達する方法や、アクチュエータと関節部とのそれぞれにプーリを取付け、プーリ同士をワイヤ、タイミングベルト等の弾性を有する動力伝達部材で連結して用いて動力を伝達する方法が考えられている。
【０００５】
前者の方法を採用した場合には、フィンガの各関節角度を十分にとる必要性があるために機構が複雑になる傾向がある。また、リンク機構の性質上、トルクを平均して伝達することが難しい等の問題がある。
【０００６】
一方、後者の方法であれば、タイミングベルトの歯飛びを起こさせないための工夫や、ワイヤテンションを一定に保つためのメンテナンス作業が必要になるものの、前者の方法に比較して動力伝達を単純化することができる。このため、設計が容易である。さらに、トルク伝達の異方性等がないため、取り扱いも容易である。
【０００７】
このような理由から、ロボットハンドに代表される小型の関節駆動装置の駆動系としては、後者の方法、すなわち、アクチュエータと、関節部側に取付けたプーリとの間をワイヤあるいはタイミングベルトで結んで動力を伝達する方法を用いることが多い。
【０００８】
ロボットハンドが物体を安定に把持し続けるためには、各指の接触力のバランスをとる必要がある。各指の接触力を調整するために、一般的には、各フィンガの関節部のトルクを所定の値に制御するフィードバック系あるいはサーボ系を使用する。
【０００９】
ところが、ロボットハンド等の関節駆動装置の場合には、アクチュエータを構成するギア等の動力駆動機構部分のがたつきでバックラッシュが生じる。したがって、関節部を目標角度位置に正確に回動させるためには、関節部側の状態、およびバックラッシュの大きさを把握する必要がある。
【００１０】
また、ワイヤあるいはタイミングベルト駆動型の関節駆動装置の場合、フレームの撓みの他、動力伝達部材が弛むことによるコンプライアンス（compliance）性を有するため、アクチュエータ側のトルク出力を求めることと同時に、実際の関節部側のトルク情報を取得することが重要である。
【００１１】
特許文献１および非特許文献１には、関節部のトルクをワイヤ張力から推定する方法が開示されている。前者にはプーリ両端のワイヤに押し当てたカンチレバーの撓み量からワイヤ張力を測定する方法が開示されており、後者には張力差動型トルクセンサを用いてワイヤ張力を測定する方法が開示されている。いずれもプーリ両端のワイヤ張力の差分から関節トルクを推定する方法であるといえる。
【００１２】
特許文献１に開示された関節駆動装置の概略構成を図８（Ａ）に示す。また、非特許文献１に開示された関節駆動装置の概略構成を図８（Ｂ）に示す。
【００１３】
（Ａ）、（Ｂ）のいずれも、モータ軸Ｓ１に連結された主動側プーリＰ１と、関節部Ｊの関節軸Ｓ２に連結された従動側プーリＰ２との間に、ワイヤＷが掛け渡されており、主動側プーリＰ１の回転がワイヤＷによって従動側プーリＰ２に伝達されて、リンクＬが連結された関節軸Ｓ２がメカリミットＭＬ１およびメカリミットＭＬ２によって規制される範囲で回動する。
【００１４】
（Ａ）に示される関節駆動装置の場合には、板ばね状の２つのカンチレバーＣＬがフレームＦに取付けられている。また、カンチレバーＣＬの先端にはワイヤＷに張力を与えるテンションプーリＰ３が設けられている。さらに、カンチレバーＣＬの両側には図示しない歪ゲージが設けられる。（Ａ）に示される関節駆動装置では、双方の歪ゲージの出力に基づいて、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力を測定することにより関節部Ｊのトルクを推定する。
【００１５】
一方、（Ｂ）に示される関節駆動装置の場合には、ワイヤＷを挟んでテンションを与えているテンションプーリＰ４が張力計測用梁Ｂを介してフレームＦに取付けられている。（Ｂ）に示される関節駆動装置では、張力計測用梁Ｂに取付けた歪ゲージにより、８の字状に襷掛けされた上下のワイヤＷの各々にかかる張力の差分を測定し、その測定結果に基づいて、関節部Ｊのトルクを推定する。
【００１６】
なお、（Ａ）および（Ｂ）に示されたトルクの推定手法は原理的には動力伝達部材がベルトであっても同様に利用することが可能である。
【特許文献１】特開平６−８１７８号公報
【非特許文献１】金子真人，横井一仁，鈴木夏夫，谷江和雄，「プーリ・ワイヤ駆動系におけるトルクセンシングとトルク制御」，日本ロボット学会誌，１９８９年２月，第７巻，第１号，ｐ．６２−ｐ．７０
【発明の開示】
【発明が解決しようとする課題】
【００１７】
関節駆動装置をデジタル制御するためには、アナログ情報をデジタル変換してデジタル量として処理する必要がある。しかしながら、一般に、アナログ情報をデジタル変換するＡ／Ｄ変換回路は純粋なデジタル回路に比較して大きいために、装置へ組み込んだときの占有スペースが大きくなる。また、アナログ入力に応じてＡ／Ｄ変換回路を設計する必要があり、設計および製作が完了した後にアナログ入力を増やすことは困難になるので、回路設計の柔軟性が乏しくなる。
【００１８】
したがって、関節駆動装置をコンパクト化し、かつ、回路設計の柔軟性を高めるためには、極力、アナログ情報の入力を避ける必要がある。
【００１９】
ところが、従来においては、たとえば、図８に示されるように、歪ゲージからのアナログ入力を必要としていた。特に、図８（Ａ）の場合には、それぞれの関節についてプーリ両側のワイヤに押し付けたカンチレバーにそれぞれ２枚の歪みゲージが必要であるため、ブリッジ回路を組んで２個のアナログ入力が必要になる。また、張力作動型トルクセンサを用いる図８（Ｂ）の場合であっても、２枚の歪ゲージを用いるために、ブリッジ回路を組んで１個のアナログ入力が各関節に必要になる。
【００２０】
本発明は、上記のような課題を解決するためになされたものであって、歪ゲージ等からの情報の入力を必要とすることなく、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る関節駆動装置の提供を目的とする。
【課題を解決するための手段】
【００２１】
この発明のある局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備える。
【００２２】
この発明の他の局面に係る関節駆動装置は、関節部を回動させるための関節側回転部材と、該関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備える。
【００２３】
この発明の他の局面に係る関節駆動装置は、関節部を正逆両方向に回動させるための関節側回転部材と、前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、前記関節側回転部材へ動力を供給するための回転型アクチュエータと、該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、前記回転型アクチュエータを制御する制御手段とを備え、該制御手段は、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備える。
【発明の効果】
【００２４】
本発明によれば、回転型アクチュエータおよび関節側回転部材の回転角度の情報を入力することによって、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。これは制御回路を小型化、省略化することに寄与するので、コンパクトかつ高性能な関節部駆動装置を容易かつ安価に構成することができるようになる。
【発明を実施するための最良の形態】
【００２５】
本発明の実施の形態について図面を参照しながら詳細に説明する。図１は本発明に係る関節駆動装置１の平面図である。また、図２は、関節駆動装置１を図１の矢印Ｌ方向から見た図である。なお、図１および図２では、関節駆動装置１の制御を行なう制御回路等、電気的回路の図示は省略している。
【００２６】
関節駆動装置１は、ＤＣモータであるモータ４を駆動源として、関節部１１に連結されたリンク１４を回動させる。関節駆動装置１のフレーム１３には、遊星ギアから成るギア５によって回転速度が１／２５６に減速されたモータ４の駆動力を伝達するモータ軸９が挿通されている。モータ軸９には、主動側プーリ６が連結されている。モータ４と、ギア５と、モータ軸９と、主動側プーリ６とにより、動力伝達機構の一例となる回転型アクチュエータが構成される。
【００２７】
関節部１１の関節軸１０には、従動側プーリ７が連結されている。従動側プーリ７はリンク１４に固定されており、関節軸１０を軸として回転する。
【００２８】
主動側プーリ６と従動側プーリ７との間には、弾性を有する動力伝達部材の一例となるワイヤ１２が８の字状に掛け渡されている。なお、弾性を有する動力伝達部材として、ワイヤに代えて、タイミングベルトを採用してもよい。
【００２９】
フレーム１３には、リンク１４の回動角を規制する規制部材の一例となるメカリミット３０，４０が取付けられている（図２では取付け構造の図示を省略）。リンク１４が矢印Ｒ１の方向（正方向）、すなわち時計回りに所定角度回動すると、リンク１４がメカリミット３０に当たって、それ以上の回動が規制される。同様に、リンク１４が矢印Ｒ２の方向（逆方向）、すなわち反時計回りに回ると、リンク１４がメカリミット４０に当たってそれ以上の回動が規制される。
【００３０】
モータ４には、モータ４の回転角を検出するためのエンコーダ３が接続されている。このエンコーダ３は、たとえば、インクリメンタル型の相対角度検出センサを構成するパルスエンコーダである。エンコーダ３は、モータ４が１回転する間に所定数のパルスを出力する。関節部１１には、関節軸１０においてフレーム１３とリンク１４とのなす角度を検出するためのポテンショメータ８が接続されている。以下、関節軸１０においてフレーム１３とリンク１４とのなす角度を“関節角度”という。ポテンショメータ８は、たとえば、関節角度が０度〜３２０度変化するのに応じて、出力電圧が０Ｖ〜２．５Ｖまで変化するように電気的に接続されている。
【００３１】
図３は、関節駆動装置１の電気的構成要素をブロック図として表したものである。関節駆動装置１は、図１および図２では図示が省略されている制御回路２、モータサーボ回路１５、モータドライバ１６、およびＡ／Ｄ変換回路１７をさらに備えている。
【００３２】
制御回路２は、ＣＰＵ２０、ＲＡＭ２２、ＲＯＭ２１を備えるマイクロコンピュータを含んでいる。ＲＯＭ２１には、ＣＰＵ２０を動作させる制御プログラムを記憶する領域とは別に、推定用データ記憶領域２３が設けられている。この推定用データ記憶領域２３には、後述する関節部１１への負荷トルクを推定するためのデータや、ギア５等の動力機構部分のがたつきで生じるバックラッシュの大きさを推定するためのデータが記憶されている。
【００３３】
制御回路２からモータサーボ回路１５へは、エンコーダ３の制御目標値が入力される。さらに、モータサーボ回路１５には、エンコーダ３から出力されるパルス信号が入力される。モータサーボ回路１５は、エンコーダ３のパルス信号をカウントしてモータ４の回転状況を特定し、モータドライバ１６に入力するＰＷＭ（Pulse Width Modulation）波形のデューティー比を変化させて、制御回路２から指令を受けたエンコーダ３の制御目標値相当の角度だけモータ４を駆動する。また、モータサーボ回路１５は、エンコーダ３のパルス信号を制御回路２へ入力する。モータ４は、モータドライバ１６からの駆動信号に応答して回転する。すなわち、モータ４、モータサーボ回路１５、およびモータドライバ１６は、モータ４の位置制御サーボ系を構成している。
【００３４】
モータ４の回転は、図１および図２に示したギア５、モータ軸９、主動側プーリ６、および動力伝達部材であるワイヤ１２により、関節部１１に伝達される。これにより、関節部１１において、リンク１４（図１、図２参照）が回動する。また、リンク１４の回動による関節角度の変化は、ポテンショメータ８の出力電圧を変化させる。この出力電圧は、Ａ／Ｄ変換回路１７によりアナログ情報からデジタル情報に変換されて制御回路２に入力される。制御回路２は、このデジタル情報に基づいて、関節角度を特定する。
【００３５】
次に、関節角度の算出手順について説明する。ここでは、関節駆動装置１の構成が、たとえば、以下のとおりであるものとする。
【００３６】
・プーリの直径比：主動側プーリ６（直径１５ｍｍ）／従動側プーリ７＝３／２
・ワイヤ長：５０ｍｍ
・Ａ／Ｄ変換回路１７の分解能：１２ビット
・ギア５による減速割合：１／２５６
・エンコーダ３のパルス発生割合：１６パルス／１回転
［エンコーダ３のカウント値から関節角度を算出する手順］
主動側プーリ６と従動側プーリ７との直径比が３：２であり、ギア５によるモータ４の回転速度の減速割合が１／２５６であるため、ワイヤ１２およびギア５に弾性やバックラッシュがない理想的な回転伝達状態においては、モータ４が１回転するごとに、関節軸１０は１／３８４回転することになる（（２／３）×（１／２５６））。
【００３７】
モータ４に接続されたエンコーダ３は、モータ４が１回転する間に１６パルスをカウントアップするので、関節角度を基準とすれば、エンコーダ３の分解能は、５．８６×１０−２度となる（（１／３８４）×（１／１６）×３６０度）。
【００３８】
関節部１１の原点位置（関節角度＝０度）におけるエンコーダ３のカウント値をｎＥ０とし、ある時点でのエンコーダ３のカウント値をｎＥとすると、式１を用いることによって、エンコーダ３のカウント値ｎＥから関節角度θＥを算出できる。なお、式１においてＣＥはエンコーダ３のカウント値を関節角度に変換する係数である。この係数は、エンコーダ３の分解能により定まるため、たとえば、５．８６×１０−２度/パルスである。
【００３９】
（式１） θＥ＝ＣＥ(ｎＥ−ｎＥ０)
［ポテンショメータ８の出力値から関節角度を算出する手順］
ポテンショメータ８に接続されるＡ／Ｄ変換回路１７の分解能が１２ビットであるため、ポテンショメータ８による関節角度の分解能は８．７８×１０−２度である（３６０度／２１２）。
【００４０】
ポテンショメータ８のＡ／Ｄ変換後の値をｎＰ、関節部１１の原点位置におけるポテンショメータ８のカウント値をｎＰ０とすると、式２を用いることによって、ポテンショメータ８の出力値から関節角度θＰを計算できる。なお、式２において、ＣＰはＡ／Ｄ変換回路１７から出力されるパルス数を関節角度に変換する係数である。この係数は、ポテンショメータ８による関節角度の分解能により定まるため、たとえば、８．７８×１０−２度/パルスである。
【００４１】
（式２） θＰ＝ＣＰ(ｎＰ−ｎＰ０)
次に、関節駆動装置１の制御回路２が実行する各種の制御について、図４〜図６を参照して説明する。
【００４２】
図４は、初期化処理を説明するためのフローチャートである。この初期化処理は、エンコーダ３およびポテンショメータ８の出力値が正常値であるか否かをチェックするとともに、ポテンショメータ８に基づく式２の演算式のパラメータを補正し、さらには、関節駆動装置１の動力駆動機構のバックラッシュを推定するための処理である。
【００４３】
まず、内側に関節が回る方向、すなわち、リンク１４が図２に示したＲ１方向に回転するように、モータ４を駆動させる（Ｓ１）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもメカリミット３０にリンク１４が当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。
【００４４】
次に、ポテンショメータ８の出力値をデジタル変換した値ｎＰ＋を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ２)。
【００４５】
次に、前記式１を用いて、入力値ｎＰ＋に基づいた関節角度θＰ＋を算出する（Ｓ３）。次に、算出した関節角度θＰ＋が予め定めた正常範囲に収まっているか否かを判断する（Ｓ４）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θＰ＋が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θＰ＋の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ４の判断を行なう。
【００４６】
算出した関節角度θＰ＋が正常な範囲に収まっていないならば（Ｓ４でＮＯ）、初期化処理を中断する。このような減少が生じる原因としては、ポテンショメータ８に繋がる電気系統の切断あるいは故障、ポテンショメータ８の取付け位置の機械的なずれ、あるいは、モータ４の故障等が考えられる。
【００４７】
Ｓ４において、算出した関節角度θＰ＋が正常な範囲に収まっているならば（Ｓ４でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎＥ＋を入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ５)。
【００４８】
次に、外側に関節が回る方向、すなわち、リンク１４が図２に示したＲ２方向に回転するように、Ｓ１と逆方向にモータ４を駆動させる（Ｓ６）。このとき、移動目標となる関節角度を定めずに、モータ４が起動トルクＴ１を発生する一定電圧Ｖ１で、モータ４を連続して駆動させる。そして、そして、リンク１４が当初、いずれの関節角度の位置から回動し始めたとしてもリンク１４がメカリミット４０に当たる十分な時間Ｔ１が経過した時点で、モータ４の駆動を停止させる。
【００４９】
次に、ポテンショメータ８の出力値をデジタル変換した値ｎＰ−を、Ａ／Ｄ変換回路１７から入力し、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ７)。
【００５０】
次に、前記式２を用いて、入力値ｎＰ−に基づいた関節角度θＰ−を算出する（Ｓ８）。次に、算出した関節角度θＰ−が予め定めた正常範囲に収まっているか否かを判断する（Ｓ９）。たとえば、制御回路２のＲＯＭ２３内には、関節角度θＰ−が予め定めた正常範囲に収まっているか否かを判断するために用いる、関節角度θＰ−の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ９の判断を行なう。
【００５１】
算出した関節角度θＰ−が正常な範囲に収まっていないならば（Ｓ９でＮＯ）、初期化処理を中断する。一方、算出した関節角度θＰ−が正常な範囲に収まっているならば（Ｓ９でＹＥＳ）、モータサーボ回路１５を介してエンコーダ３の値ｎＥ−を入力し（図３参照）、制御回路２内のメモリ（ＲＡＭ２２）に記憶する(Ｓ１０)。
【００５２】
次に、Ｓ５およびＳ１０において入力されたエンコーダ３の値に基づいて、エンコーダ３の値が正常に変化しているか否かを判断する（Ｓ１１）。具体的には、Ｓ５において取得したエンコーダ３のパルス数ｎＥ＋とＳ１０において取得したエンコーダ３のパルス数ｎＥ−との差分ｎＥ＋−ｎＥ−が、リンク１４がメカリミット３０とメカリミット４０との間を回動する間にエンコーダ３によってカウントすることが期待されるパルス数に対して一定の範囲内に収まっているか否かを判別する。たとえば、制御回路２のＲＯＭ２３内には、エンコーダ３の閾値データが記憶されており、制御回路２は、この閾値データに基づいて、Ｓ１１の判断を行なう。
【００５３】
エンコーダ３のパルス数の差分(ｎＥ＋−ｎＥ−)が、期待される値の範囲内に無い場合（Ｓ１１でＮＯ）には、初期化処理を異常終了する。このような現象が起きる原因としてはエンコーダ３に繋がる電気系系統の切断、あるいはエンコーダ３やそれに関わる回路の故障が考えられる。
【００５４】
エンコーダ３のパルス数の差分が期待される値の範囲内にある場合（Ｓ１１でＹＥＳ）には、ギア等の動力駆動機構部分のがたつきによるバックラッシュ成分とワイヤ１２のコンプライアンス成分とによって生じる、アクチュエータ側と関節部側との間の角度差αを算出する(Ｓ１２)。
【００５５】
このバックラッシュ成分およびコンプライアンス成分が存在することによって、関節部１１が固定されている場合であってもモータ４が若干回転し、逆にモータ４の駆動が停止している場合であっても関節部１１が若干回転してしまう。そのため、アクチュエータ側の制御上の角度から計算で求められる関節角度と、関節部側の実際の関節角度との間で角度差αが生じる。この角度差αの大きさを特定することは、動力伝達系の制御の正確性を高めるために重要である。角度差αのうち、特にバックラッシュ成分に起因するものを、無負荷時のバックラッシュα０という。
【００５６】
ここで、Ｓ１２において制御回路２が実行する処理を詳細に説明する。
【００５７】
まず、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれでエンコーダ３から入力された値ｎＥ＋(Ｓ５で入力)，ｎＥ−(Ｓ１０で入力)に基づいて、前記式１を用いて関節角度を算出する。ｎＥ＋に基づいて算出された関節角度をθＥ＋とし、ｎＥ−に基づいて算出された関節角度をθＥ−とする。
【００５８】
次に、メカリミット３０にリンク１４を押し当てた状態とメカリミット４０にリンク１４を押し当てた状態とのそれぞれのポテンショメータ８の出力値（デジタル値）ｎＰ＋(Ｓ２で入力)，ｎＰ−(Ｓ７で入力)に基づいてＳ３およびＳ８ですでに算出されている関節角度θＰ＋およびθＰ−を読み出す。
【００５９】
最後に、「（θＥ＋−θＥ−）−(θＰ＋−θＰ−)」を実行することによって、角度差αを算出する。すなわち、角度差α＝（θＥ＋−θＥ−）−(θＰ＋−θＰ−)である。
【００６０】
ところで、図４に示される初期化処理中のエンコーダ３による関節角度換算値の変化量（θＥ＋−θＥ−）と、ポテンショメータ８による関節角度換算値の変化量(θＰ＋−θＰ−)との角度差αは、式３に示すように、バックラッシュα０の関数と考えることができる。
【００６１】
（式３） α(α０)＝（θＥ＋−θＥ−）−(θＰ＋−θＰ−)
モータ４に一定電圧Ｖ１を連続的に印加して、リンク１４をメカリミット３０または４０に押し付ける場合、関節角度換算値の変化量の差αはバックラッシュα０について単調増加になる。したがって電圧Ｖ１下におけるα０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα０を算出できる。
【００６２】
関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたα０とαとの関係を示すバックラッシュデータが記憶されている。初期化処理のＳ１２では、さらに、推定用データ記憶領域２３からバックラッシュデータが読出されて、算出した角度差αに対応するバックラッシュα０が算出される。
【００６３】
Ｓ１２でバックラッシュの推定処理が終了した後、θＰ＋，θＰ−と、メカリミット３０，４０にリンク１４が接している時の機械的にあるべき角度θ＋，θ−との比較により、ポテンショメータ入力値を関節角度に変換するために用いる前記式２のパラメータＣＰ，ｎＰ０の補正を行なう（Ｓ１３）。
【００６４】
具体的には、以下の式４および式５に従う演算をして、パラメータＣＰ，ｎＰ０を補正する。
【００６５】
（式４） ＣＰ＝（θ＋−θ−）／（ｎＰ＋−ｎＰ−）
（式５） ｎＰ０＝ｎＰ＋−（θ＋／ＣＰ）
以降、パラメータＣＰおよびｎＰ０は、変更せず、固定値とする。
【００６６】
なお、式４および式５は、既知の（θ＋，ｎＰ＋）および（θ−，ｎＰ−）の各々を、前記式２「θＰ＝ＣＰ(ｎＰ−ｎＰ０)」に代入して、
θ＋＝ＣＰ(ｎＰ＋−ｎＰ０)
θ−＝ＣＰ(ｎＰ−−ｎＰ０)
とし、これを連立方程式として、ＣＰおよびｎＰ０を導出するものである。
【００６７】
たとえば、伝達機構がマニプレータやロボットフィンガに搭載され機構の周囲をケース等でシールドされた状態であっても本実施の形態の手法は適用可能である。つまり、上記のような初期化処理を制御回路２が実行することにより、ワイヤ１２を直接観測できない条件下であっても簡易に伝達系のバックラッシュを推定することができるようになるので、メンテナンスが容易である。また、本実施の形態によれば、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、伝達系のバックラッシュを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００６８】
次に、図５を用いて、位置制御サーボ補正処理を説明する。この処理は、関節角度が所望の値になるように装置を制御するための処理である。
【００６９】
まず、ポテンショメータ８の値ｎＰを入力し、メモリ（ＲＡＭ２２）に記憶し、入力値に基づいて、前記式２によって関節角度θＰを算出する（Ｓ１０１）。関節部１１とポテンショメータ８は直接接続してあるので、θＰは実際の関節角度θを正確に反映している。なお、Ｓ１０１では、図４の初期化処理のＳ１３において補正されたパラメータＣＰおよびｎＰ０が用いられる。
【００７０】
次に、エンコーダ３のカウント値ｎＥを入力し、メモリ（ＲＡＭ２２）に記憶する（Ｓ１０２）。エンコーダ３と関節部１１との間には動力伝達系(ギア５およびワイヤ１２)が存在することから、エンコーダ３のカウント値ｎＥから前記式１を用いて算出した関節角度θＥは、実際の関節角度θを正確に反映していない。したがって、単にエンコーダ３のパルスカウントのみに基づいてサーボ形を構成した場合、リンク１４を目標の関節角度θの位置に正確に制御することができない。
【００７１】
そこで式６の計算により、エンコーダ３のパルスカウント原点を、初期値ｎＥ０から関節角度制御の回毎（ｉ回毎）に逐次変更して、ｎＥ０ｉに補正する(Ｓ１０３)。
【００７２】
（式６） ｎＥ０ｉ＝（１−ｋ）ｎＥ０ｉ−１＋ｋ（ｎＥ−θＰ／ＣＥ）
（ただし、ｉは自然数であり、ｎＥ００＝ｎＥ０である）
式６において、θＰ／ＣＥは、実際の関節角度θＰに対応するエンコーダ３のパルスカウント値である。したがって、ｎＥ−θＰ／ＣＥは、前回（ｉ−１回目）のエンコーダ３のパルスカウント値ｎＥと、実際にカウントされるべきエンコーダ３のパルスカウント値θＰ／ＣＥとの差、すなわち、今回（ｉ回目）の制御に際して、補正すべき原点の誤差である。
【００７３】
理論上は、この誤差（ｎＥ−θＰ／ＣＥ）を、前回のエンコーダ３のパルスカウント原点ｎＥ０ｉ−１に加算することによって、次回の制御に用いるパルスカウント原点を求めることができる。
【００７４】
しかしながら、このようなパルスカウント原点の補正処理が毎回行なわれることで処理Ｓ１０３の実行周期が大きくなると、一度の変更によるパルスカウント原点ｎＥ０の変化が大きくなり、制御系の振動が生じやすくなる。そこで、本実施の形態では、１以下の比例ゲインであるｋを式６に示すように採用し、ゲインｋを適切に設定して振動を抑制している。
【００７５】
以上の計算が終わった後、式７によって、次回の関節角度目標値θｉに制御するためのエンコーダ制御目標値ｎＥｉを算出する(Ｓ１０４)。
【００７６】
（式７） ｎＥｉ＝ｎＥ０ｉ＋θｉ／ＣＥ
この式７は、前記式１の「θＥ＝ＣＥ(ｎＥ−ｎＥ０)」から変形した式「ｎＥ＝ｎＥ０＋θＥ／ＣＥ」を応用したものである。
【００７７】
最後に、Ｓ１０４において算出したエンコーダ制御目標値ｎＥｉをモータサーボ回路１５に設定する（Ｓ１０５）。なお、モータサーボ回路１５は、モータドライバ１６を制御して、設定されたエンコーダ制御目標値に従う回転量だけモータ４を駆動させる。
【００７８】
以上のような制御を行なうことにより、ワイヤやギア等バックラッシュあるいはコンプライアンスをもった動力伝達形を持った装置であっても、振動を生じさせることなく正確に関節部１１を目標関節角度に制御することが可能になる。また、極力、アナログ入力を必要とせずに、正確に関節部１１を目標関節角度に制御することができるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００７９】
次に、図６を用いて、関節トルク推定処理を説明する。この処理は、図４の初期化処理が実行された後に、ポテンショメータ８およびエンコーダ３の出力値に基づいて、関節部１１のトルクを推定するための処理である。なお、この処理は、図５を用いて説明を行った関節角度の制御が実行されていないときに行なわれる。したがってパルスカウント原点を補正するＳ１０３の処理も実行しておらず、エンコーダ３のパルスカウント原点は一定である。
【００８０】
まず、ポテンショメータ８の値ｎＰを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０１)。次に、エンコーダ３の値ｎＥを入力して、メモリ（ＲＡＭ２２）に記憶する(Ｓ２０２)。
【００８１】
次に、記憶したポテンショメータ８の値ｎＰを前記式２により関節角度θＰに、記憶したエンコーダ３の値ｎＥを前記式１により関節角度θＥに、それぞれ変換し、その角度差θＰ−θＥを計算する(Ｓ２０３)。
【００８２】
ポテンショメータ８およびエンコーダ３の値に基づいて算出した関節角度の角度差αは、ワイヤ１２およびギア５からなる動力伝達系のコンプライアンス性およびバックラッシュによって生じるものと考えてよい。これを図示すると、図７のようになる。
【００８３】
図７は、関節部１１の負荷トルクＴと角度差α（θＰ−θＥ）との関係を示している。理想的に動力伝達系のバックラッシュα０が存在しない場合には、両者の関係は、図中の直線Ｃ２で表される。しかしながら、実際にはバックラッシュα０が存在するために、両者の関係は直線では表されず、角度差軸部分で断続する図中のＣ１のような関係をなす。
【００８４】
バックラッシュα０の角度差軸方向の中央値は、エンコーダ３の原点ｎＥ０をどのタイミングで初期化したかによって変化する。エンコーダ３のパルスカウント原点は、前記式６によって、逐次、補正されるが、初期化したときの原点の初期値ｎＥ０は、以下のようにして求められる。
【００８５】
前記式２「θＰ＝ＣＰ(ｎＰ−ｎＰ０)」を用いて、ポテンショメータ値ｎＰから角度θＰを算出し、θＰをθとする。
【００８６】
前記式１「θＥ＝ＣＥ(ｎＥ−ｎＥ０)」を「ｎＥ０＝(ｎＥ−θＥ／ＣＥ)」と変形した式に、エンコーダ値ｎＥを代入し、さらに、θＥに算出したθを代入し、初期値ｎＥ０を求める。
【００８７】
このように、エンコーダ３のパルスカウント原点を初期化したときに、バックラッシュα０が、初期化されたパルスカウント原点に対していずれの側に偏っているかを判別できないために、Ｃ１はグラフ原点を中心として、角度差軸方向にバックラッシュα０分の不確定性を持つことになる。
【００８８】
Ｃ１が角度差軸方向に確定していれば、関節部１１の負荷トルクが０の場合以外は、角度差θＰ−θＥと関節部１１の負荷トルクＴとの関係が一意に決まるので、Ｓ２０３によって算出した角度差θＰ−θＥから関節部１１の負荷トルクＴを推定することができるようになる。
【００８９】
関節駆動装置１のＲＯＭ２１内の推定用データ記憶領域２３（図３参照）には、事前に検定しておいたＣ１に従う、角度差θＰ−θＥと関節部１１の負荷トルクＴとの関係を示す負荷トルクデータが記憶されている。Ｓ２０４では、推定用データ記憶領域２３から負荷トルクデータを読出して、Ｓ２０３で算出した角度差θＰ−θＥに対応する関節部１１の負荷トルクＴを算出する。エンコーダ３およびポテンショメータ８それぞれの停止が推定された後、処理Ｓ２０１〜Ｓ２０４によりトルクの推定を行ない、モータ４の電圧の調整を行なう。
【００９０】
なお、図６に示される関節トルク推定処理の前に、図４に示される初期化処理が実行されるために、図４に示されるフローチャートにおいて、モータ４に発生させる動力を可能な限り低減させた状態で角度差αが推定された後（図４のＳ１２）、関節部１１にかかるトルクが図６に示されるフローチャートによって推定されることになる。
【００９１】
このように、本実施の形態の場合には、ワイヤ張力等、ワイヤ１２を直接観測することによって得られるアナログ入力を必要とすることなく、関節部１１の負荷トルクＴを推定できるために、関節部の制御の正確性を高めながらも、極力、アナログ情報の入力数を低減し得る。
【００９２】
図７のグラフ中のＣ１は、たとえば、モータ４の出力トルクを複数段階に変化させながら角度差θＰ−θＥを計測することにより特定することが可能である。しかしながら、モータ４の出力トルクを変更してからその値が安定するのを待ってからデータを取得するという手順では、作業に時間がかかるという問題がある。実用的な手法として以下のような方法がある。
【００９３】
まず、バックラッシュα０を算出する。バックラッシュα０は、前記式３を利用して算出できる。すなわち、前述したように、電圧Ｖ１下におけるα０とαとの関係を事前に検定しておくことにより、関節角度換算値の変化量の差αからバックラッシュα０を算出できる。これにより、角度差軸上の正の値と負の値との２点を特定できる。
【００９４】
次に、トルクを計測する直前までは関節部１１を駆動する最低限のトルクＴ２でモータ４を回転させる。やがて、ポテンショメータ８の値θＰの変化がなくなり、エンコーダ３の値θＥの変化がなくなった時点での角度差θＰ−θＥを計算する。そして、計算した角度差θＰ−θＥとトルクＴ２とに基づいて、図７のグラフに点をプロットする（たとえば、図７のＰＴ）。これにより、Ｃ１を確定させることができる。
【００９５】
以上、説明した本実施の形態では、制御に必要なアナログ入力数は、関節角度検出用センサがポテンショメータであれば１であり、従来技術として説明した図８の構成に比較すると、極力、アナログ入力数を減少させることができる。
【００９６】
同様に、アクチュエータ側および関節部側のうちのいずれか一方の回転角度センサを、ポテンショメータ等アナログ出力を持ったセンサで構成し、もう一方の回転角度センサをロータリエンコーダによって構成した場合であっても、アナログ入力はアクチュエータごとに１個となるので、従来の構成以下のアナログ入力数で、バックラッシュ推定、関節部側トルク推定等が可能になり、制御の正確性を高めることができる。
【００９７】
また、本実施の形態において、ポテンショメータ８に代えて、アブソルート型エンコーダのようなデジタル的な絶対角度検出手段を用いてもよく、この場合には、制御に必要な入力をすべてデジタル入力とすることが可能になる。
【００９８】
同様に、アクチュエータ側および関節側の回転角度センサを両方ともロータリエンコーダのようなデジタル出力を持ったセンサによって構成すれば、ハンドやマニプレータの制御回路に代表される関節駆動装置は、アナログ入力を必要としない。
【００９９】
その結果、Ａ／Ｄ変換回路を減少あるいは不要とすることができ、コンパクトかつ高性能な関節駆動装置を容易かつ安価に構成することができる。
【０１００】
また、本実施の形態では、ワイヤやベルト等弾性を持った動力伝達機構で接続されたアクチュエータおよび関節部それぞれに角度センサを取付ける。そして、関節部を所望の角度まで回転させるために、アクチュエータのサーボ系をアクチュエータ本体と、アクチュエータの回転角度を計測する角度センサにより構成している。これにより、動力伝達機構に弾性があっても剛性の高い制御を行なうことができる。
【０１０１】
さらに、本実施の形態では、上記サーボ系の目標値を、関節部側に取付けた角度センサの読み値と回転型アクチュエータ側に取付けた角度センサの読み値の差分に比較的低いゲインをかけたもので逐次修正することにより、関節部が目標角度に到達することを保証している。そして、上記のような構成をとることにより、動力伝達部材の弾性の影響下であっても振動を生じさせることなく正確に関節部の角度位置決めを行なうことができるようになる。
【０１０２】
本発明の関節駆動装置１をロボットフィンガに用いる場合、無負荷状態に置いて回転型アクチュエータの回転角度の読み値と関節部の回転角度の読み値とを一致させておけば、回転型アクチュエータ側の回転角度センサ読み値と、関節側の回転角度センサ読み値との差は、その中間にある弾性を持った動力伝達部材の変形によって生じたものと考えられるようになる。
【０１０３】
したがって、回転型アクチュエータ側の回転角度センサの読み値と、関節側の回転角度センサの読み値との差は、関節部に働くトルクを反映して生じているものと捉えられるので、関節部に働くトルクと回転角度センサの読み値の差との関係を事前に測定しておけば回転角度センサの読み値の差に基づいて関節部に働くトルクを検出することができる。
【０１０４】
以上をまとめると、ワイヤあるいはベルト等弾性を持った動力伝達部材により動力伝達を行なうハンドやマニプレータに本発明を適用することにより、振動を発生させることなく確実に関節角度の位置決めを行なう位置制御が可能になり、また、関節部に働くトルクの制御をすることが可能になる。
【０１０５】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【図面の簡単な説明】
【０１０６】
【図１】関節駆動装置の平面図である。
【図２】関節駆動装置を図１の矢印Ｌ方向から見た図である。
【図３】関節駆動装置の構成を示すブロック図である。
【図４】初期化処理を説明するためのフローチャートである。
【図５】位置制御サーボ補正処理を説明するためのフローチャートである。
【図６】関節トルク推定処理を説明するためのフローチャートである。
【図７】関節部の負荷トルクと角度差との関係を示す図である。
【図８】従来の関節駆動装置を示す図である。
【符号の説明】
【０１０７】
１ 関節駆動装置、２ 制御回路、 ２０ ＣＰＵ、２１ ＲＯＭ、２２ ＲＡＭ、３ エンコーダ、４ モータ、５ ギア、６ 主動側プーリ、７ 従動側プ−リ、８ ポテンショメータ、９ モータ軸、１０ 関節軸、１１ 関節部、１２ ワイヤ、１３ フレーム、１４ リンク、１５ モータサーボ回路、１６ モータドライバ、１７ Ａ／Ｄ変換回路、３０ メカリミット（内側）、４０ メカリミット（外側）。
【特許請求の範囲】
【請求項１】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、
前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、
前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備えることを特徴とする、関節駆動装置。
【請求項２】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、
該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、
該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備えることを特徴とする、関節駆動装置。
【請求項３】
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備えることを特徴とする、関節駆動装置。
【請求項４】
前記動力側回転角度検出手段と前記関節側回転角度検出手段とのうち少なくとも一方は、検出値として出力される情報がデジタル情報である回転角度センサであることを特徴とする、請求項１〜請求項３のいずれかに記載の関節駆動装置。
【請求項５】
前記動力側回転角度検出手段は、前記回転型アクチュエータの一部を構成するモータに取付けた相対角度検出センサであり、
前記関節側回転角度検出手段は、絶対角度検出型センサであることを特徴とする、請求項１〜請求項４のいずれかに記載の関節駆動装置。
【請求項６】
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材とをさらに備え、
前記制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とをさらに備え、
前記負荷トルク特定手段は、前記角度差算出手段が前記動力伝達前後角度差を算出した後に、前記負荷トルクを特定することを特徴とする、請求項１に記載の関節駆動装置。
【請求項７】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出ステップと、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出ステップと、
前記第１算出ステップの算出値と前記第２算出ステップの算出値との角度差を算出する角度差算出ステップと、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する手段であって前記関節駆動装置が備える記憶手段に記憶されている負荷トルクデータと、前記角度差算出ステップにより算出された角度差とから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定ステップとからなることを特徴とする、関節駆動装置の制御方法。
【請求項８】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正ステップと、
該基準値補正ステップにより補正された基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出ステップと、
該制御量算出ステップにより算出された制御量に基づいて前記回転型アクチュエータを駆動する駆動制御ステップとからなることを特徴とする、関節駆動装置の制御方法。
【請求項９】
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段とを備える関節駆動装置の制御方法であって、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出ステップと、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出ステップと、
前記第１正方向最大角算出ステップの算出値と前記第１逆方向最大角算出ステップの算出値との差数と、前記第２正方向最大角算出ステップの算出値と前記第２逆方向最大角算出ステップの算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段ステップとからなることを特徴とする、関節駆動装置の制御方法。
【請求項１】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出手段と、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出手段と、
前記第１算出手段の算出値と前記第２算出手段の算出値との角度差を算出する角度差算出手段と、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する記憶手段と、
前記角度差算出手段により算出された角度差と、前記記憶手段に記憶されている負荷トルクデータとから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定手段とを備えることを特徴とする、関節駆動装置。
【請求項２】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正手段と、
該基準値補正手段が補正した基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出手段と、
該制御量算出手段が算出した制御量に基づいて前記回転型アクチュエータを駆動する駆動制御手段とを備えることを特徴とする、関節駆動装置。
【請求項３】
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備え、
該制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とを備えることを特徴とする、関節駆動装置。
【請求項４】
前記動力側回転角度検出手段と前記関節側回転角度検出手段とのうち少なくとも一方は、検出値として出力される情報がデジタル情報である回転角度センサであることを特徴とする、請求項１〜請求項３のいずれかに記載の関節駆動装置。
【請求項５】
前記動力側回転角度検出手段は、前記回転型アクチュエータの一部を構成するモータに取付けた相対角度検出センサであり、
前記関節側回転角度検出手段は、絶対角度検出型センサであることを特徴とする、請求項１〜請求項４のいずれかに記載の関節駆動装置。
【請求項６】
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材とをさらに備え、
前記制御手段は、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出手段と、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出手段と、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出手段と、
前記第１正方向最大角算出手段の算出値と前記第１逆方向最大角算出手段の算出値との差数と、前記第２正方向最大角算出手段の算出値と前記第２逆方向最大角算出手段の算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段とをさらに備え、
前記負荷トルク特定手段は、前記角度差算出手段が前記動力伝達前後角度差を算出した後に、前記負荷トルクを特定することを特徴とする、請求項１に記載の関節駆動装置。
【請求項７】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１算出ステップと、
前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２算出ステップと、
前記第１算出ステップの算出値と前記第２算出ステップの算出値との角度差を算出する角度差算出ステップと、
前記角度差と前記関節側回転部材に対する負荷トルクとの関係を示す負荷トルクデータを予め記憶する手段であって前記関節駆動装置が備える記憶手段に記憶されている負荷トルクデータと、前記角度差算出ステップにより算出された角度差とから、前記関節側回転部材に対する負荷トルクを特定する負荷トルク特定ステップとからなることを特徴とする、関節駆動装置の制御方法。
【請求項８】
関節部を回動させるための関節側回転部材と、
該関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段と、
前記回転型アクチュエータを制御する制御手段とを備える関節駆動装置の制御方法であって、
前記動力側回転角度検出手段の検出出力と、前記関節側回転角度検出手段の検出出力とに基づいて、前記回転型アクチュエータの回転角を制御するために用いる基準値を補正する基準値補正ステップと、
該基準値補正ステップにより補正された基準値を基準として、前記関節側回転部材の回転角を予め定めた目標角度に制御するために必要となる前記回転型アクチュエータの制御量を算出する制御量算出ステップと、
該制御量算出ステップにより算出された制御量に基づいて前記回転型アクチュエータを駆動する駆動制御ステップとからなることを特徴とする、関節駆動装置の制御方法。
【請求項９】
関節部を正逆両方向に回動させるための関節側回転部材と、
前記関節部の正方向への所定角以上の回動を規制する正方向規制部材と、
前記関節部の逆方向への所定角以上の回動を規制する逆方向規制部材と、
前記関節側回転部材へ動力を供給するための回転型アクチュエータと、
該回転型アクチュエータと前記関節側回転部材との間に掛け渡されて前記回転型アクチュエータの動力を前記関節側回転部材へ伝達する、弾性を有する動力伝達部材と、
前記回転型アクチュエータの回転角を検出するための動力側回転角度検出手段と、
前記関節側回転部材の回転角を検出するための関節側回転角度検出手段とを備える関節駆動装置の制御方法であって、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記動力側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第１逆方向最大角算出ステップと、
前記関節部が前記正方向規制部材によって規制される最大限まで前記正方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２正方向最大角算出ステップと、
前記関節部が前記逆方向規制部材によって規制される最大限まで前記逆方向に回動しているときの前記関節側回転角度検出手段の検出出力に基づいて、前記関節側回転部材の回転角を算出する第２逆方向最大角算出ステップと、
前記第１正方向最大角算出ステップの算出値と前記第１逆方向最大角算出ステップの算出値との差数と、前記第２正方向最大角算出ステップの算出値と前記第２逆方向最大角算出ステップの算出値との差数との差を算出することによって、前記動力伝達部材のコンプライアンス成分および前記回転型アクチュエータのバックラッシュ成分によって生じる動力伝達前後角度差を算出する角度差算出手段ステップとからなることを特徴とする、関節駆動装置の制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００６−５５９２７（Ｐ２００６−５５９２７Ａ）
【公開日】平成１８年３月２日（２００６．３．２）
【国際特許分類】
【出願番号】特願２００４−２３８５７２（Ｐ２００４−２３８５７２）
【出願日】平成１６年８月１８日（２００４．８．１８）
【出願人】（０００００５０４９）シャープ株式会社 (33,933)
【Ｆターム（参考）】
【公開日】平成１８年３月２日（２００６．３．２）
【国際特許分類】
【出願日】平成１６年８月１８日（２００４．８．１８）
【出願人】（０００００５０４９）シャープ株式会社 (33,933)
【Ｆターム（参考）】
[ Back to top ]