パラレル型リンクロボットにおける関節角の誤差検知装置
【課題】パラレル型リンクロボットにおいて、その関節角を検出する検出部の検出誤差を的確に検知する。
【解決手段】ｎ角形リンク機構（ｎは４以上の整数）を有するパラレル型リンクロボットにおいて、関節角の検出部における検出誤差を検知する誤差検知装置であって、ｎ角形リンク機構を構成する関節部のうち、ｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、関節角検出部によって検出されたｎ−１個の関節部の関節角とに基づき、関節角における角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、角度誤差パラメータ取得部によって取得されたパラメータに基づいて、ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、を備える。
【解決手段】ｎ角形リンク機構（ｎは４以上の整数）を有するパラレル型リンクロボットにおいて、関節角の検出部における検出誤差を検知する誤差検知装置であって、ｎ角形リンク機構を構成する関節部のうち、ｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、関節角検出部によって検出されたｎ−１個の関節部の関節角とに基づき、関節角における角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、角度誤差パラメータ取得部によって取得されたパラメータに基づいて、ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、４本以上のリンク部材と対応する関節部とで構成された閉ループのリンク機構を有するパラレル型リンクロボットに関する。
【背景技術】
【０００２】
一般に、複数のリンク部材とそれらを可動的につなぐ関節部からなるロボットマニピュレータの構造は、シリアル型とパラレル型の２つに大別することができる。シリアル型はベースからエンドエフェクタまで一自由度の関節部が直列に繋がり、各関節部がアクチュエータにより駆動される。このようにシリアル型のマニピュレータは、構造は簡単であるが、アクチュエータなどの質量が各リンク部材に加算され各関節部が曲げモーメントを受けるため、マニピュレータ全体の剛性も低下し、精度が低く、可搬重量が小さいなどの問題がある。
【０００３】
一方、パラレル型はそれを構成するリンク部材が閉ループを成すため、マニピュレータ全体の剛性も高くでき、その結果、アクチュエータ誤差が小さく、高精度の位置決めが可能となる。基本的に、エンドエフェクタに対して複数のリンク部材が支持する形となるため、各関節部にかかる分担過重もシリアル型に比べて小さくなり、以てリンク部材の軽量化もはかることができる。このように、パラレル型のロボットは多くの利点を有することから、広く普及しつつあり、また国内外の多くの研究機関において精力的に研究がなされている（例えば、特許文献１、２、非特許文献１を参照。）。
【特許文献１】特開２００２−２９５６２３号公報
【特許文献２】特開２００３−９４２６６号公報
【非特許文献１】舟橋 宏明，“ロボット機構としてのパラレルメカニズム”日本ロボット学会誌， Vol.10，No.6，pp699-704，1992
【発明の開示】
【発明が解決しようとする課題】
【０００４】
このようにパラレル型のリンクロボット（以下、単に「パラレルロボット」ともいう。）では、一般に、それを構成するリンク部材が閉ループを構成する。話を簡潔にするために、パラレルロボットで構成される当該閉ループが多角形構造を有するものとする。例えば、五本のリンク部材と五個の関節部とで構成されるパラレルロボットでは、五角形の閉ループが構成されることになる。このような閉ループの多角形構造を有するパラレルロボットでは、アクチュエータによって稼動される必要最少の関節部の数は、関節部の総数をｎ個（ｎは４以上の整数）とすると、ｎ−３個となる。したがって、エンドエフェクタの位置制御などのパラレルロボットの制御を行う場合、アクチュエータによって駆動される関節部の関節角を、最低限検出し、それを当該制御にフィードバック等すればよい。
【０００５】
しかし、上記ロボットの制御をより正確に行うため、例えば、駆動軸のアクチュエータの回転制御だけでなく、最終的に位置決めされるべきエンドエフェクタの位置の精度を高めるために、アクチュエータが設置されていない関節部の関節角を検出するための検出部も設け、それをロボットの制御に利用する場合がある。また、このようなパラレルロボットにおいては、各関節部の関節角を検出するためにロータリエンコーダ等の検出装置が検出部として利用される。
【０００６】
しかし、当然ながら、これらの関節部の関節角の検出部が故障、もしくは故障までに至らなくともその検出誤差が大きくなりすぎると、ロボット制御への影響が無視できなくなり、場合によってロボットの駆動が困難となる場合がある。したがって、パラレルロボッ
トにおいて、その関節角検出部の検出誤差が適正なものか否か的確に判断する必要があるが、それについて十分な考察が為されていないのが現状である。
【０００７】
本発明では、上記した問題に鑑み、パラレルロボットにおいて、その関節角を検出する検出部の検出誤差を的確に検知する誤差検知装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
そこで、上記の課題を解決するために、本発明では、パラレルロボットの構造的条件、すなわち幾何学的条件を利用することで、関節角の検出部の誤差検知を的確に行うことを可能とした。より詳細には、本発明に係るパラレル型リンクロボット（パラレルロボット）における関節角の誤差検知装置（以下、単に「誤差検知装置」ともいう。）は、ｎ個以上（ｎは４以上の整数）のリンク部材と該リンク部材の各々を連結する関節部とで形成されるｎ角形リンク機構を有するパラレル型リンクロボット（パラレルロボット）において、該リンク機構の関節部の角度を検出する検出部における検出誤差を検知する誤差検知装置であって、前記ｎ角形リンク機構を構成する関節部のうち、一つの関節部を除くｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、前記ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、前記関節角検出部によって検出された前記ｎ−１個の関節部の関節角とに基づき、前記ｎ−１個の関節部の前記関節角検出部によって検出された関節角と、該ｎ−１個の関節部の実際の関節角との間に生じる角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、前記角度誤差パラメータ取得部によって取得されたパラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、を備える。
【０００９】
上記誤差検知装置では、角度誤差パラメータ取得部が、関節部について、関節角検出部によって検出された関節角（検出値）と、実際の関節角との差に相当する角度誤差に関連するパラメータを、パラレルロボットのｎ角形リンク機構の構造的条件の下、すなわち各リンク部材と各関節部とで決定されるｎ角形リンク機構の幾何学的要素を考慮した上で、各リンク部材の長さと関節角の検出値とに基づいて取得する。言い換えるならば、角度誤差パラメータ部は、パラレルロボットの構造的な拘束条件であるリンク部材と関節部に対して、必ずしもこの拘束条件に縛られるとは限らない関節角の検出値（すなわち検出部による検出誤差が大きくなると、本来的には当該拘束条件に縛られるべき関節角の値が、その拘束条件から外れた値へと移っていくことを意味する。）を重ね合わせることで、ｎ角形リンク機構の構造上、矛盾が生じる状態となる。その矛盾を上記角度誤差に関するパラメータとして、角度誤差パラメータ取得部が取得する。
【００１０】
そして、誤差判定部が、角度誤差パラメータ取得部が取得したパラメータに基づいて、パラメータを算出するために用いられた関節部の関節角の検出値のうち、何れの検出値において誤差が含まれているかを判定する。なお、角度誤差パラメータ部によるパラメータ取得には、ｎ角形リンク機構における全ての関節部の関節角の検出値が必要ではなく、ｎ−１個の関節角の検出値で必要十分である。これは、ｎ角形リンク機構の構造的条件を利用することで、上記パラメータを的確に算出できるからである。その結果、関節角検出部が検出すべき関節部の数が少なくでき、以てパラレルロボットの構成を簡潔なものとしながらも、各関節角の検出、および関節角検出における誤差検知を的確に行うことができるのである。
【００１１】
なお、上記の構成は、パラレルロボットのｎ角形リンク機構において、全ての関節部の関節角を検出部によって検出し、該パラレルロボットの制御等に利用することを妨げるものではない。あくまでも、本発明にかかる誤差検知装置では、ｎ角形リンク機構を構成するｎ個の関節部のうちｎ−１個の関節部の検出値を利用することで、当該ｎ−１個の関節
部における関節角の検出誤差を的確に検知できる点が重要である。
【００１２】
ここで、上記誤差検知装置において、前記角度誤差パラメータ取得部は、前記角度誤差に関するパラメータを算出するための、ｎ−２個の所定の算出式に従い、該パラメータを取得するように構成されてもよい。そして、その場合、前記ｎ−２個の所定の算出式は、前記ｎ角形リンク機構の構造的条件を前提として、前記ｎ角形リンク機構の各リンク部材の長さと、前記検出されたｎ−１個の関節部の関節角とから設定され、更に、以下の(a)
の条件を満たす。
(a)各所定の算出式に含まれる関節角の数はｎ−２個であって、所定の算出式間同士に
含まれる関節角の組合せは完全一致しないこと。
更に、前記誤差判定部については、前記ｎ−２個の所定の算出式から算出された各算出値の組合せを前記パラメータとして、該パラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断が行われる。
【００１３】
上記ｎ−２個の所定の算出式が、上記の条件(a)を満たしながら、多角形リンク機構の
構造的条件に対して、関節角検出部によって検出されたｎ−１個の関節角を重ね合わせることで、各算出式の算出値の組合せを誤差判定のためのパラメータとすることができる。すなわち、各算出式の算出値が、特定のｎ−２個の関節部の関節角とｎ角形リンク機構の構造的条件から導かれる拘束条件を満たしているか否かについて反映する値となるべく、上記条件(a)が設定される。
【００１４】
また、上記誤差検知装置において、前記角度誤差パラメータ取得部は、それぞれが前記ｎ角形リンク機構における構造的条件の下、前記リンク部材と二つの関節を結んで形成される仮想辺とで区分される仮想三角形での、該リンク部材の長さおよび該仮想辺の長さと前記検出された関節角との相関関係に基づいた前記ｎ−２個の所定の算出式にしたがって、前記パラメータを取得するようにしてもよい。すなわち、ｎ角形リンク機構の構造的条件として、該ｎ角形リンク機構内に形成される仮想三角形成立のための条件を利用するものである。
【００１５】
そして、リンク部材の長さおよび仮想辺の長さと検出関節角との相関関係において、検出関節角が実際の関節角と誤差がない場合には、ｎ角形リンク機構の構造的条件に対して矛盾するパラメータが、算出式から導かれない。しかし、検出関節角が実際の関節角に対して誤差が生じている場合には、上記仮想三角形の成立条件に矛盾が生じ、以てｎ角形リンク機構の構造的条件に対して矛盾するパラメータが、算出式から導かれることになる。それにより、誤差判定部が、ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行うことが可能となる。
【００１６】
ここで、上述までの誤差検知装置において、前記誤差判定部により、前記ｎ−１個の関節部のうち何れかの関節部に角度誤差が生じていると判定されたとき、該角度誤差が生じたとされた関節部の前記関節角度検出部によって検出された関節角を、該角度誤差が生じたとされた関節部を除く一部又は全部の関節部の該関節角度検出部によって検出された関節角と前記ｎ角形リンク機構における構造的条件とに従って修正する関節角修正部を、更に備えるようにしてもよい。このようなｎ角形リンク機構の構造的条件に基づく関節角修正部を有することにより、角度誤差が生じている関節部の関節角を正確に補完することができ、例えば発生した角度誤差がパラレルロボットの運転制御に与える影響を可及的に抑制することができる。
【発明の効果】
【００１７】
パラレルロボットにおいて、誤差検知装置により、その関節角を検出する検出部の検出誤差を的確に検知することが可能となる。
【発明を実施するための最良の形態】
【００１８】
以下に、本発明に係る誤差検知装置を備えるパラレルロボット１について図面に基づいて説明する。パラレルロボット１は、コの字形状をしたベースリンク部材Ｌ１が最下部に位置するリンク部材として設けられ、このベースリンク部材Ｌ１は、図示されてはいないパラレルロボット１の台座に対して、回転軸Ｚを介して取り付けられている。したがって、パラレルロボット１は、回転軸Ｚを介して台座に対してその全体の向きを旋回させることが可能となる。
【００１９】
ここで、ベースリンク部材Ｌ１の右側端部には、それに対して回動可能となるように関節部Ｐ１を介して、リンク部材Ｌ５が取り付けられ、一方、ベースリンク部材Ｌ１の左側端部には、それに対して回動可能となるように関節部Ｐ２を介して、リンク部材Ｌ２が取り付けられている。更に、リンク部材Ｌ５の、ベースリンク部材Ｌ１と接続されていない側の端部には、それに対して可動可能となるように関節部Ｐ５を介して、リンク部材Ｌ４が取り付けられており、また、リンク部材Ｌ２の、ベースリンク部材Ｌ１と接続されていない側の端部には、それに対して可動可能となるように関節部Ｐ３を介して、リンク部材Ｌ３が取り付けられている。更に、リンク部材Ｌ３、Ｌ４の、それぞれ関節部Ｐ３、Ｐ５に接続されていない端部同士は、関節部Ｐ４を介して接続され、両リンク部材が回動可能状態となっている。また、この関節部Ｐ４に支持されるように取付テーブル２が設けられ、該取付テーブル２の上方には、パラレルロボット１のエンドエフェクタ３が設置される。
【００２０】
このように構成されるパラレルロボット１は、ベースリンク部材Ｌ１と４本のリンク部材Ｌ２〜Ｌ５の合計五本のリンク部材によって形成される五角形のリンク機構を有することになる。そして、各リンク部材の位置や姿勢が制御されて、この五角形リンク機構が変形し、関節部Ｐ４に設けられた取付テーブル２の位置と姿勢が制御され、以てエンドエフェクタ３の動作領域が制御されることになる。ここで、五角形リンク機構を有するパラレルロボット１では、そのリンク機構の構造的条件により、五つの関節部Ｐ１〜Ｐ５の全てをアクチュエータによって駆動しなくても、エンドエフェクタ３の位置や姿勢を制御できる。具体的には、一般にｎ角形（ｎは４以上の整数）のリンク機構を有するパラレルロボットでは、最小でもｎ−３個の関節部をアクチュエータで駆動しさえすれば、各関節部の位置を制御することが可能である。したがって、本実施例では、図１に示すリンク機構は５角形リンク機構であるから、エンドエフェクタ３の位置・姿勢制御用として、関節部Ｐ１、Ｐ２の２箇所にアクチュエータである回転モータを設置する。図１においては、このモータをそれぞれＭ１、Ｍ２で表している。
【００２１】
更に、パラレルロボット１においては、各関節部Ｐ１〜Ｐ５のうち、関節部Ｐ５を除く４箇所の関節部に、その関節部の関節角（該関節部に接続される二本のリンク部材によって形成される角度）を検出するための角度センサ（具体的には、ロータリエンコーダ等が使用可能であり、以下、単に「センサ」という。）が設けられている。具体的には、関節部Ｐ１、Ｐ２、Ｐ３、Ｐ４には、それぞれセンサＳ１、Ｓ２、Ｓ３、Ｓ４がそれぞれ対応するように設けられ、各関節角をθ１、θ２、θ３、θ４として検出される。
【００２２】
上記の通り、五角形リンク機構においては、五つの関節角のうち二つが決定されると、残りの三つの関節角も決定される。しかし、上記のようにモータＭ１、Ｍ２に対応する関節部Ｐ１、Ｐ２以外の関節部Ｐ３、Ｐ４にも関節角検出のためのセンサを冗長的に設けているのは、これらの関節角を検出しモータの出力にフィードバックすることで、エンドエフェクタ３の位置制御等をより正確に行うためである。なお、このような冗長的な検出部を利用したロボット制御は、従来から行われているものであるから、本明細書ではその詳細は割愛する。
【００２３】
このように構成されるパラレルロボット１を制御するために、制御装置１０がパラレルロボット１に対してケーブルで接続されている。制御装置１０は一般的なコンピュータによって構成され、これにより制御装置１０は、パラレルロボット１が有する各センサからの出力値を見ながら、エンドエフェクタ３が所望の位置や姿勢に至るように、モータＭ１、Ｍ２に対して駆動指令を出力する。ここで、図２に、この制御装置１０の機能をイメージ化して機能ブロックとして示す。以下に、図２に基づいて制御装置１０の詳細について説明する。
【００２４】
制御装置１０では、パラレルロボット１のセンサＳ１〜Ｓ４からの検出値が、センサ誤差検知部２０とロボット制御部３０とに引き渡される。センサ誤差検知部２０は、本発明に係る誤差検知装置を形成するための主な機能部である。そして、ロボット制御部３０は、上述したような各センサからの検出値を基に、パラレルロボット１を制御するための種々の制御を行う機能部であるが、それらの技術は本発明に係る誤差検知装置と直接的な関係性は小さく、また従来からの技術においても実現されていることであるので、本明細書ではその説明は割愛する。
【００２５】
ここで、センサ誤差検知部２０は、関節角検出値取得部２１と、誤差パラメータ取得部２２と、誤差判定部２３と、関節角修正部２４を有する。以下、各機能部の説明をするが、本発明の理解を促進するために、図１に示した構造を有するパラレルロボット１をモデル化したものを図３に示す。このようにパラレルロボット１のモデル化にあたり、必要でない要素（例えば、回転軸Ｚ等）については、図３では記載を省略した。また、各リンク部材の参照番号Ｌ１〜Ｌ４は、そのリンク部材の長さを表す記号としても用いることとする。ここで、リンク部材の長さは、当該リンク部材に接続される二つの関節部同士の間の距離と定義する（後述する仮想辺についても同様）。したがって、ベースリンク部材Ｌ１のようにコ字形状を有しているものの長さは、その形状に沿った長さではなく、そこに接続された関節部Ｐ１とＰ２の二点間の距離となる。また、図３においては、センサが設置されている関節部Ｐ１〜Ｐ４を黒塗りの丸印で表し、センサが設けられていない関節部Ｐ５を白塗りの丸印で表している。
【００２６】
まず、関節角検出値取得部２１は、パラレルロボット１に設置されたセンサＳ１〜Ｓ４により、対応する各関節部の関節角を取得する。具体的には、関節角検出値取得部２１は、センサＳ１より関節角θ１を、センサＳ２より関節角θ２を、センサＳ３より関節角θ３を、センサＳ４より関節角θ４を、それぞれ取得する。
【００２７】
次に、誤差パラメータ取得部２２は、関節角検出値取得部２１によって取得された各関節角θ１〜θ４と、各リンク部材Ｌ１〜Ｌ５で構成される図３に示す五角形リンク機構の構造的条件に基づいて、センサが取り付けられた関節部における関節角の検出値と実際の関節角の値との誤差に関連するパラメータを取得する。ここで、五角形リンク機構の構造的条件とは、各リンク部材の長さと、それらをつなぐ各関節部の配置条件であり、五角形リンク機構の形状を決定するための物理的、構造的拘束条件である。また、上記パラメータは、この五角形リンク機構の構造的条件をベースとして、算出される誤差検知のためのパラメータである。以下に、図４Ａ〜図４Ｃに基づいてこのパラメータを算出するための三つの算出式ｆ１、ｆ２、ｆ３の技術的意義について説明する。なお、図４Ａ〜図４Ｃに示す五角形リンク機構の形状、すなわち各関節部の関節角はいずれも同じ状態のものであり、すなわちこれらの図においてはエンドエフェクタ３については、同じ位置、同じ姿勢とされる。
【００２８】
図４Ａは、算出式ｆ１を導出するための図である。図４Ａに示される五角形リンク機構では、関節部Ｐ２とＰ５、Ｐ２とＰ４とをそれぞれ仮想辺ｘ１、ｘ２で結ぶことで、五角
形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ２の箇所において、３つの仮想三角形の頂角α（∠Ｐ１Ｐ２Ｐ５）、β（∠Ｐ５Ｐ２Ｐ４）、γ（∠Ｐ４Ｐ２Ｐ３）が集まることで、関節部２の関節角θ２が形成されることに着目する。そして、各仮想三角形の構造的条件を利用すると、言い換えると各仮想三角形での余弦定理を利用すると、これら頂角α、β、γが以下のように表される。
【数１】
【数２】
【００２９】
ここで、上述したように関節角θ２は、頂角α、β、γの和で表されることより、算出式ｆ１を以下のように定義する。
【数３】
【００３０】
この算出式ｆ１は、そこに含まれる三つの関節角θ１、θ２、θ３について、対応するセンサによる検出値が、実際の関節角の値と同値である場合には、「関節角θ２は、頂角α、β、γの和である。」という拘束条件に矛盾は生じないためその算出値はゼロとなる。一方で、上記三つの関節角の何れかに誤差を含む異常値があれば上記拘束条件に矛盾が生じるため算出式ｆ１の算出値はゼロとならない。これらの算出式ｆ１の特性により、該算出式ｆ１の出力がゼロか否かによって、当該算出式に含まれている関節角の誤差の有無を判定することができる。これを踏まえると、算出式ｆ１は、当該上記誤差検知のためのパラメータを導き出すための式となり得る。言い換えると、算出式ｆ１は、五角形リンク機構において現実に存在する構造的な拘束条件を踏まえて、センサによる検出値がその拘束条件と矛盾する結果を導き出すか否かの結果に基づいて、関節角の検出値に誤差が含まれているか否かに関するパラメータを算出するものである。
【００３１】
次に、図４Ｂに示される五角形リンク機構では、関節部Ｐ３とＰ５、Ｐ２とＰ５とをそれぞれ仮想辺ｘ３、ｘ１で結ぶことで、五角形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ２の箇所において、２つの仮想三角形の頂角∠Ｐ１Ｐ２Ｐ５、∠Ｐ３Ｐ２Ｐ５が集まることで、関節部Ｐ２の関節角θ２が形成されることに着目し、算出式ｆ１の場合と同様の技術的思想にしたがって、図４Ｂに示す五角形リンク機構の構造的条件に基づいた算出式ｆ２を以下のように導出する。
【数４】
【００３２】
最後に、図４Ｃに示される五角形リンク機構では、関節部Ｐ３とＰ５、Ｐ３とＰ１とをそれぞれ仮想辺ｘ３、ｘ４で結ぶことで、五角形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ３の箇所において、３つの仮想三角形の頂角∠Ｐ２Ｐ３Ｐ１、∠Ｐ１Ｐ３Ｐ５、∠Ｐ５Ｐ３Ｐ４が集まることで、関節部Ｐ３の関節角θ３が形成されることに着目し、算出式ｆ３の場合と同様の技術的思想にしたがって、図４Ｃに示す五角形リンク機構の構造的条件に基づいた算出式ｆ３を以下のように導出する。
【数５】
【００３３】
ここで、上記算出式ｆ１、ｆ２、ｆ３は、以下に示す条件を満たしている。
（条件ａ）３個の算出式ｆ１、ｆ２、ｆ３に含まれる関節角の数は３個ずつであって（すなわち、五角形リンク機構での関節数５から２を差し引いた数の関節角によって各算出が形成される）、算出式間同士に含まれる関節角の組合せは完全一致しないこと。
この結果、各算出式から算出された算出値の組合せに基づいて、何れの関節角を検出するセンサの検出値が、実際の関節角の値とずれているか否かを判定することが可能となる。
【００３４】
そこで、この算出式による算出値の組合せで形成されるパラメータに基づくセンサの誤差判定は、図２に示す誤差判定部２３によって行われる。この誤差判定部２３による判定について、図５に基づいて説明する。図５は、各算出式の算出値の組合せである誤差パラメータと、誤差判定とを関連づけたテーブルである。したがって、誤差判定部２３は、図５に示すテーブルにしたがって関節角の検出誤差の有無を判定する。
（１）算出式ｆ１、ｆ２、ｆ３の全てがゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の中に誤差は無いことを意味し、また算出式ｆ２よりθ１、θ２、θ４の中に誤差は無いことを意味し、また算出式ｆ３よりθ２、θ３、θ４の中に誤差は無いことを意味する。そのため、誤差判定部２３は、全てのセンサは正常に駆動していると判定する。
（２）算出式ｆ１、ｆ２が非ゼロで、算出式ｆ３がゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の中には誤差は無いことを意味する。そのため、誤差判定部２３は、センサＳ１において誤差が生じていることを検知する。
（３）算出式ｆ１、ｆ２、ｆ３の全てが非ゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに誤差があることを意味する。そのため、誤差判定部２３は、センサＳ２において誤差が生じていることを検知する。
（４）算出式ｆ１、ｆ３が非ゼロで、算出式ｆ２がゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の中には誤差がないことを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに
誤差があることを意味する。そのため、誤差判定部２３は、センサＳ３において誤差が生じていることを検知する。
（５）算出式ｆ１がゼロで、算出式ｆ２、ｆ３が非ゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の中には誤差が無いことを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに誤差があることを意味する。そのため、誤差判定部２３は、センサＳ４において誤差が生じていることを検知する。
【００３５】
このように誤差判定部は、誤差パラメータ取得部２２が上記算出式ｆ１〜ｆ３にしたがって、取得したパラメータ（すなわち、各算出式の算出値がゼロか非ゼロかの組合せ）に基づいて、センサＳ１〜Ｓ４の何れのセンサが誤差を有している状態にあるかを的確に検知することができる。本実施例では、パラレルロボット１の駆動に必要な駆動されるべき関節部の数よりも多いセンサを設けているが、全ての関節部にセンサを取り付けなくても、すなわち関節部の数より一つ少ないセンサで、各センサの誤差判定を的確に行うことができる。したがって、パラレルロボット１において、冗長に設けられるセンサの数がいたずらに多くなることを抑制することにもなる。
【００３６】
ここで、誤差判定部２３によっていずれかのセンサに誤差が生じていることが検知された場合、パラレルロボット１の運転上の安全を優先して、ロボット制御部３０に対してパラレルロボット１の駆動を停止するように指示を出してもよい。
【００３７】
一方で、パラレルロボット１においては上述の通り、冗長なセンサが２台設けられているため、一台のセンサが誤差が生じていることを理由に、そこからの検出値の利用は控えるべきであっても、他のセンサからの検出値をもって誤差が生じてしまったセンサの代替を果たすことができる。仮に、関節部Ｐ１のセンサＳ１において誤差が生じ、センサＳ１の使用を禁止しようとするとき、下記の式にしたがって、代替的な関節角θ１を算出し、パラレルロボット１の制御を継続することが可能である。
【数６】
【００３８】
このように誤差が生じてしまったセンサが対応していた関節角の修正を行い、代替的な関節角を設定し、それをロボット制御部３０に渡してパラレルロボットの制御を継続させる機能は、図２に示す関節角修正部２４によって発揮される。なお、この関節角修正部２４によって代替的な関節角がロボット制御部３０に渡されているとき、センサ誤差検知部２０によるセンサ誤差の検知を的確に行えない状態になっている。そのため、そのような場合は、適切なタイミングで誤差が生じたセンサを交換、修理等するのが好ましい。
【００３９】
また、上記実施例では、関節部Ｐ１〜Ｐ４の４箇所にセンサを取り付け、関節部Ｐ５にはセンサを取り付けなかったが、センサを取り付けない関節部をＰ５以外の関節部としても、上記と同様にパラレルロボット１の制御には支障はなく、また取り付けた四台のセンサにおける誤差発生を的確に検知することは可能である。なお、パラレルロボット１の的確な制御のためには、モータＭ１、Ｍ２で直接駆動される関節部Ｐ１、Ｐ２にはセンサを取り付けるのが好ましい。
【００４０】
さらに、上記実施例では、パラレルロボット１が有する多角形リンク機構が五角形リンク機構の場合を例示したが、それ以外の多角形リンク機構においても、上述した技術的思想に基づいてセンサにおける誤差発生の検知を的確に行うことができる。例えば、図６に
示すような四角形リンク機構においても、そこに設置される三台のセンサにおける誤差発生の検知が可能である。なお、図６に記載の参照番号は、図３、図４Ａ等における記載に準ずる。この場合、センサＳ２、Ｓ３が冗長的なセンサとなり、この四角形リンク機構の構造的条件にしたがって、上記のようなパラメータ算出式を二つ設定することで、センサ誤差検知部２０による誤差検知が可能となる。
【００４１】
また、図示した四角形リンク機構、五角形リンク機構に限らず、それ以上の多角形リンク機構にも本発明の技術的思想は適用可能である。
【図面の簡単な説明】
【００４２】
【図１】パラレル型リンクロボットの概略構成を示す図である。
【図２】図１に示すパラレル型リンクロボットの制御装置の機能を、イメージ化して示したブロック図である。
【図３】図１に示すパラレル型リンクロボットの構成をモデル化して示した図である。
【図４Ａ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第一の図である。
【図４Ｂ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第二の図である。
【図４Ｃ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第三の図である。
【図５】図１に示すパラレル型リンクロボットにおけるセンサの誤差発生を判定するためのパラメータと、判定内容とを関連付けたテーブルである。
【図６】本発明に係る誤差検知装置が適用可能な別の形態のパラレル型リンクロボットの構造モデル図である。
【符号の説明】
【００４３】
１・・・・パラレル型リンクロボット
２・・・・取付テーブル
３・・・・エンドエフェクタ
１０・・・・制御装置
Ｌ１・・・・ベースリンク部材
Ｌ２、Ｌ３、Ｌ４、Ｌ５・・・・リンク部材
Ｐ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５・・・・関節部
Ｓ１、Ｓ２、Ｓ３、Ｓ４・・・・センサ
【技術分野】
【０００１】
本発明は、４本以上のリンク部材と対応する関節部とで構成された閉ループのリンク機構を有するパラレル型リンクロボットに関する。
【背景技術】
【０００２】
一般に、複数のリンク部材とそれらを可動的につなぐ関節部からなるロボットマニピュレータの構造は、シリアル型とパラレル型の２つに大別することができる。シリアル型はベースからエンドエフェクタまで一自由度の関節部が直列に繋がり、各関節部がアクチュエータにより駆動される。このようにシリアル型のマニピュレータは、構造は簡単であるが、アクチュエータなどの質量が各リンク部材に加算され各関節部が曲げモーメントを受けるため、マニピュレータ全体の剛性も低下し、精度が低く、可搬重量が小さいなどの問題がある。
【０００３】
一方、パラレル型はそれを構成するリンク部材が閉ループを成すため、マニピュレータ全体の剛性も高くでき、その結果、アクチュエータ誤差が小さく、高精度の位置決めが可能となる。基本的に、エンドエフェクタに対して複数のリンク部材が支持する形となるため、各関節部にかかる分担過重もシリアル型に比べて小さくなり、以てリンク部材の軽量化もはかることができる。このように、パラレル型のロボットは多くの利点を有することから、広く普及しつつあり、また国内外の多くの研究機関において精力的に研究がなされている（例えば、特許文献１、２、非特許文献１を参照。）。
【特許文献１】特開２００２−２９５６２３号公報
【特許文献２】特開２００３−９４２６６号公報
【非特許文献１】舟橋 宏明，“ロボット機構としてのパラレルメカニズム”日本ロボット学会誌， Vol.10，No.6，pp699-704，1992
【発明の開示】
【発明が解決しようとする課題】
【０００４】
このようにパラレル型のリンクロボット（以下、単に「パラレルロボット」ともいう。）では、一般に、それを構成するリンク部材が閉ループを構成する。話を簡潔にするために、パラレルロボットで構成される当該閉ループが多角形構造を有するものとする。例えば、五本のリンク部材と五個の関節部とで構成されるパラレルロボットでは、五角形の閉ループが構成されることになる。このような閉ループの多角形構造を有するパラレルロボットでは、アクチュエータによって稼動される必要最少の関節部の数は、関節部の総数をｎ個（ｎは４以上の整数）とすると、ｎ−３個となる。したがって、エンドエフェクタの位置制御などのパラレルロボットの制御を行う場合、アクチュエータによって駆動される関節部の関節角を、最低限検出し、それを当該制御にフィードバック等すればよい。
【０００５】
しかし、上記ロボットの制御をより正確に行うため、例えば、駆動軸のアクチュエータの回転制御だけでなく、最終的に位置決めされるべきエンドエフェクタの位置の精度を高めるために、アクチュエータが設置されていない関節部の関節角を検出するための検出部も設け、それをロボットの制御に利用する場合がある。また、このようなパラレルロボットにおいては、各関節部の関節角を検出するためにロータリエンコーダ等の検出装置が検出部として利用される。
【０００６】
しかし、当然ながら、これらの関節部の関節角の検出部が故障、もしくは故障までに至らなくともその検出誤差が大きくなりすぎると、ロボット制御への影響が無視できなくなり、場合によってロボットの駆動が困難となる場合がある。したがって、パラレルロボッ
トにおいて、その関節角検出部の検出誤差が適正なものか否か的確に判断する必要があるが、それについて十分な考察が為されていないのが現状である。
【０００７】
本発明では、上記した問題に鑑み、パラレルロボットにおいて、その関節角を検出する検出部の検出誤差を的確に検知する誤差検知装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
そこで、上記の課題を解決するために、本発明では、パラレルロボットの構造的条件、すなわち幾何学的条件を利用することで、関節角の検出部の誤差検知を的確に行うことを可能とした。より詳細には、本発明に係るパラレル型リンクロボット（パラレルロボット）における関節角の誤差検知装置（以下、単に「誤差検知装置」ともいう。）は、ｎ個以上（ｎは４以上の整数）のリンク部材と該リンク部材の各々を連結する関節部とで形成されるｎ角形リンク機構を有するパラレル型リンクロボット（パラレルロボット）において、該リンク機構の関節部の角度を検出する検出部における検出誤差を検知する誤差検知装置であって、前記ｎ角形リンク機構を構成する関節部のうち、一つの関節部を除くｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、前記ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、前記関節角検出部によって検出された前記ｎ−１個の関節部の関節角とに基づき、前記ｎ−１個の関節部の前記関節角検出部によって検出された関節角と、該ｎ−１個の関節部の実際の関節角との間に生じる角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、前記角度誤差パラメータ取得部によって取得されたパラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、を備える。
【０００９】
上記誤差検知装置では、角度誤差パラメータ取得部が、関節部について、関節角検出部によって検出された関節角（検出値）と、実際の関節角との差に相当する角度誤差に関連するパラメータを、パラレルロボットのｎ角形リンク機構の構造的条件の下、すなわち各リンク部材と各関節部とで決定されるｎ角形リンク機構の幾何学的要素を考慮した上で、各リンク部材の長さと関節角の検出値とに基づいて取得する。言い換えるならば、角度誤差パラメータ部は、パラレルロボットの構造的な拘束条件であるリンク部材と関節部に対して、必ずしもこの拘束条件に縛られるとは限らない関節角の検出値（すなわち検出部による検出誤差が大きくなると、本来的には当該拘束条件に縛られるべき関節角の値が、その拘束条件から外れた値へと移っていくことを意味する。）を重ね合わせることで、ｎ角形リンク機構の構造上、矛盾が生じる状態となる。その矛盾を上記角度誤差に関するパラメータとして、角度誤差パラメータ取得部が取得する。
【００１０】
そして、誤差判定部が、角度誤差パラメータ取得部が取得したパラメータに基づいて、パラメータを算出するために用いられた関節部の関節角の検出値のうち、何れの検出値において誤差が含まれているかを判定する。なお、角度誤差パラメータ部によるパラメータ取得には、ｎ角形リンク機構における全ての関節部の関節角の検出値が必要ではなく、ｎ−１個の関節角の検出値で必要十分である。これは、ｎ角形リンク機構の構造的条件を利用することで、上記パラメータを的確に算出できるからである。その結果、関節角検出部が検出すべき関節部の数が少なくでき、以てパラレルロボットの構成を簡潔なものとしながらも、各関節角の検出、および関節角検出における誤差検知を的確に行うことができるのである。
【００１１】
なお、上記の構成は、パラレルロボットのｎ角形リンク機構において、全ての関節部の関節角を検出部によって検出し、該パラレルロボットの制御等に利用することを妨げるものではない。あくまでも、本発明にかかる誤差検知装置では、ｎ角形リンク機構を構成するｎ個の関節部のうちｎ−１個の関節部の検出値を利用することで、当該ｎ−１個の関節
部における関節角の検出誤差を的確に検知できる点が重要である。
【００１２】
ここで、上記誤差検知装置において、前記角度誤差パラメータ取得部は、前記角度誤差に関するパラメータを算出するための、ｎ−２個の所定の算出式に従い、該パラメータを取得するように構成されてもよい。そして、その場合、前記ｎ−２個の所定の算出式は、前記ｎ角形リンク機構の構造的条件を前提として、前記ｎ角形リンク機構の各リンク部材の長さと、前記検出されたｎ−１個の関節部の関節角とから設定され、更に、以下の(a)
の条件を満たす。
(a)各所定の算出式に含まれる関節角の数はｎ−２個であって、所定の算出式間同士に
含まれる関節角の組合せは完全一致しないこと。
更に、前記誤差判定部については、前記ｎ−２個の所定の算出式から算出された各算出値の組合せを前記パラメータとして、該パラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断が行われる。
【００１３】
上記ｎ−２個の所定の算出式が、上記の条件(a)を満たしながら、多角形リンク機構の
構造的条件に対して、関節角検出部によって検出されたｎ−１個の関節角を重ね合わせることで、各算出式の算出値の組合せを誤差判定のためのパラメータとすることができる。すなわち、各算出式の算出値が、特定のｎ−２個の関節部の関節角とｎ角形リンク機構の構造的条件から導かれる拘束条件を満たしているか否かについて反映する値となるべく、上記条件(a)が設定される。
【００１４】
また、上記誤差検知装置において、前記角度誤差パラメータ取得部は、それぞれが前記ｎ角形リンク機構における構造的条件の下、前記リンク部材と二つの関節を結んで形成される仮想辺とで区分される仮想三角形での、該リンク部材の長さおよび該仮想辺の長さと前記検出された関節角との相関関係に基づいた前記ｎ−２個の所定の算出式にしたがって、前記パラメータを取得するようにしてもよい。すなわち、ｎ角形リンク機構の構造的条件として、該ｎ角形リンク機構内に形成される仮想三角形成立のための条件を利用するものである。
【００１５】
そして、リンク部材の長さおよび仮想辺の長さと検出関節角との相関関係において、検出関節角が実際の関節角と誤差がない場合には、ｎ角形リンク機構の構造的条件に対して矛盾するパラメータが、算出式から導かれない。しかし、検出関節角が実際の関節角に対して誤差が生じている場合には、上記仮想三角形の成立条件に矛盾が生じ、以てｎ角形リンク機構の構造的条件に対して矛盾するパラメータが、算出式から導かれることになる。それにより、誤差判定部が、ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行うことが可能となる。
【００１６】
ここで、上述までの誤差検知装置において、前記誤差判定部により、前記ｎ−１個の関節部のうち何れかの関節部に角度誤差が生じていると判定されたとき、該角度誤差が生じたとされた関節部の前記関節角度検出部によって検出された関節角を、該角度誤差が生じたとされた関節部を除く一部又は全部の関節部の該関節角度検出部によって検出された関節角と前記ｎ角形リンク機構における構造的条件とに従って修正する関節角修正部を、更に備えるようにしてもよい。このようなｎ角形リンク機構の構造的条件に基づく関節角修正部を有することにより、角度誤差が生じている関節部の関節角を正確に補完することができ、例えば発生した角度誤差がパラレルロボットの運転制御に与える影響を可及的に抑制することができる。
【発明の効果】
【００１７】
パラレルロボットにおいて、誤差検知装置により、その関節角を検出する検出部の検出誤差を的確に検知することが可能となる。
【発明を実施するための最良の形態】
【００１８】
以下に、本発明に係る誤差検知装置を備えるパラレルロボット１について図面に基づいて説明する。パラレルロボット１は、コの字形状をしたベースリンク部材Ｌ１が最下部に位置するリンク部材として設けられ、このベースリンク部材Ｌ１は、図示されてはいないパラレルロボット１の台座に対して、回転軸Ｚを介して取り付けられている。したがって、パラレルロボット１は、回転軸Ｚを介して台座に対してその全体の向きを旋回させることが可能となる。
【００１９】
ここで、ベースリンク部材Ｌ１の右側端部には、それに対して回動可能となるように関節部Ｐ１を介して、リンク部材Ｌ５が取り付けられ、一方、ベースリンク部材Ｌ１の左側端部には、それに対して回動可能となるように関節部Ｐ２を介して、リンク部材Ｌ２が取り付けられている。更に、リンク部材Ｌ５の、ベースリンク部材Ｌ１と接続されていない側の端部には、それに対して可動可能となるように関節部Ｐ５を介して、リンク部材Ｌ４が取り付けられており、また、リンク部材Ｌ２の、ベースリンク部材Ｌ１と接続されていない側の端部には、それに対して可動可能となるように関節部Ｐ３を介して、リンク部材Ｌ３が取り付けられている。更に、リンク部材Ｌ３、Ｌ４の、それぞれ関節部Ｐ３、Ｐ５に接続されていない端部同士は、関節部Ｐ４を介して接続され、両リンク部材が回動可能状態となっている。また、この関節部Ｐ４に支持されるように取付テーブル２が設けられ、該取付テーブル２の上方には、パラレルロボット１のエンドエフェクタ３が設置される。
【００２０】
このように構成されるパラレルロボット１は、ベースリンク部材Ｌ１と４本のリンク部材Ｌ２〜Ｌ５の合計五本のリンク部材によって形成される五角形のリンク機構を有することになる。そして、各リンク部材の位置や姿勢が制御されて、この五角形リンク機構が変形し、関節部Ｐ４に設けられた取付テーブル２の位置と姿勢が制御され、以てエンドエフェクタ３の動作領域が制御されることになる。ここで、五角形リンク機構を有するパラレルロボット１では、そのリンク機構の構造的条件により、五つの関節部Ｐ１〜Ｐ５の全てをアクチュエータによって駆動しなくても、エンドエフェクタ３の位置や姿勢を制御できる。具体的には、一般にｎ角形（ｎは４以上の整数）のリンク機構を有するパラレルロボットでは、最小でもｎ−３個の関節部をアクチュエータで駆動しさえすれば、各関節部の位置を制御することが可能である。したがって、本実施例では、図１に示すリンク機構は５角形リンク機構であるから、エンドエフェクタ３の位置・姿勢制御用として、関節部Ｐ１、Ｐ２の２箇所にアクチュエータである回転モータを設置する。図１においては、このモータをそれぞれＭ１、Ｍ２で表している。
【００２１】
更に、パラレルロボット１においては、各関節部Ｐ１〜Ｐ５のうち、関節部Ｐ５を除く４箇所の関節部に、その関節部の関節角（該関節部に接続される二本のリンク部材によって形成される角度）を検出するための角度センサ（具体的には、ロータリエンコーダ等が使用可能であり、以下、単に「センサ」という。）が設けられている。具体的には、関節部Ｐ１、Ｐ２、Ｐ３、Ｐ４には、それぞれセンサＳ１、Ｓ２、Ｓ３、Ｓ４がそれぞれ対応するように設けられ、各関節角をθ１、θ２、θ３、θ４として検出される。
【００２２】
上記の通り、五角形リンク機構においては、五つの関節角のうち二つが決定されると、残りの三つの関節角も決定される。しかし、上記のようにモータＭ１、Ｍ２に対応する関節部Ｐ１、Ｐ２以外の関節部Ｐ３、Ｐ４にも関節角検出のためのセンサを冗長的に設けているのは、これらの関節角を検出しモータの出力にフィードバックすることで、エンドエフェクタ３の位置制御等をより正確に行うためである。なお、このような冗長的な検出部を利用したロボット制御は、従来から行われているものであるから、本明細書ではその詳細は割愛する。
【００２３】
このように構成されるパラレルロボット１を制御するために、制御装置１０がパラレルロボット１に対してケーブルで接続されている。制御装置１０は一般的なコンピュータによって構成され、これにより制御装置１０は、パラレルロボット１が有する各センサからの出力値を見ながら、エンドエフェクタ３が所望の位置や姿勢に至るように、モータＭ１、Ｍ２に対して駆動指令を出力する。ここで、図２に、この制御装置１０の機能をイメージ化して機能ブロックとして示す。以下に、図２に基づいて制御装置１０の詳細について説明する。
【００２４】
制御装置１０では、パラレルロボット１のセンサＳ１〜Ｓ４からの検出値が、センサ誤差検知部２０とロボット制御部３０とに引き渡される。センサ誤差検知部２０は、本発明に係る誤差検知装置を形成するための主な機能部である。そして、ロボット制御部３０は、上述したような各センサからの検出値を基に、パラレルロボット１を制御するための種々の制御を行う機能部であるが、それらの技術は本発明に係る誤差検知装置と直接的な関係性は小さく、また従来からの技術においても実現されていることであるので、本明細書ではその説明は割愛する。
【００２５】
ここで、センサ誤差検知部２０は、関節角検出値取得部２１と、誤差パラメータ取得部２２と、誤差判定部２３と、関節角修正部２４を有する。以下、各機能部の説明をするが、本発明の理解を促進するために、図１に示した構造を有するパラレルロボット１をモデル化したものを図３に示す。このようにパラレルロボット１のモデル化にあたり、必要でない要素（例えば、回転軸Ｚ等）については、図３では記載を省略した。また、各リンク部材の参照番号Ｌ１〜Ｌ４は、そのリンク部材の長さを表す記号としても用いることとする。ここで、リンク部材の長さは、当該リンク部材に接続される二つの関節部同士の間の距離と定義する（後述する仮想辺についても同様）。したがって、ベースリンク部材Ｌ１のようにコ字形状を有しているものの長さは、その形状に沿った長さではなく、そこに接続された関節部Ｐ１とＰ２の二点間の距離となる。また、図３においては、センサが設置されている関節部Ｐ１〜Ｐ４を黒塗りの丸印で表し、センサが設けられていない関節部Ｐ５を白塗りの丸印で表している。
【００２６】
まず、関節角検出値取得部２１は、パラレルロボット１に設置されたセンサＳ１〜Ｓ４により、対応する各関節部の関節角を取得する。具体的には、関節角検出値取得部２１は、センサＳ１より関節角θ１を、センサＳ２より関節角θ２を、センサＳ３より関節角θ３を、センサＳ４より関節角θ４を、それぞれ取得する。
【００２７】
次に、誤差パラメータ取得部２２は、関節角検出値取得部２１によって取得された各関節角θ１〜θ４と、各リンク部材Ｌ１〜Ｌ５で構成される図３に示す五角形リンク機構の構造的条件に基づいて、センサが取り付けられた関節部における関節角の検出値と実際の関節角の値との誤差に関連するパラメータを取得する。ここで、五角形リンク機構の構造的条件とは、各リンク部材の長さと、それらをつなぐ各関節部の配置条件であり、五角形リンク機構の形状を決定するための物理的、構造的拘束条件である。また、上記パラメータは、この五角形リンク機構の構造的条件をベースとして、算出される誤差検知のためのパラメータである。以下に、図４Ａ〜図４Ｃに基づいてこのパラメータを算出するための三つの算出式ｆ１、ｆ２、ｆ３の技術的意義について説明する。なお、図４Ａ〜図４Ｃに示す五角形リンク機構の形状、すなわち各関節部の関節角はいずれも同じ状態のものであり、すなわちこれらの図においてはエンドエフェクタ３については、同じ位置、同じ姿勢とされる。
【００２８】
図４Ａは、算出式ｆ１を導出するための図である。図４Ａに示される五角形リンク機構では、関節部Ｐ２とＰ５、Ｐ２とＰ４とをそれぞれ仮想辺ｘ１、ｘ２で結ぶことで、五角
形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ２の箇所において、３つの仮想三角形の頂角α（∠Ｐ１Ｐ２Ｐ５）、β（∠Ｐ５Ｐ２Ｐ４）、γ（∠Ｐ４Ｐ２Ｐ３）が集まることで、関節部２の関節角θ２が形成されることに着目する。そして、各仮想三角形の構造的条件を利用すると、言い換えると各仮想三角形での余弦定理を利用すると、これら頂角α、β、γが以下のように表される。
【数１】
【数２】
【００２９】
ここで、上述したように関節角θ２は、頂角α、β、γの和で表されることより、算出式ｆ１を以下のように定義する。
【数３】
【００３０】
この算出式ｆ１は、そこに含まれる三つの関節角θ１、θ２、θ３について、対応するセンサによる検出値が、実際の関節角の値と同値である場合には、「関節角θ２は、頂角α、β、γの和である。」という拘束条件に矛盾は生じないためその算出値はゼロとなる。一方で、上記三つの関節角の何れかに誤差を含む異常値があれば上記拘束条件に矛盾が生じるため算出式ｆ１の算出値はゼロとならない。これらの算出式ｆ１の特性により、該算出式ｆ１の出力がゼロか否かによって、当該算出式に含まれている関節角の誤差の有無を判定することができる。これを踏まえると、算出式ｆ１は、当該上記誤差検知のためのパラメータを導き出すための式となり得る。言い換えると、算出式ｆ１は、五角形リンク機構において現実に存在する構造的な拘束条件を踏まえて、センサによる検出値がその拘束条件と矛盾する結果を導き出すか否かの結果に基づいて、関節角の検出値に誤差が含まれているか否かに関するパラメータを算出するものである。
【００３１】
次に、図４Ｂに示される五角形リンク機構では、関節部Ｐ３とＰ５、Ｐ２とＰ５とをそれぞれ仮想辺ｘ３、ｘ１で結ぶことで、五角形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ２の箇所において、２つの仮想三角形の頂角∠Ｐ１Ｐ２Ｐ５、∠Ｐ３Ｐ２Ｐ５が集まることで、関節部Ｐ２の関節角θ２が形成されることに着目し、算出式ｆ１の場合と同様の技術的思想にしたがって、図４Ｂに示す五角形リンク機構の構造的条件に基づいた算出式ｆ２を以下のように導出する。
【数４】
【００３２】
最後に、図４Ｃに示される五角形リンク機構では、関節部Ｐ３とＰ５、Ｐ３とＰ１とをそれぞれ仮想辺ｘ３、ｘ４で結ぶことで、五角形リンク機構の内部に３つの仮想三角形を区分する。ここで、関節部Ｐ３の箇所において、３つの仮想三角形の頂角∠Ｐ２Ｐ３Ｐ１、∠Ｐ１Ｐ３Ｐ５、∠Ｐ５Ｐ３Ｐ４が集まることで、関節部Ｐ３の関節角θ３が形成されることに着目し、算出式ｆ３の場合と同様の技術的思想にしたがって、図４Ｃに示す五角形リンク機構の構造的条件に基づいた算出式ｆ３を以下のように導出する。
【数５】
【００３３】
ここで、上記算出式ｆ１、ｆ２、ｆ３は、以下に示す条件を満たしている。
（条件ａ）３個の算出式ｆ１、ｆ２、ｆ３に含まれる関節角の数は３個ずつであって（すなわち、五角形リンク機構での関節数５から２を差し引いた数の関節角によって各算出が形成される）、算出式間同士に含まれる関節角の組合せは完全一致しないこと。
この結果、各算出式から算出された算出値の組合せに基づいて、何れの関節角を検出するセンサの検出値が、実際の関節角の値とずれているか否かを判定することが可能となる。
【００３４】
そこで、この算出式による算出値の組合せで形成されるパラメータに基づくセンサの誤差判定は、図２に示す誤差判定部２３によって行われる。この誤差判定部２３による判定について、図５に基づいて説明する。図５は、各算出式の算出値の組合せである誤差パラメータと、誤差判定とを関連づけたテーブルである。したがって、誤差判定部２３は、図５に示すテーブルにしたがって関節角の検出誤差の有無を判定する。
（１）算出式ｆ１、ｆ２、ｆ３の全てがゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の中に誤差は無いことを意味し、また算出式ｆ２よりθ１、θ２、θ４の中に誤差は無いことを意味し、また算出式ｆ３よりθ２、θ３、θ４の中に誤差は無いことを意味する。そのため、誤差判定部２３は、全てのセンサは正常に駆動していると判定する。
（２）算出式ｆ１、ｆ２が非ゼロで、算出式ｆ３がゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の中には誤差は無いことを意味する。そのため、誤差判定部２３は、センサＳ１において誤差が生じていることを検知する。
（３）算出式ｆ１、ｆ２、ｆ３の全てが非ゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに誤差があることを意味する。そのため、誤差判定部２３は、センサＳ２において誤差が生じていることを検知する。
（４）算出式ｆ１、ｆ３が非ゼロで、算出式ｆ２がゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の何れかに誤差があることを意味し、また算出式ｆ２よりθ１、θ２、θ４の中には誤差がないことを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに
誤差があることを意味する。そのため、誤差判定部２３は、センサＳ３において誤差が生じていることを検知する。
（５）算出式ｆ１がゼロで、算出式ｆ２、ｆ３が非ゼロになった場合、算出式ｆ１よりθ１、θ２、θ３の中には誤差が無いことを意味し、また算出式ｆ２よりθ１、θ２、θ４の何れかに誤差があることを意味し、また算出式ｆ３よりθ２、θ３、θ４の何れかに誤差があることを意味する。そのため、誤差判定部２３は、センサＳ４において誤差が生じていることを検知する。
【００３５】
このように誤差判定部は、誤差パラメータ取得部２２が上記算出式ｆ１〜ｆ３にしたがって、取得したパラメータ（すなわち、各算出式の算出値がゼロか非ゼロかの組合せ）に基づいて、センサＳ１〜Ｓ４の何れのセンサが誤差を有している状態にあるかを的確に検知することができる。本実施例では、パラレルロボット１の駆動に必要な駆動されるべき関節部の数よりも多いセンサを設けているが、全ての関節部にセンサを取り付けなくても、すなわち関節部の数より一つ少ないセンサで、各センサの誤差判定を的確に行うことができる。したがって、パラレルロボット１において、冗長に設けられるセンサの数がいたずらに多くなることを抑制することにもなる。
【００３６】
ここで、誤差判定部２３によっていずれかのセンサに誤差が生じていることが検知された場合、パラレルロボット１の運転上の安全を優先して、ロボット制御部３０に対してパラレルロボット１の駆動を停止するように指示を出してもよい。
【００３７】
一方で、パラレルロボット１においては上述の通り、冗長なセンサが２台設けられているため、一台のセンサが誤差が生じていることを理由に、そこからの検出値の利用は控えるべきであっても、他のセンサからの検出値をもって誤差が生じてしまったセンサの代替を果たすことができる。仮に、関節部Ｐ１のセンサＳ１において誤差が生じ、センサＳ１の使用を禁止しようとするとき、下記の式にしたがって、代替的な関節角θ１を算出し、パラレルロボット１の制御を継続することが可能である。
【数６】
【００３８】
このように誤差が生じてしまったセンサが対応していた関節角の修正を行い、代替的な関節角を設定し、それをロボット制御部３０に渡してパラレルロボットの制御を継続させる機能は、図２に示す関節角修正部２４によって発揮される。なお、この関節角修正部２４によって代替的な関節角がロボット制御部３０に渡されているとき、センサ誤差検知部２０によるセンサ誤差の検知を的確に行えない状態になっている。そのため、そのような場合は、適切なタイミングで誤差が生じたセンサを交換、修理等するのが好ましい。
【００３９】
また、上記実施例では、関節部Ｐ１〜Ｐ４の４箇所にセンサを取り付け、関節部Ｐ５にはセンサを取り付けなかったが、センサを取り付けない関節部をＰ５以外の関節部としても、上記と同様にパラレルロボット１の制御には支障はなく、また取り付けた四台のセンサにおける誤差発生を的確に検知することは可能である。なお、パラレルロボット１の的確な制御のためには、モータＭ１、Ｍ２で直接駆動される関節部Ｐ１、Ｐ２にはセンサを取り付けるのが好ましい。
【００４０】
さらに、上記実施例では、パラレルロボット１が有する多角形リンク機構が五角形リンク機構の場合を例示したが、それ以外の多角形リンク機構においても、上述した技術的思想に基づいてセンサにおける誤差発生の検知を的確に行うことができる。例えば、図６に
示すような四角形リンク機構においても、そこに設置される三台のセンサにおける誤差発生の検知が可能である。なお、図６に記載の参照番号は、図３、図４Ａ等における記載に準ずる。この場合、センサＳ２、Ｓ３が冗長的なセンサとなり、この四角形リンク機構の構造的条件にしたがって、上記のようなパラメータ算出式を二つ設定することで、センサ誤差検知部２０による誤差検知が可能となる。
【００４１】
また、図示した四角形リンク機構、五角形リンク機構に限らず、それ以上の多角形リンク機構にも本発明の技術的思想は適用可能である。
【図面の簡単な説明】
【００４２】
【図１】パラレル型リンクロボットの概略構成を示す図である。
【図２】図１に示すパラレル型リンクロボットの制御装置の機能を、イメージ化して示したブロック図である。
【図３】図１に示すパラレル型リンクロボットの構成をモデル化して示した図である。
【図４Ａ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第一の図である。
【図４Ｂ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第二の図である。
【図４Ｃ】図１に示すパラレル型リンクロボットにおいて、センサの誤差発生を検知するためのパラメータ算出式を導出するための、該リンクロボットの構造的条件を示す第三の図である。
【図５】図１に示すパラレル型リンクロボットにおけるセンサの誤差発生を判定するためのパラメータと、判定内容とを関連付けたテーブルである。
【図６】本発明に係る誤差検知装置が適用可能な別の形態のパラレル型リンクロボットの構造モデル図である。
【符号の説明】
【００４３】
１・・・・パラレル型リンクロボット
２・・・・取付テーブル
３・・・・エンドエフェクタ
１０・・・・制御装置
Ｌ１・・・・ベースリンク部材
Ｌ２、Ｌ３、Ｌ４、Ｌ５・・・・リンク部材
Ｐ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５・・・・関節部
Ｓ１、Ｓ２、Ｓ３、Ｓ４・・・・センサ
【特許請求の範囲】
【請求項１】
ｎ個以上（ｎは４以上の整数）のリンク部材と該リンク部材の各々を連結する関節部とで形成されるｎ角形リンク機構を有するパラレル型リンクロボットにおいて、該リンク機構の関節部の角度を検出する検出部における検出誤差を検知する誤差検知装置であって、
前記ｎ角形リンク機構を構成する関節部のうち、一つの関節部を除くｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、
前記ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、前記関節角検出部によって検出された前記ｎ−１個の関節部の関節角とに基づき、前記ｎ−１個の関節部の前記関節角検出部によって検出された関節角と、該ｎ−１個の関節部の実際の関節角との間に生じる角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、
前記角度誤差パラメータ取得部によって取得されたパラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、
を備える、パラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項２】
前記角度誤差パラメータ取得部は、前記角度誤差に関するパラメータを算出するための、ｎ−２個の所定の算出式に従い、該パラメータを取得し、
前記ｎ−２個の所定の算出式は、前記ｎ角形リンク機構の構造的条件を前提として、前記ｎ角形リンク機構の各リンク部材の長さと、前記検出されたｎ−１個の関節部の関節角とから設定され、更に、以下の条件(a)を満たし、
(a)各所定の算出式に含まれる関節角の数はｎ−２個であって、所定の算出式間同士に
含まれる関節角の組合せは完全一致しないこと、
前記誤差判定部は、前記ｎ−２個の所定の算出式から算出された各算出値の組合せを前記パラメータとして、該パラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う、
請求項１に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項３】
前記角度誤差パラメータ取得部は、それぞれが前記ｎ角形リンク機構における構造的条件の下、前記リンク部材と二つの関節を結んで形成される仮想辺とで区分される仮想三角形での、該リンク部材の長さおよび該仮想辺の長さと前記検出された関節角との相関関係に基づいた前記ｎ−２個の所定の算出式にしたがって、前記パラメータを取得する、
請求項２に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項４】
前記誤差判定部により、前記ｎ−１個の関節部のうち何れかの関節部に角度誤差が生じていると判定されたとき、該角度誤差が生じたとされた関節部の前記関節角度検出部によって検出された関節角を、該角度誤差が生じたとされた関節部を除く一部又は全部の関節部の該関節角度検出部によって検出された関節角と前記ｎ角形リンク機構における構造的条件とに従って修正する関節角修正部を、更に備える、請求項１から請求項３の何れか一項に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項１】
ｎ個以上（ｎは４以上の整数）のリンク部材と該リンク部材の各々を連結する関節部とで形成されるｎ角形リンク機構を有するパラレル型リンクロボットにおいて、該リンク機構の関節部の角度を検出する検出部における検出誤差を検知する誤差検知装置であって、
前記ｎ角形リンク機構を構成する関節部のうち、一つの関節部を除くｎ−１個の関節部における関節角をそれぞれ検出する関節角検出部と、
前記ｎ角形リンク機構の構造的条件の下、該ｎ角形リンク機構の各リンク部材の長さと、前記関節角検出部によって検出された前記ｎ−１個の関節部の関節角とに基づき、前記ｎ−１個の関節部の前記関節角検出部によって検出された関節角と、該ｎ−１個の関節部の実際の関節角との間に生じる角度誤差に関するパラメータを取得する角度誤差パラメータ取得部と、
前記角度誤差パラメータ取得部によって取得されたパラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う誤差判定部と、
を備える、パラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項２】
前記角度誤差パラメータ取得部は、前記角度誤差に関するパラメータを算出するための、ｎ−２個の所定の算出式に従い、該パラメータを取得し、
前記ｎ−２個の所定の算出式は、前記ｎ角形リンク機構の構造的条件を前提として、前記ｎ角形リンク機構の各リンク部材の長さと、前記検出されたｎ−１個の関節部の関節角とから設定され、更に、以下の条件(a)を満たし、
(a)各所定の算出式に含まれる関節角の数はｎ−２個であって、所定の算出式間同士に
含まれる関節角の組合せは完全一致しないこと、
前記誤差判定部は、前記ｎ−２個の所定の算出式から算出された各算出値の組合せを前記パラメータとして、該パラメータに基づいて、前記ｎ−１個の関節部のうち何れの関節部において角度誤差が生じているかについての判断を行う、
請求項１に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項３】
前記角度誤差パラメータ取得部は、それぞれが前記ｎ角形リンク機構における構造的条件の下、前記リンク部材と二つの関節を結んで形成される仮想辺とで区分される仮想三角形での、該リンク部材の長さおよび該仮想辺の長さと前記検出された関節角との相関関係に基づいた前記ｎ−２個の所定の算出式にしたがって、前記パラメータを取得する、
請求項２に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【請求項４】
前記誤差判定部により、前記ｎ−１個の関節部のうち何れかの関節部に角度誤差が生じていると判定されたとき、該角度誤差が生じたとされた関節部の前記関節角度検出部によって検出された関節角を、該角度誤差が生じたとされた関節部を除く一部又は全部の関節部の該関節角度検出部によって検出された関節角と前記ｎ角形リンク機構における構造的条件とに従って修正する関節角修正部を、更に備える、請求項１から請求項３の何れか一項に記載のパラレル型リンクロボットにおける関節角の誤差検知装置。
【図１】
【図２】
【図３】
【図４Ａ】
【図４Ｂ】
【図４Ｃ】
【図５】
【図６】
【図２】
【図３】
【図４Ａ】
【図４Ｂ】
【図４Ｃ】
【図５】
【図６】
【公開番号】特開２０１０−２３２０３（Ｐ２０１０−２３２０３Ａ）
【公開日】平成２２年２月４日（２０１０．２．４）
【国際特許分類】
【出願番号】特願２００８−１８９４８８（Ｐ２００８−１８９４８８）
【出願日】平成２０年７月２３日（２００８．７．２３）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 社団法人日本機械学会 九州学生会 第３９回 卒業研究発表講演会 「パラレルロボットの関節センサ故障検知法」 ２００８年３月１８日発表
【出願人】（０００１２５３６９）学校法人東海大学 (352)
【Ｆターム（参考）】
【公開日】平成２２年２月４日（２０１０．２．４）
【国際特許分類】
【出願日】平成２０年７月２３日（２００８．７．２３）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 社団法人日本機械学会 九州学生会 第３９回 卒業研究発表講演会 「パラレルロボットの関節センサ故障検知法」 ２００８年３月１８日発表
【出願人】（０００１２５３６９）学校法人東海大学 (352)
【Ｆターム（参考）】
[ Back to top ]