(57)【特許請求の範囲】
【請求項１】
ベース部と、
前記ベース部に設けられた複数の駆動源と、
関節部を有するリンクをそれぞれ含み、前記複数の駆動源によりそれぞれ駆動される複数のアーム部と、
前記複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、前記アーム部の関節部が外れたか否かを判断する外れ検出部とを備え、
前記外れ検出部は、所定の前記アーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における前記所定のアーム部に対応する所定の駆動源の駆動力を推定し、前記所定の駆動源の推定した駆動力と前記所定の駆動源の実駆動力との駆動力比較結果に基づいて、前記所定のアーム部の関節部が外れたか否かを判断するとともに、前記駆動力比較結果により前記所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に前記所定のアーム部の関節部が外れたと判断するように構成されている、パラレルリンクロボット。
【請求項２】
前記外れ検出部は、前記所定のアーム部の関節部の外れ状態における前記所定の駆動源の駆動力を外れ状態時駆動力として推定するとともに、前記所定の駆動源の外れ状態時駆動力と前記所定の駆動源の実駆動力との差が第１のしきい値以下の場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項１に記載のパラレルリンクロボット。
【請求項３】
前記外れ検出部は、前記所定のアーム部の正常状態における前記所定の駆動源の駆動力のうち、前記所定のアーム部以外の部分を駆動させるための駆動力成分を含む所定の駆動力を推定し、前記所定の駆動力が第２のしきい値以上の場合で、かつ、前記所定の駆動源の外れ状態時駆動力と前記所定の駆動源の実駆動力との差が前記第１のしきい値以下の場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項２に記載のパラレルリンクロボット。
【請求項４】
前記外れ検出部は、前記所定の駆動源の外れ状態時駆動力と前記所定の駆動源の実駆動力との差が前記第１のしきい値以下となる状態が所定時間継続した場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項２または３に記載のパラレルリンクロボット。
【請求項５】
前記外れ検出部は、前記所定のアーム部の正常状態における前記所定の駆動源の駆動力を正常時駆動力として推定するとともに、前記所定の駆動源の正常時駆動力と前記所定の駆動源の実駆動力との差が第３のしきい値以上の場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項１に記載のパラレルリンクロボット。
【請求項６】
前記外れ検出部は、前記所定の駆動源の正常時駆動力と前記所定の駆動源の実駆動力との差が前記第３のしきい値以上となる状態が所定時間継続した場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項５に記載のパラレルリンクロボット。
【請求項７】
前記外れ検出部は、前記所定のアーム部の関節部の外れ状態における前記所定の駆動源の駆動力を外れ状態時駆動力として推定するとともに、前記所定のアーム部の正常状態における前記所定の駆動源の駆動力を正常時駆動力として推定し、前記所定の駆動源の外れ状態時駆動力と前記所定の駆動源の実駆動力との差が第１のしきい値以下の場合、および、前記所定の駆動源の正常時駆動力と前記所定の駆動源の実駆動力との差が第３のしきい値以上の場合の少なくともいずれか一方の場合に、前記所定のアーム部の関節部が外れたと判断するように構成されている、請求項１に記載のパラレルリンクロボット。
【請求項８】
前記外れ検出部は、前記所定の駆動源の推定した駆動力と前記所定の駆動源の実駆動力とに基づいて、前記所定のアーム部の関節部が外れたと判断した場合には、前記複数の駆動源を停止させる制御を行うように構成されている、請求項１〜７のいずれか１項に記載のパラレルリンクロボット。
【請求項９】
前記駆動源は、モータであり、
前記駆動源の駆動力は、前記モータのトルクである、請求項１〜８のいずれか１項に記載のパラレルリンクロボット。
【請求項１０】
ベース部と、前記ベース部に設けられた複数の駆動源と、関節部を有するリンクをそれぞれ含み、前記複数の駆動源によりそれぞれ駆動される複数のアーム部とを含むパラレルリンクロボットと、
前記パラレルリンクロボットの動作を制御するロボットコントローラとを備え、
前記ロボットコントローラは、前記複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、前記アーム部の関節部が外れたか否かを判断するように構成されているとともに、所定の前記アーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における前記所定のアーム部に対応する所定の駆動源の駆動力を推定し、前記所定の駆動源の推定した駆動力と前記所定の駆動源の実駆動力との駆動力比較結果に基づいて、前記所定のアーム部の関節部が外れたか否かを判断するとともに、前記駆動力比較結果により前記所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に前記所定のアーム部の関節部が外れたと判断するように構成されている、パラレルリンクロボットシステム。
【請求項１１】
ベース部と、前記ベース部に設けられた複数の駆動源と、関節部を有するリンクをそれぞれ含むとともに前記複数の駆動源によりそれぞれ駆動される複数のアーム部とを備えたパラレルリンクロボットの制御方法であって、
前記複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、前記アーム部の関節部が外れたか否かを判断するステップを備え、
前記関節部が外れたか否かを判断するステップは、
所定の前記アーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における前記所定のアーム部に対応する所定の駆動源の駆動力を推定するステップと、
前記所定の駆動源の推定した駆動力と前記所定の駆動源の実駆動力との駆動力比較結果に基づいて、前記所定のアーム部の関節部が外れたか否かを判断するステップと、
前記駆動力比較結果により前記所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に前記所定のアーム部の関節部が外れたと判断するステップとを含む、パラレルリンクロボットの制御方法。
【請求項１２】
前記所定の駆動源の駆動力を推定するステップは、前記所定のアーム部の関節部の外れ状態における前記所定の駆動源の駆動力を外れ状態時駆動力として推定するステップを有し、
前記所定のアーム部の関節部が外れたか否かを判断するステップは、前記所定の駆動源の外れ状態時駆動力と前記所定の駆動源の実駆動力との差が第１のしきい値以下の場合に、前記所定のアーム部の関節部が外れたと判断するステップを有する、請求項１１に記載のパラレルリンクロボットの制御方法。
【請求項１３】
前記所定の駆動源の駆動力を推定するステップは、前記所定のアーム部の正常状態における前記所定の駆動源の駆動力を正常時駆動力として推定するステップを有し、
前記所定のアーム部の関節部が外れたか否かを判断するステップは、前記所定の駆動源の正常時駆動力と前記所定の駆動源の実駆動力との差が第３のしきい値以上の場合に、前記所定のアーム部の関節部が外れたと判断するステップを有する、請求項１１に記載のパラレルリンクロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パラレルリンクロボット、パラレルリンクロボットシステムおよびパラレルリンクロボットの制御方法に関し、特に、アーム部の関節部の外れ検出が可能なパラレルリンクロボット、パラレルリンクロボットシステムおよびパラレルリンクロボットの制御方法に関する。
【背景技術】
【０００２】
従来、並列に設けられた複数のアーム部により協同してエンドエフェクタを移動させるパラレルリンクロボットが知られている。また、アーム部の関節部の外れ検出が可能なパラレルリンクロボットも知られている（たとえば、特許文献１参照）。
【０００３】
上記特許文献１には、複数の電動モータ（駆動源）と、複数の電動モータによりそれぞれ駆動される複数のアーム部とを備えたパラレルメカニズム（パラレルリンクロボット）が開示されている。このパラレルメカニズムのアーム部は、ボールジョイント部（関節部）を介して互いに連結された第１リンクおよび第２リンクを含んでいる。また、ボールジョイント部は、球状頭部と、球状頭部を保持するソケットと、球状頭部およびソケットを互いに電気的に接続する導電部材とを有している。そして、上記特許文献１のパラレルメカニズムは、球状頭部とソケットとの間に設けられた導電部材の導通状態に基づいて、ボールジョイント部（関節部）が外れたか否かを判断するように構成されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−２４８２８９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１のパラレルメカニズム（パラレルリンクロボット）では、ボールジョイント部（関節部）の導電部材の導通状態に基づいてボールジョイント部（関節部）の外れ検出が可能である一方、ボールジョイント部の球状頭部とソケットとの間に、球状頭部およびソケットを互いに電気的に接続する導電部材を設ける必要があるので、ボールジョイント部（関節部）の構造が複雑になるという問題点がある。
【０００６】
この発明は、上記のような課題を解決するためになされたものであり、この発明の１つの目的は、アーム部の関節部の構造が複雑になるのを抑制しながら、関節部の外れを検出することが可能なパラレルリンクロボット、パラレルリンクロボットシステムおよびパラレルリンクロボットの制御方法を提供することである。
【課題を解決するための手段および発明の効果】
【０００７】
上記目的を達成するために、この発明の第１の局面におけるパラレルリンクロボットは、ベース部と、ベース部に設けられた複数の駆動源と、関節部を有するリンクをそれぞれ含み、複数の駆動源によりそれぞれ駆動される複数のアーム部と、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断する外れ検出部とを備え、外れ検出部は、所定のアーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における所定のアーム部に対応する所定の駆動源の駆動力を推定し、所定の駆動源の推定した駆動力と所定の駆動源の実駆動力との駆動力比較結果に基づいて、所定のアーム部の関節部が外れたか否かを判断するとともに、駆動力比較結果により所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に所定のアーム部の関節部が外れたと判断するように構成されている。
【０００８】
この発明の第１の局面によるパラレルリンクロボットでは、上記のように、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断するように外れ検出部を構成することによって、アーム部の関節部の外れ検出のために関節部に導電部材などの専用部材を別途設けることなく、駆動源の駆動力に基づいて関節部が外れたか否かを判断することができるので、アーム部の関節部の構造が複雑になるのを抑制しながら、関節部の外れを検出することができる。
【０００９】
上記目的を達成するために、この発明の第２の局面におけるパラレルリンクロボットシステムは、ベース部と、ベース部に設けられた複数の駆動源と、関節部を有するリンクをそれぞれ含み、複数の駆動源によりそれぞれ駆動される複数のアーム部とを含むパラレルリンクロボットと、パラレルリンクロボットの動作を制御するロボットコントローラとを備え、ロボットコントローラは、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断するように構成されているとともに、所定のアーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における所定のアーム部に対応する所定の駆動源の駆動力を推定し、所定の駆動源の推定した駆動力と所定の駆動源の実駆動力との駆動力比較結果に基づいて、所定のアーム部の関節部が外れたか否かを判断するとともに、駆動力比較結果により所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に所定のアーム部の関節部が外れたと判断するように構成されている。
【００１０】
この発明の第２の局面によるパラレルリンクロボットシステムでは、上記のように、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断するようにロボットコントローラを構成することによって、ロボットコントローラにより、外れ検出のために関節部に導電部材などの専用部材を別途設けることなく、駆動源の駆動力に基づいて関節部が外れたか否かを判断することができるので、アーム部の関節部の構造が複雑になるのを抑制しながら、関節部の外れを検出することができる。また、外れ検出のために既存のロボットコントローラを用いることによって、外れ検出のために導電部材などの専用部材を別途設ける必要がないので、部品点数の増加を抑制することができる。
【００１１】
上記目的を達成するために、この発明の第３の局面におけるパラレルリンクロボットの制御方法は、ベース部と、ベース部に設けられた複数の駆動源と、関節部を有するリンクをそれぞれ含むとともに複数の駆動源によりそれぞれ駆動される複数のアーム部とを備えたパラレルリンクロボットの制御方法であって、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断するステップを備え、関節部が外れたか否かを判断するステップは、所定のアーム部の関節部の外れ状態および正常状態の少なくとも一方の状態における所定のアーム部に対応する所定の駆動源の駆動力を推定するステップと、所定の駆動源の推定した駆動力と所定の駆動源の実駆動力との駆動力比較結果に基づいて、所定のアーム部の関節部が外れたか否かを判断するステップと、駆動力比較結果により所定のアーム部の関節部が外れたと判断可能な状態が所定時間継続した場合に所定のアーム部の関節部が外れたと判断するステップとを含む。
【００１２】
この発明の第３の局面によるパラレルリンクロボットの制御方法では、上記のように、複数の駆動源のうちの少なくとも１つの駆動源の駆動力に基づいて、アーム部の関節部が外れたか否かを判断するステップを設けることによって、外れ検出のために関節部に導電部材などの専用部材を別途設けることなく、駆動源の駆動力に基づいて関節部が外れたか否かを判断することができるので、アーム部の関節部の構造が複雑になるのを抑制しながら、関節部の外れを検出することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の一実施形態によるパラレルリンクロボットの全体構成を示した概略図である。
【図２】本発明の一実施形態によるパラレルリンクロボットの関節部の構成を説明するための概略図である。
【図３】本発明の一実施形態によるパラレルリンクロボットの外れ検出処理について説明するためのフローチャートである。
【図４】本発明の一実施形態によるパラレルリンクロボットの一方の関節部が外れた状態を示した概略図である。
【図５】本発明の一実施形態によるパラレルリンクロボットの両方の関節部が外れた状態を示した概略図である。
【図６】本発明の一実施形態によるパラレルリンクロボットの第１判定処理について説明するためのフローチャートである。
【図７】本発明の一実施形態によるパラレルリンクロボットの第２判定処理について説明するためのフローチャートである。
【図８】本発明の一実施形態によるパラレルリンクロボットの関節部の外れが発生した状態における実トルクと外れ状態時トルクとを比較した図である。
【図９】本発明の一実施形態によるパラレルリンクロボットの正常時の状態における実トルクと正常時トルクと付加的トルクとを比較した図である。
【図１０】本発明の一実施形態によるパラレルリンクロボットの関節部の外れが発生した状態における実トルクと外れ状態時トルクと正常時トルクとを比較した図である。
【図１１】本発明の一実施形態によるパラレルリンクロボットの変形例における第１判定処理について説明するためのフローチャートである。
【図１２】本発明の一実施形態によるパラレルリンクロボットの変形例における第２判定処理について説明するためのフローチャートである。
【発明を実施するための形態】
【００１４】
以下、本発明の実施形態を図面に基づいて説明する。
【００１５】
図１および図２を参照して、本発明の一実施形態によるパラレルリンクロボット１００の構成について説明する。
【００１６】
本実施形態によるパラレルリンクロボット１００は、図１に示すように、ロボット本体１と、ロボット本体１を制御するロボットコントローラ２とを備えている。パラレルリンクロボット１００は、ロボット本体１に取り付けられた所定のエンドエフェクタ１１０を任意の位置に移動させる機能を有している。また、パラレルリンクロボット１００は、ロボット本体１の関節部１３３および１３４（図１および図２参照）の少なくとも一方が外れたか否かを判断する外れ検出機能を有している。なお、外れ検出機能の詳細な説明は後述する。
【００１７】
ロボット本体１は、ベース部１１と、ベース部１１に設けられた３つのサーボモータ１２と、３つのサーボモータ１２によりそれぞれ駆動される３つのアーム部１３と、エンドエフェクタ１１０が取り付けられる１つのヘッド部１４とを備えている。なお、サーボモータ１２は、本発明の「駆動源」の一例である。
【００１８】
ベース部１１は、天井などの固定面に取り付けられている。また、ベース部１１は、カバー部１１１を有し、カバー部１１１の内部に３つのサーボモータ１２が設けられている。また、３つのサーボモータ１２は、平面的に見て（平面視で）、鉛直方向に延びるベース部１１の中心軸Ｌ１回りに互いに等角度間隔（１２０度間隔）で配置されている。
【００１９】
３つのアーム部１３は、互いに同様の構成を有している。具体的には、アーム部１３は、第１リンク１３１と、第１リンク１３１に連結される第２リンク１３２とを有している。第１リンク１３１は、一方端部がサーボモータ１２の出力軸に取り付けられており、サーボモータ１２により鉛直面内で上下方向（Ｚ方向）に揺動（回動）されるように構成されている。第２リンク１３２は、互いに平行に配置された一対のポール１３２ａにより主として構成されている。また、第２リンク１３２の一方端部は、第１関節部１３３を介して第１リンク１３１の他方端部１３１ａに連結されている。また、第２リンク１３２の他方端部は、第２関節部１３４を介してヘッド部１４に連結されている。なお、第１リンク１３１および第２リンク１３２は、本発明の「リンク」の一例である。また、第１関節部１３３および第２関節部１３４は、本発明の「関節部」の一例である。
【００２０】
第１関節部１３３および第２関節部１３４は、図１および図２に示すように、ボールジョイント型の関節構造を有している。具体的には、図２に示すように、第１リンク１３１の他方端部１３１ａには、ブラケット１３３ａを介して一対の球体部１３３ｂが固定的に取り付けられている。また、一対の球体部１３３ｂは、ブラケット１３３ａの両側面から外側に突出するように設けられている。また、第２リンク１３２の一対のポール１３２ａの一方端部（第１リンク１３１側の端部）には、一対のソケット１３３ｃが一対のポール１３２ａに固定的に取り付けられている。一対のソケット１３３ｃは、球体部１３３ｂが挿入されて当接される半球形状の凹部１３３ｄを有している。また、一対のソケット１３３ｃは、一対の球体部１３３ｂを外側から挟み込むように設けられており、一対の球体部１３３ｂに対して互いに摺動可能（回動可能）に構成されている。また、一対のポール１３２ａの一方端部側には、一対のポール１３２ａを互いに近づける方向に付勢する引張コイルバネ１３３ｅが設けられている。これにより、一対のソケット１３３ｃは、一対の球体部１３３ｂに対して外側から押圧された状態で一対の球体部１３３ｂを安定的に保持することが可能である。このような構成により、第１関節部１３３は、第１リンク１３１と第２リンク１３２とを屈伸動作可能に連結している。
【００２１】
第２関節部１３４は、上記第１関節部１３３と同様に構成されている。具体的には、ヘッド部１４には、取付部材１３４ａを介して一対の球体部１３４ｂが固定的に取り付けられている。また、一対の球体部１３４ｂは、取付部材１３４ａの両端部から外側に突出するように設けられている。また、第２リンク１３２の一対のポール１３２ａの他方端部（ヘッド部１４側の端部）には、一対のソケット１３４ｃが一対のポール１３２ａに固定的に取り付けられている。一対のソケット１３４ｃは、球体部１３４ｂが挿入されて摺動可能に当接される半球形状の凹部１３４ｄを有している。また、一対のポール１３２ａの他方端部側には、一対のポール１３２ａを互いに近づける方向に付勢する引張コイルバネ１３４ｅが設けられている。なお、第２関節部１３４の球体部１３４ｂ、ソケット１３４ｃ、凹部１３４ｄおよび引張コイルバネ１３４ｅは、それぞれ、第１関節部１３３の球体部１３３ｂ、ソケット１３３ｃ、凹部１３３ｄおよび引張コイルバネ１３３ｅに対応している。
【００２２】
また、上記のような構成の第１関節部１３３（第２関節部１３４）では、引張コイルバネ１３３ｅ（１３４ｅ）に過度の負荷が掛かったり、球体部１３３ｂ（１３４ｂ）やソケット１３３ｃ（１３４ｃ）が経年劣化により変形したりすることに起因して、ロボット本体１の動作中に、球体部１３３ｂ（１３４ｂ）からソケット１３３ｃ（１３４ｃ）が脱落して（第１関節部１３３（第２関節部１３４）が外れて）しまう場合がある。図４は第２関節部１３４の１つが外れた状態を示している。
【００２３】
また、上記のような構成では、第１関節部１３３および第２関節部１３４のいずれか一方が外れた場合、第２リンク１３２は外れていない方の関節部を支点としてぶら下がることになる。ぶら下がった状態の第２リンク１３２がロボット本体１の動作によって振られると、その勢いで他方の関節部も外れることになる。図５は、図４の状態からさらに第１関節部１３３も外れて第２リンク１３２が脱落した状態を示している。
【００２４】
ヘッド部１４は、３つのアーム部１３により支持されている。また、ヘッド部１４は、３つのアーム部１３それぞれの姿勢の組み合わせにより、任意の位置で任意の姿勢となるように移動される。これにより、ヘッド部１４に取り付けられるエンドエフェクタ１１０を任意の位置で任意の姿勢にすることが可能である。
【００２５】
ロボットコントローラ２は、図１に示すように、ロボット指令ケーブル３を介してロボット本体１に通信可能に接続されている。また、ロボットコントローラ２は、ロボット本体１の各サーボモータ１２に対して動作指令を出力してロボット本体１の動作を制御するように構成されている。また、ロボットコントローラ２は、３つのサーボモータ１２のトルクに基づいて、３組の第１関節部１３３および第２関節部１３４のそれぞれについて外れが発生したか否かを判断可能に構成されている。ユーザは、ロボットコントローラ２による外れ検出機能の有効または無効を選択的に設定可能である。なお、ロボットコントローラ２は、本発明の「外れ検出部」の一例である。また、トルクは、本発明の「駆動力」の一例である。
【００２６】
次に、図３を参照して、ロボットコントローラ２による外れ検出処理について説明する。なお、ロボットコントローラ２は、図３のステップＳ２以下の外れ検出処理を所定の頻度（例えば数ｍｓｅｃ〜数十ｍｓｅｃに一度）で実行する。この頻度はロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。
【００２７】
まず、ステップＳ１において、ロボットコントローラ２は、外れ検出機能が有効か否かを判断する。ユーザにより外れ検出機能が有効に設定されている場合には、ステップＳ２において、ロボットコントローラ２は、３つのサーボモータ１２の軸角度を取得する。３つのサーボモータ１２の軸角度は、対応するアーム部１３の第１リンク１３１の鉛直面内における回動角度である。ロボットコントローラ２は、各サーボモータ１２に設けられた図示しないエンコーダの検出結果に基づいて各サーボモータ１２の軸角度を取得する。また、ロボットコントローラ２は、ステップＳ３において、各サーボモータ１２の軸角速度を取得する。ロボットコントローラ２は、ステップＳ２で取得した軸角度を１回時間微分することにより軸角速度を取得する。また、ロボットコントローラ２は、ステップＳ４において、各サーボモータ１２の軸角加速度を取得する。ロボットコントローラ２は、ステップＳ２で取得した軸角度を２回時間微分することにより軸角加速度を取得する。
【００２８】
次に、ステップＳ５において、ロボットコントローラ２は、各サーボモータ１２の実トルクτＪを取得する。実トルクτＪは、サーボモータ１２に掛かる負荷の増減により変動する。ロボットコントローラ２は、各サーボモータ１２に与える位置指令および速度指令に基づいて各サーボモータ１２の実トルクτＪを取得する。すなわち、ロボットコントローラ２は、各サーボモータ１２に与える指令値に基づいて実トルクτＪを取得する。なお、ステップＳ４で取得される各サーボモータ１２の軸角加速度およびステップＳ５で取得される各サーボモータ１２の実トルクτＪは、ノイズ除去のために一次遅れフィルタを通過させることが好ましい。なお、実トルクτＪは、本発明の「実駆動力」の一例である。なお、ステップＳ２からステップＳ５までの処理は外れ検出処理のためだけに行う必要はない。換言すれば、ロボット本体１の動作制御のために検出される各サーボモータ１２の軸角度、軸角速度、軸角加速度や、トルク指令値を取得して外れ検出処理に用いればよい。
【００２９】
ロボットコントローラ２は、ステップＳ６において、以下の式（１）により各サーボモータ１２の外れ状態時トルクτＤを算出（推定）する。サーボモータ１２の外れ状態時トルクτＤは、アーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れた状態において、対応するサーボモータ１２に掛かるトルクである。第１関節部１３３が外れた場合や、図５に示すように第２リンク１３２が脱落した場合には、サーボモータ１２には第１リンク１３１の質量のみが掛かる。また、図４に示すように、所定のアーム部１３の第２関節部１３４が外れた場合には、ヘッド部１４が第２リンク１３２から外れる。この場合、第２リンク１３２の質量は第１リンク１３１やエンドエフェクタ１１０の質量に対して小さいので、第２リンク１３２の質量を無視して、対応するサーボモータ１２は第１リンク１３１の影響だけを受けると考えられる。このため、本実施形態では、外れ状態時トルクτＤを第１リンク１３１の影響だけを考慮して算出（推定）する。
【００３０】
【数１】
0005408211.tif 000002
なお、上記式（１）において、τＤ１は、３つのアーム部１３のうちの所定のアーム部１３に対応する所定のサーボモータ１２の外れ状態時トルク、Ｊ１は、上記所定のアーム部１３の第１リンク１３１のイナーシャ、Ｄ１は、上記所定のアーム部１３の第１リンク１３１の移動速度に関する係数、Ｃ１は、上記所定のアーム部１３の第１リンク１３１に掛かる重力に関する係数、θ１は、上記所定のサーボモータ１２の軸角度をそれぞれ表す。また、Ｊ１、Ｄ１およびＣ１は、ロボット本体１固有の値である。３つのアーム部１３のその他の２つのアーム部１３を駆動させるサーボモータ１２についても式（１）と同様にしてそれぞれの外れ状態時トルクτＤを算出（推定）する。
【００３１】
次に、ロボットコントローラ２は、ステップＳ７において、以下の式（２）により各サーボモータ１２の付加的トルクτＦを算出（推定）する。サーボモータ１２の付加的トルクτＦは、アーム部１３が正常状態（関節部が外れていない状態）である場合に、第１リンク１３１に対して第２リンク１３２側から加えられる負荷に起因するトルクである。この付加的トルクτＦは、対応するアーム部１３以外の他のアーム部１３からの干渉力に起因するトルク成分と、非線形項に対応するトルク成分とを含む。また、本実施形態では、第２リンク１３２の質量が第１リンク１３１やエンドエフェクタ１１０の質量に対して小さいため、付加的トルクτＦの算出において第２リンク１３２の質量を無視している。また、サーボモータ１２の付加的トルクτＦは、アーム部１３が正常状態（関節部が外れていない状態）である場合に、ヘッド部１４やエンドエフェクタ１１０などのアーム部１３以外の部分を駆動させるためのトルク成分を含むトルクであると言い換えることが可能である。また、サーボモータ１２の付加的トルクτＦは、３つのアーム部１３それぞれの姿勢により変動するとともに、対応するアーム部１３以外の他のアーム部１３からの干渉力の影響を受ける。なお、付加的トルクτＦは、本発明の「所定の駆動力」の一例である。
【００３２】
【数２】
0005408211.tif 000003
なお、上記式（２）において、τＦ１は、３つのアーム部１３のうちの所定のアーム部１３に対応する所定のサーボモータ１２の付加的トルク、ｂ１、ｃ１、ｄ１、ｅ１、ｆ１、ｇ１、ｈ１およびｉ１は、３つのアーム部１３それぞれの姿勢により決定される係数、θ１は、上記所定のサーボモータ１２の軸角度、θ２およびθ３は、上記所定のサーボモータ１２以外の他の２つのサーボモータ１２の軸角度をそれぞれ表す。また、上記式（２）のｂ１およびｃ１に係る項は、所定のアーム部１３以外のアーム部１３からの干渉力に起因するトルクであり、ｄ１、ｅ１、ｆ１、ｇ１、ｈ１およびｉ１に係る項は、非線形項である。
【００３３】
上記式（２）の係数ｂ１、ｃ１、ｄ１、ｅ１、ｆ１、ｇ１、ｈ１およびｉ１の算出方法は、予めロボット本体１に関する運動方程式を解くことによって求めることができるが、詳細な算出方法の説明は割愛する。なお式（２）のｃ１は式（１）のＣ１とは異なる。また、ステップＳ６と同様に、３つのアーム部１３のその他の２つのアーム部１３を駆動させるサーボモータ１２についても式（２）と同様にしてそれぞれの付加的トルクτＦを算出（推定）する。
【００３４】
そして、ロボットコントローラ２は、ステップＳ８において、以下の式（３）により各サーボモータ１２の正常時トルクτＳを算出（推定）する。サーボモータ１２の正常時トルクτＳは、アーム部１３の正常状態（関節部が外れていない状態）における対応するサーボモータ１２に掛かるトルクである。すなわち、所定の正常時トルクτＳは、所定のアーム部１３を駆動させるためのトルク成分と、ヘッド部１４やエンドエフェクタ１１０などの所定のアーム部１３以外の部分を駆動させるためのトルク成分とを含むトルクである。なお、本実施形態では、上記のとおり、第２リンク１３２の質量を無視しているので、所定のアーム部１３を駆動させるためのトルク成分は、第１リンク１３１を駆動させるためのトルク成分と実質的に同じになる。
【００３５】
【数３】
0005408211.tif 000004
なお、上記式（３）において、τＳ１は、３つのアーム部１３のうちの所定のアーム部１３に対応する所定のサーボモータ１２の正常時トルク、τＤ１は、上記所定のサーボモータ１２の外れ状態時トルク、τＦ１は、上記所定のサーボモータ１２の付加的トルクをそれぞれ表す。また、ステップＳ６およびステップＳ７と同様に、３つのアーム部１３のその他の２つのアーム部１３を駆動させるサーボモータ１２についても式（３）と同様にしてそれぞれの正常時トルクτＳを算出（推定）する。
【００３６】
その後、ロボットコントローラ２は、ステップＳ９において、第１および第２判定処理を互いに並行して実行する。そして、ロボットコントローラ２は、上記ステップＳ２〜Ｓ９の一連の動作を前述の所定の頻度で繰り返し実行する。ステップＳ９では、３つのアーム部１３を駆動させる各サーボモータ１２について個別に第１および第２判定処理が行われる。
【００３７】
次に、図６を参照して、第１判定処理について説明する。まず、ステップＳ９１１において、ロボットコントローラ２は、上記ステップＳ７で算出（推定）した各サーボモータ１２の付加的トルクτＦがしきい値τａ以上か否かを判断する。なお、しきい値τａは、本発明の「第２のしきい値」の一例である。しきい値τａはロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。付加的トルクτＦがしきい値τａ以上の場合には、ロボットコントローラ２は、ステップＳ９１２において、各サーボモータ１２について上記ステップＳ５で取得した実トルクτＪと上記ステップＳ６で算出（推定）した外れ状態時トルクτＤとの差の絶対値を算出する。そして、ロボットコントローラ２は、ステップＳ９１３において、各サーボモータ１２について実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下か否かを判断する。なお、しきい値τｂは、本発明の「第１のしきい値」の一例である。しきい値τｂはロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。いずれかのサーボモータ１２の実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下であれば、ロボットコントローラ２は、ステップＳ９１４において、Ｍの値をカウントアップ（Ｍ＝Ｍ＋１）する。なお、Ｍは、ゼロ以上の整数である。
【００３８】
そして、ロボットコントローラ２は、ステップＳ９１５において、カウンタの回数Ｍが所定回数（例えば数回〜数十回）以上か否かを判断する。この回数はロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。すなわち、ロボットコントローラ２は、付加的トルクτＦがしきい値τａ以上で、かつ、実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下である状態が所定時間継続しているか否かを判断する。ロボットコントローラ２は、外れ検出処理を数ｍｓｅｃ〜数十ｍｓｅｃに１回の頻度で実行するとすれば、所定時間は数ｍｓｅｃ〜数百ｍｓｅｃとなり、この間継続して上記条件を満たしたか否かを判断していることになる。また、ロボットコントローラ２は、回数Ｍが所定回数未満であれば、第１判定処理を終了し、回数Ｍが所定回数以上であれば、ステップＳ９１６において、アーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断する。そして、ロボットコントローラ２は、ステップＳ９１７において、３つのサーボモータ１２の駆動を停止してロボット本体１の動作を停止させるとともに、ユーザに関節部が外れたことを知らせるためのエラー報知を行う。なお、エラー報知は、例えばロボットコントローラ２に設けた表示部（図示せず）により視覚的に報知してもよいし、音声により聴覚的に報知してもよい。
【００３９】
また、ステップＳ９１１で付加的トルクτＦがしきい値τａ以上でない場合（しきい値τａ未満の場合）、または、ステップＳ９１３で実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下でない場合（しきい値τｂよりも大きい場合）には、ロボットコントローラ２は、ステップＳ９１８において、カウンタをクリア（Ｍ＝０）する。
【００４０】
次に、図７を参照して、第２判定処理について説明する。まず、ステップＳ９２１において、ロボットコントローラ２は、上記ステップＳ５で取得した各サーボモータ１２の実トルクτＪと上記ステップＳ８で算出（推定）した正常時トルクτＳとの差の絶対値を算出する。そして、ロボットコントローラ２は、ステップＳ９２２において、実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上か否かを判断する。なお、しきい値τｃは、本発明の「第３のしきい値」の一例である。しきい値τｃはロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。いずれかのサーボモータ１２の実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上であれば、ロボットコントローラ２は、ステップＳ９２３において、Ｎの値をカウントアップ（Ｎ＝Ｎ＋１）する。なお、Ｎは、ゼロ以上の整数である。
【００４１】
そして、ロボットコントローラ２は、ステップＳ９２４において、カウンタの回数Ｎが所定回数（例えば数回〜数十回）以上か否かを判断する。この回数はロボットコントローラ２内の不揮発性メモリ（図示せず）に書き換え可能なパラメータとして設定可能である。すなわち、ロボットコントローラ２は、実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上である状態が所定時間継続しているか否かを判断する。ロボットコントローラ２は、外れ検出処理を数ｍｓｅｃ〜数十ｍｓｅｃに１回の頻度で実行するとすれば、所定時間は数ｍｓｅｃ〜数百ｍｓｅｃとなり、この間継続して上記条件を満たしたか否かを判断していることになる。また、ロボットコントローラ２は、回数Ｎが所定回数未満であれば、第２判定処理を終了し、回数Ｎが所定回数以上であれば、ステップＳ９２５において、アーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断する。そして、ロボットコントローラ２は、ステップＳ９２６において、上記ステップＳ９１７と同様に、３つのサーボモータ１２の駆動を停止してロボット本体１の動作を停止させるとともに、ユーザに関節部が外れたことを知らせるためのエラー報知を行う。
【００４２】
また、ステップＳ９２２で実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上でない場合（しきい値τｃ未満の場合）には、ロボットコントローラ２は、ステップＳ９２７において、カウンタをクリア（Ｎ＝０）する。また、本実施形態では、上記のように、第１および第２判定処理が互いに並行して実行されており、第１および第２判定処理の少なくとも一方の条件が満たされた場合に、ロボットコントローラ２は、アーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断してロボット本体１の動作を停止させる。
【００４３】
本実施形態では、上記のように、３つのサーボモータ１２のトルクに基づいて、アーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたか否かを判断するようにロボットコントローラ２を構成することによって、外れ検出のために関節部に導電部材などの専用部材を別途設けることなく、各サーボモータ１２のトルクに基づいて関節部が外れたか否かを判断することができるので、アーム部１３の関節部の構造が複雑になるのを抑制しながら、関節部の外れを検出することができる。また、外れ検出のために既存のロボットコントローラ２を用いることによって、外れ検出のために導電部材などの専用部材を別途設ける必要がないので、部品点数の増加を抑制することができる。
【００４４】
また、本実施形態では、上記のように、所定のアーム部１３の外れ状態および正常状態における対応する所定のサーボモータ１２のトルク（外れ状態時トルクτＤおよび正常時トルクτＳ）を推定するとともに、所定のサーボモータ１２の推定したトルクと所定のサーボモータ１２の実トルクτＪとに基づいて、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたか否かを判断するようにロボットコントローラ２を構成する。このように構成すれば、推定したトルク（外れ状態時トルクτＤおよび正常時トルクτＳ）と実トルクτＪとを比較して関節部が外れたか否かを判断することができるので、関節部の外れを容易に検出することができる。
【００４５】
また、本実施形態では、上記のように、所定のサーボモータ１２の外れ状態時トルクτＤと所定のサーボモータ１２の実トルクτＪとの差がしきい値τｂ以下の場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、実トルクτＪが推定した外れ状態時トルクτＤに近づいた場合に関節部の外れを確実に検出することができる。
【００４６】
また、本実施形態では、上記のように、所定のサーボモータ１２の付加的トルクτＦがしきい値τａ以上の場合で、かつ、所定のサーボモータ１２の外れ状態時トルクτＤと所定のサーボモータ１２の実トルクτＪとの差がしきい値τｂ以下の場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、関節部が外れていない状態（正常状態）において、所定のアーム部１３以外の部分（ヘッド部１４やエンドエフェクタ１１０）を駆動させるためにサーボモータ１２に掛かるトルクが小さくなった結果（付加的トルクτＦが小さくなる状態となった結果）、外れ状態時トルクτＤと実トルクτＪとの差が小さくなった場合に、関節部が外れていない状態（正常状態）であるにも関わらず、関節部が外れたと誤検出してしまうのを抑制することができる。その結果、関節部の外れをより正確に検出することができる。
【００４７】
また、本実施形態では、上記のように、所定のサーボモータ１２の外れ状態時トルクτＤと所定のサーボモータ１２の実トルクτＪとの差がしきい値τｂ以下となる状態が所定時間継続した場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、外れ状態時トルクτＤと実トルクτＪとの差がしきい値τｂ以下となる状態が極短時間生じただけで関節部が外れたと判断されるのを抑制することができるので、関節部の外れの誤検出を抑制することができる。これによっても、関節部の外れをより正確に検出することができる。
【００４８】
また、本実施形態では、上記のように、所定のサーボモータ１２の正常時トルクτＳと実トルクτＪとの差がしきい値τｃ以上の場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、実トルクτＪが推定した正常時トルクτＳから乖離した場合に関節部の外れを確実に検出することができる。
【００４９】
また、本実施形態では、上記のように、所定のサーボモータ１２の正常時トルクτＳと実トルクτＪとの差がしきい値τｃ以上となる状態が所定時間継続した場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、正常時トルクτＳと実トルクτＪとの差がしきい値τｃ以上となる状態が極短時間生じただけで関節部が外れたと判断されるのを抑制することができるので、関節部の外れの誤検出を抑制することができる。
【００５０】
また、本実施形態では、上記のように、所定のサーボモータ１２の外れ状態時トルクτＤと実トルクτＪとの差がしきい値τｂ以下の場合、および、所定のサーボモータ１２の正常時トルクτＳと実トルクτＪとの差がしきい値τｃ以上の場合の少なくともいずれか一方の場合に、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断するようにロボットコントローラ２を構成する。このように構成すれば、外れ状態時トルクτＤと実トルクτＪとの差に基づく判断と、正常時トルクτＳと実トルクτＪとの差に基づく判断との複数の異なる判断基準で関節部の外れの有無を判断することができるので、関節部の外れを確実に検出することができる。
【００５１】
また、本実施形態では、上記のように、所定のサーボモータ１２の推定したトルク（外れ状態時トルクτＤおよび正常時トルクτＳ）と実トルクτＪとに基づいて、所定のアーム部１３の関節部（第１関節部１３３および第２関節部１３４の少なくとも一方）が外れたと判断した場合には、３つのサーボモータ１２を停止させる制御を行うようにロボットコントローラ２を構成する。このように構成すれば、関節部の外れに起因してパラレルリンクロボット１００が所望の動作を行うことができない状態であるにも関わらず、アーム部１３がサーボモータ１２により継続して駆動されてしまうのを抑制することができる。その結果、アーム部１３やアーム部１３に支持されたエンドエフェクタ１１０などが意図しない位置に移動されるのを抑制することができる。
【００５２】
また、本願の発明者は、上記外れ検出処理における第１判定処理（図６参照）および第２判定処理（図７参照）による外れ検出精度を確認するための実験を行った。図８〜図１０を参照して、所定のアーム部１３についての第１判定処理（図６参照）および第２判定処理（図７参照）による外れ検出精度について説明する。なお、図８〜図１０では、縦軸にサーボモータ１２の定格トルクに対するトルク比率（％）を示し、横軸に時間（ｓ）を示している。
【００５３】
まず、図８に示すように、アーム部１３の関節部が外れている状態では、実トルクτＪと上記式（１）により算出（推定）された外れ状態時トルクτＤとは、略合致した状態で連動しており、上記式（１）により外れ状態時トルクτＤを精度よく算出（推定）可能であることが確認できた。
【００５４】
また、図９に示すように、アーム部１３の関節部が外れていない状態（正常状態）では、実トルクτＪと上記式（３）により算出（推定）された正常時トルクτＳとは、略合致した状態で連動しており、上記式（３）により正常時トルクτＳを精度よく算出（推定）可能であることが確認できた。また、上記式（２）により算出（推定）された付加的トルクτＦは、実トルクτＪよりも小さい増減幅で実トルクτＪの増減に連動している。
【００５５】
また、図１０の正常状態の区間では、実トルクτＪと上記式（３）により算出（推定）された正常時トルクτＳとは、上記図９の場合と同様に略合致した状態で連動しており、実トルクτＪと上記式（１）により算出（推定）された外れ状態時トルクτＤとは、互いに乖離した状態で変動している。これに対して、外れ発生後（外れ状態の区間）では、実トルクτＪと上記式（１）により算出（推定）された外れ状態時トルクτＤとが、上記図８の場合と同様に略合致した状態（第１判定処理における実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下である状態）で連動するようになり、実トルクτＪと上記式（３）により算出（推定）された正常時トルクτＳとは、互いに乖離した状態（第２判定処理における実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上である状態）で変動している。そして、関節部の外れが発生してから外れ状態が所定時間継続した後、ロボットコントローラ２により関節部が外れていると判断されてサーボモータ１２の駆動が停止されている。
【００５６】
なお、今回開示された実施形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施形態の説明ではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれる。
【００５７】
たとえば、上記実施形態では、本発明のパラレルリンクロボットの一例として、ロボット本体とロボットコントローラとを備えたパラレルリンクロボットを示したが、本発明はこれに限られない。本発明では、ロボットコントローラを含まないパラレルリンクロボットであってもよい。この場合、パラレルリンクロボットとロボットコントローラとにより、本発明のパラレルリンクロボットシステムが主として構成される。
【００５８】
また、上記実施形態では、本発明のパラレルリンクロボットの一例として、３つのアーム部を備えたパラレルリンクロボットを示したが、本発明はこれに限られない。本発明では、２つまたは４つ以上のアーム部を備えたパラレルリンクロボットであってもよい。
【００５９】
また、上記実施形態では、サーボモータ（駆動源）の推定したトルク（駆動力）と、サーボモータの実トルク（実駆動力）とに基づいて、関節部の外れ検出を行う構成を示したが、本発明はこれに限られない。本発明では、サーボモータの駆動力に基づいていれば、実駆動力または推定した駆動力のいずれか一方だけに基づいて関節部の外れ検出を行う構成であってもよい。
【００６０】
また、上記実施形態では、本発明の駆動源の一例としてサーボモータを示すとともに、本発明の駆動力の一例としてトルクを示したが、本発明はこれに限られない。本発明では、サーボモータ以外の駆動源であってもよいし、トルク以外の駆動力であってもよい。
【００６１】
また、上記実施形態では、全てのサーボモータ（駆動源）のトルク（駆動力）に基づいて、関節部の外れ検出を行う構成を示したが、本発明はこれに限られない。本発明では、少なくとも１つの駆動源の駆動力に基づいて、関節部の外れ検出を行う構成であってもよい。
【００６２】
また、上記実施形態では、図６に示した第１判定処理および図７に示した第２判定処理の両方を用いて関節部の外れ検出を行う構成を示したが、本発明はこれに限られない。本発明では、第１判定処理および第２判定処理のいずれか一方だけを用いて関節部の外れ検出を行う構成であってもよい。
【００６３】
また、上記実施形態では、図６に示した第１判定処理において、ステップＳ９１１で付加的トルクτＦがしきい値τａ以上でない場合、または、ステップＳ９１３で実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下でない場合に、カウンタをクリア（Ｍ＝０）する構成を示したが、本発明はこれに限られない。本発明では、図１１に示すように、ステップＳ９１１で付加的トルクτＦがしきい値τａ以上でない場合、または、ステップＳ９１３で実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下でない場合に、ステップＳ９３１において、ロボットコントローラ２により、カウンタが所定回数の５０％を超えているか否かを判断する構成であってもよい。超えていなければ、ステップＳ９１８においてカウンタをクリア（Ｍ＝０）する。
【００６４】
一方、カウンタが所定回数の５０％を超えている場合には、ステップＳ９３２において、ロボットコントローラ２により、本状態が所定回数の２５％以上継続しているか否かを判断する。すなわち、ロボットコントローラ２は、付加的トルクτＦがしきい値τａ以上でない状態、または、実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下でない状態が所定回数の２５％以上継続しているか否かを判断する。２５％以上継続している場合には、ステップＳ９１８においてカウンタをクリア（Ｍ＝０）し、２５％以上継続していない場合（２５％未満の場合）には、カウンタをクリアすることなく第１判定処理を終了する。これにより、所定時間のうちの一定時間以上の間、付加的トルクτＦがしきい値τａ以上で、かつ、実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下の状態が継続したにも関わらず、付加的トルクτＦがしきい値τａ以上でない状態、または、実トルクτＪと外れ状態時トルクτＤとの差の絶対値がしきい値τｂ以下でない状態が極短時間生じただけで、カウンタがクリアされてしまうのを抑制することができる。その結果、外れ検出の精度を向上させることができる。
【００６５】
また、上記実施形態では、図７に示した第２判定処理において、ステップＳ９２２で実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上でない場合に、カウンタをクリア（Ｎ＝０）する構成を示したが、本発明はこれに限られない。本発明では、上記第１判定処理の場合と同様に、図１２に示すように、ステップＳ９２２で実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上でない場合に、ステップＳ９４１において、ロボットコントローラ２により、カウンタが所定回数の５０％を超えているか否かを判断する構成であってもよい。超えていなければ、ステップＳ９２７においてカウンタをクリア（Ｎ＝０）する。
【００６６】
また、カウンタが所定回数の５０％を超えている場合には、ステップＳ９４２において、ロボットコントローラ２により、本状態が所定回数の２５％以上継続しているか否かを判断する。すなわち、ロボットコントローラ２は、実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上でない状態が所定回数の２５％以上継続しているか否かを判断する。２５％以上継続している場合には、ステップＳ９２７においてカウンタをクリア（Ｎ＝０）し、２５％以上継続していない場合（２５％未満の場合）には、カウンタをクリアすることなく第２判定処理を終了する。これにより、所定時間のうちの一定時間以上の間、実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上である状態が継続したにも関わらず、実トルクτＪと正常時トルクτＳとの差の絶対値がしきい値τｃ以上でない状態が極短時間生じただけで、カウンタがクリアされてしまうのを抑制することができる。その結果、外れ検出の精度を向上させることができる。
【００６７】
また、上記実施形態では、説明の便宜上、本発明の外れ検出部としてのロボットコントローラの処理を処理フローに沿って順番に処理を行うフロー駆動型のフローチャートを用いて説明したが、本発明はこれに限られない。本発明では、外れ検出部の処理動作を、イベント単位で処理を実行するイベント駆動型（イベントドリブン型）の処理により行ってもよい。この場合、完全なイベント駆動型で行ってもよいし、イベント駆動およびフロー駆動を組み合わせて行ってもよい。
【符号の説明】
【００６８】
１  ロボット本体（パラレルリンクロボット）
２  ロボットコントローラ（外れ検出部）
１１  ベース部
１２  サーボモータ（駆動源）
１３  アーム部
１００  パラレルリンクロボット（パラレルリンクロボットシステム）
１３１  第１リンク（リンク）
１３２  第２リンク（リンク）
１３３  第１関節部（関節部）
１３４  第２関節部（関節部）
【図１】
0005408211.tif 000005
【図２】
0005408211.tif 000006
【図３】
0005408211.tif 000007
【図４】
0005408211.tif 000008
【図５】
0005408211.tif 000009
【図６】
0005408211.tif 000010
【図７】
0005408211.tif 000011
【図８】
0005408211.tif 000012
【図９】
0005408211.tif 000013
【図１０】
0005408211.tif 000014
【図１１】
0005408211.tif 000015
【図１２】
0005408211.tif 000016