JP2021146438A - ロボット制御装置、ロボット制御システム、及びロボット制御方法 - Google Patents

Abstract

【課題】パラレルリンクロボットの機構部における異常を判定する。【解決手段】パラレルリンクロボット４を制御するロボット制御装置３であって、パラレルリンクロボット４における複数のモータ４４を制御する駆動制御部３１と、パラレルリンクロボット４の機構部５０における衝突および脱臼の少なくともいずれか一方を、複数のモータ４４の状態データに基づいて判定する異常判定部３２と、を有し、異常判定部３２が衝突を判定した場合、駆動制御部３１は、Ｚ軸方向が鉛直方向に対応したＸＹＺ軸直交座標系の動作空間におけるＸＹ軸方向での衝突とＺ軸方向での衝突とで機構部５０の動作が異なるように複数のモータ４４を制御する。【選択図】図３

Description

開示の実施形態は、ロボット制御装置、ロボット制御システム、及びロボット制御方法に関する。

例えば特許文献１には、ロボット機構を駆動するモータの状態情報を周波数帯域別に取り出し、各周波数帯域ごとに出力を積算した値に基づいて異常を検出する異常監視装置が記載されている。

特開２０２０−０２２３２９号公報

一方、パラレルリンクロボットのように閉リンク機構を複数の駆動軸で連携制御する構成においては、それら複数の駆動軸が互いに異なるトルクを付加して相殺させる状況があり、それら駆動軸の間の状態情報の関係は複雑となる。このためパラレルリンクロボットの制御においては、上記従来技術のように１つの駆動軸の状態情報だけで衝突や脱臼などの異常を判定することは困難であった。

本発明はこのような問題点に鑑みてなされたものであり、パラレルリンクロボットの機構部における異常を判定可能なロボット制御装置、ロボット制御システム、及びロボット制御方法を提供することを目的とする。

上記課題を解決するため、本発明の一の観点によれば、パラレルリンクロボットを制御するロボット制御装置であって、前記パラレルリンクロボットにおける複数の駆動軸を制御する駆動制御部と、前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記複数の駆動軸の状態データに基づいて判定する異常判定部と、を有するロボット制御装置が適用される。

また、本発明の別の観点によれば、パラレルリンクロボットと、前記パラレルリンクロボットを制御するロボット制御装置とを有するロボット制御システムであって、前記ロボット制御装置は、前記パラレルリンクロボットにおける複数の駆動軸を制御する駆動制御部と、前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記複数の駆動軸の状態データに基づいて判定する異常判定部と、を有するロボット制御システムが適用される。

また、本発明の別の観点によれば、パラレルリンクロボットを制御するロボット制御装置が備える演算装置が実行するロボット制御方法であって、前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記パラレルリンクロボットにおける複数の駆動軸の状態データに基づいて判定し、前記衝突を判定した場合、前記パラレルリンクロボットにおける複数の駆動軸に対して所定の制御を実行し、前記脱臼を判定した場合、前記複数の駆動軸の制御を停止するロボット制御方法が適用される。

本発明によれば、パラレルリンクロボットの機構部における異常を判定できる。

第１実施形態に係るロボット制御システム全体構成の一例を表す斜視図である。 モータの概略構成の一例を表す図である。 ロボット制御装置の内部構成とその周辺で送受される各種情報の一例を表す図である。 サーボ内において処理されるフィードバックループの一例を表す図である。 衝突判定手法を説明する１軸駆動機構モデルの一例を表す図である。 １軸駆動機構モデルの制御シーケンスにおいてモータの状態データの時間変化の一例を表す図である。 パラレルリンクロボットの機構モデルの一例を表す図である。 ＸＹ軸方向での衝突に対する対処動作の一例を表す図である。 Ｚ軸方向での衝突に対する対処動作の一例を表す図である。 脱臼に対する対処動作の一例を表す図である。 異常判定処理の制御手順の一例を表すフローチャートである。 作業制御処理の制御手順の一例を表すフローチャートである。 第２実施形態に係るロボット制御システム全体構成の一例を表す斜視図である。 ロボット制御装置の内部構成とその周辺で送受される各種情報の一例を表す図である。 ロボット制御装置のハードウェア構成を表すシステムブロック図である。

＜１．第１実施形態＞
＜１−１．ロボット制御システムの概略的構成＞
以下、第１実施形態のロボット制御システムについて図面を参照しつつ説明する。本実施形態の例に示すロボット制御システムは、例えばピックアンドプレースなどのワークの移送作業等を実行するパラレルリンクロボットを制御するシステムである。図１においてロボット制御システム１は、上位制御装置２と、ロボット制御装置３と、パラレルリンクロボット４とを有している。なお、以下において、パラレルリンクロボット等の構成の説明の便宜上、上下左右前後等の方向を適宜使用する場合があるが、パラレルリンクロボット４等の各構成の位置関係を限定するものではない。

上位制御装置２は、例えばユーザによる入力操作や、特に図示しない通信回線を介してパラレルリンクロボット４に実行させる作業の種類を特定し、対応する作業指令を後述のロボット制御装置３に出力する。

ロボット制御装置３は、上記の上位制御装置２から入力された作業指令に対応する所定の作業シーケンスに従い、パラレルリンクロボット４が備える複数のモータ４４にそれぞれモータ駆動電力を給電することで当該パラレルリンクロボット４の動作を制御する。また後述するように、パラレルリンクロボット４の上記複数のモータ４４にはそれぞれの回転子の回転位置をモータ検出位置として検出するエンコーダ部１４が設けられており、ロボット制御装置３は各モータ検出位置を参照して上記作業シーケンスに基づく動作を実現するよう各モータ駆動電力の給電を制御する。なお、このロボット制御装置３の内部構成や処理内容については、後に詳述する（後述の図３参照）。

パラレルリンクロボット４は、図示する例では３つのモータ４４を備えており、各モータ４４の軸駆動によってエンドエフェクタ（特に図示せず）をその動作空間における任意の座標位置へ移動させるなどの動作制御が可能な機械システムである。

＜１−２．パラレルリンクロボットの構成＞
図１に示すように、この例のパラレルリンクロボット４は、基礎部４１と、可動部４２と、３つのリンク機構部４３ａ，４３ｂ，４３ｃと、３つのモータ４４ａ，４４ｂ，４４ｃとを有する。

３つのリンク機構部４３ａ，４３ｂ，４３ｃは、パラレルリンクロボット４の中心軸ＡＸ周りの円周方向に沿って配置され、基礎部４１と可動部４２とを連結する。３つのモータ４４ａ，４４ｂ，４４ｃは、基礎部４１に配置され、それぞれリンク機構部４３ａ，４３ｂ，４３ｃを駆動する。基礎部４１は、この例では円盤状に形成されており、その内部に３つのモータ４４ａ〜４４ｃを固定して収容している。可動部４２は、この例では円盤状に形成されており、下端に取付部材４５を備える。取付部材４５には、例えばロボットハンド等の図示しないエンドエフェクタが取り付けられる。なお、各モータ４４ａ〜４４ｃの構成については、後に詳述する（後述の図２参照）。

３つのリンク機構部４３ａ〜４３ｃはいずれも同様の構成を有している。リンク機構部４３ａは、モータ４４ａの出力軸に連結された駆動リンク４６ａと、駆動リンク４６ａと可動部４２とに連結された２つの受動リンク４７ａとを有する。２つの受動リンク４７ａは、それぞれ球面軸受４８ａを介して駆動リンク４６ａと連結され、それぞれ球面軸受４９ａを介して可動部４２と連結される。リンク機構部４３ｂは、モータ４４ｂの出力軸に連結された駆動リンク４６ｂと、駆動リンク４６ｂと可動部４２とに連結された２つの受動リンク４７ｂとを有する。２つの受動リンク４７ｂは、それぞれ球面軸受４８ｂを介して駆動リンク４６ｂと連結され、それぞれ球面軸受４９ｂを介して可動部４２と連結される。リンク機構部４３ｃは、モータ４４ｃの出力軸に連結された駆動リンク４６ｃと、駆動リンク４６ｃと可動部４２とに連結された２つの受動リンク４７ｃとを有する。２つの受動リンク４７ｃは、それぞれ球面軸受４８ｃを介して駆動リンク４６ｃと連結され、それぞれ球面軸受４９ｃを介して可動部４２と連結される。駆動リンク４６ａ，４６ｂ，４６ｃは直線状の部材であり、中心軸ＡＸを中心とする半径方向に延設されている。本実施形態においては、上記の３つのリンク機構部４３ａ，４３ｂ，４３ｃ、可動部４２、取付部材４５、及びエンドエフェクタ（特に図示せず）をまとめて機構部５０という。

なお、パラレルリンクロボット４には、エンドエフェクタの動作空間においてＺ軸方向が鉛直方向に対応したＸＹＺ軸直交座標のロボット座標系が設定されている。また、パラレルリンクロボット４が備えるモータ４４とそれに対応するリンク機構部４３は図示する例の３組に限定されるものではなく、４組以上備えた構成（つまり４軸以上の多軸駆動型の構成）としてもよい（特に図示せず）。

図２は、各モータ４４ａ〜４４ｃの概略構成の一例について説明する。３つのモータ４４ａ〜４４ｃは同様の構成を有する。図２に示すように、モータ４４（４４ａ〜４４ｃ）は、モータ本体部１２と、ブレーキ部１３と、エンコーダ部１４と、減速機１５とを有する。なお、この構成のモータ４４ａ〜４４ｃが、各請求項記載の駆動軸の一例に相当する。

モータ本体部１２は、固定子及び回転子（図示省略）を備え、モータ駆動電力が給電されることにより回転子が固定子に対し回転する回転型（ロータリタイプ）のモータである。

ブレーキ部１３は、ブレーキ信号が入力されることにより回転子の回転を制動する。

エンコーダ部１４は、回転子の位置（「回転位置」や「回転角度」等ともいう）を検出し、その位置をモータ検出位置として出力する。

減速機１５は、回転子の回転軸（図示省略）を入力軸とし、その軸出力を内部に備える歯車減速機構を介して減速変換（位置変換、トルク変換）してシャフトＳＨに出力する。上記の駆動リンク４６（４６ａ〜４６ｃ）は、このシャフトＳＨに固定されて揺動駆動される。

＜１−３．ロボット制御装置の詳細構成＞
図３は、ロボット制御装置３の内部構成とその周辺で送受される各種情報を表している。この図３において、ロボット制御装置３は、駆動制御部３１と、異常判定部３２を有している。

駆動制御部３１は、パラレルリンクロボット４が備える３つのモータ４４ａ〜４４ｃを制御する。駆動制御部３１は、作業制御部３３と、モーション制御部３４と、各モータ４４ａ〜４４ｃに対応する３つのサーボ３５ａ，３５ｂ，３５ｃを有している。

作業制御部３３は、上記の上位制御装置２から入力された作業指令に対応する作業シーケンスに従って、パラレルリンクロボット４のエンドエフェクタの移動先位置である座標位置指令をモーション制御部３４へ出力する。この座標位置指令は、上記ロボット座標系上における３次元座標位置であり、作業制御部３３は常にエンドエフェクタが次に移動すべき移動先位置として座標位置指令を逐次出力し続ける（同じ座標位置を出力し続けることで移動停止）。

また作業制御部３３は、後述の異常判定部３２から入力される異常判定情報に基づいて、全てのサーボ３５に対しモータ駆動電力の給電停止を指示するサーボオフ信号と、出力トルクの制限を指示するトルク制限信号とをそれぞれ必要に応じて出力可能となっている。また作業制御部３３は、同様に異常判定情報に基づいて、全てのモータ４４のブレーキ部１３に対しモータ４４の回転を制動するブレーキ信号をそれぞれ必要に応じて出力可能となっている。なお、パラレルリンクロボット４の取付部材４５に取り付けられるエンドエフェクタ自体の動作についても当該作業制御部３３が上記作業シーケンスに従って制御するが、以下においては説明の便宜上、エンドエフェクタの制御に関する図示説明を省略する。また、この作業制御部３３における処理内容については、後に詳述する。

モーション制御部３４は、上記作業制御部３３から入力された座標位置指令へのエンドエフェクタの移動に必要な各モータ４４の目標位置をいわゆる逆キネマティック演算により算出し、それぞれモータ位置指令として対応するサーボ３５へ逐次出力する。

サーボ３５は、対応するモータ４４のエンコーダ部１４から検出されたモータ検出位置を参照しつつ、上記モーション制御部３４から入力されたモータ位置指令に基づいて当該モータ４４を駆動制御（この場合は位置制御）する駆動電力の給電制御を行う。また各サーボ３５は、それぞれの内部で生成されたモータトルク指令と、対応するエンコーダ部１４から検出されたモータ検出位置及びモータ検出速度とをまとめてモータ状態データとして後述の異常判定部３２へ逐次出力する。なお、このサーボ３５における制御内容については、後に詳述する（後述の図４参照）。

異常判定部３２は、各サーボ３５から入力されるモータ状態データ（この例のモータトルク指令、モータ検出位置、及びモータ検出速度）に基づいて、パラレルリンクロボット４の機構部５０において後述する衝突や脱臼などの異常状態が発生しているかを判定し、その判定結果とそれに関係する情報を含んだ異常判定情報を作業制御部３３に出力する。なお、この異常判定部３２における処理内容についても、後に詳述する。

なお、上述した駆動制御部３１（作業制御部３３、モーション制御部３４、サーボ３５）、異常判定部３２等における処理等は、これらの処理の分担の例に限定されるものではなく、例えば、更に少ない数の処理部（例えば１つの処理部）で処理されてもよく、また、更に細分化された処理部により処理されてもよい。また、ロボット制御装置３は、後述するＣＰＵ９０１（演算装置：図１５参照）が実行するプログラムによりソフトウェア的に実装されてもよいし、その一部又は全部がＡＳＩＣやＦＰＧＡ、その他の電気回路等の実際の装置によりハードウェア的に実装されてもよい。

＜１−４．サーボにおける制御処理構成＞
図４は、上記サーボ３５内において処理されるフィードバックループを表している。この図４に示すフィードバックループは、サーボ３５において実行される制御処理を伝達関数形式で表現したものである。本実施形態の例では、モーション制御部３４が出力したモータ位置指令に基づいてサーボ３５が位置制御を行うものとし、それに対応して図示するような位置制御フィードバックループと速度制御フィードバックループの二重ループ処理を実行する。

この二重ループ処理では、モーション制御部３４から入力されたモータ位置指令とエンコーダ部１４から検出されたモータ検出位置との間の偏差を位置偏差として求め、この位置偏差に基づいて位置制御部６１が速度指令を生成する。さらに、この速度指令とエンコーダ部１４から検出されたモータ検出速度との間の偏差を速度偏差として求め、この速度偏差に基づいて速度制御部６２がモータトルク指令を生成する。なおモータ検出速度は、図示するようにモータ検出位置を微分演算子６５により１階時間微分して算出すればよい。そして、トルクリミッタ６３を経由したモータトルク指令に基づいてＰＷＭ制御部６４が駆動電力を給電することによりモータ４４を駆動する。

ここで上記トルクリミッタ６３は、通常時においては入力されたモータトルク指令をそのままの値で出力するが、作業制御部３３からトルク制限信号が入力されている間はあらかじめ設定された上限値（または下限値）でモータトルク指令の値を制限して出力する。また上記ＰＷＭ制御部６４は、通常時においてはモータトルク指令に基づくＰＷＭ制御によりモータ駆動電力を給電するが、作業制御部３３からサーボオフ信号が入力されている間はモータ駆動電力の給電自体を停止してモータ４４の動作を自由とする（以下、サーボオフという）。

＜１−５．本実施形態の特徴＞
上記構成のようにパラレルリンクロボット４は、一般的に複数のリンク機構部４３を並列に連結した閉リンク機構の機構部５０を備え、駆動制御部３１が各リンク機構部４３をそれぞれ個別に駆動する複数のモータ（駆動軸）を連携して制御することで、上記の機構部５０の全体を任意の動作に制御できる。

このように複数のモータの複合的な連携制御によって閉リンク機構を駆動する構成の場合、機構部５０の配置や姿勢によってはその状態を安定させるために複数のモータが互いに異なるトルクを各リンク機構に付加して相殺させる場合があり、それらトルクの間の関係は複雑となる。このため、機構部５０における衝突や脱臼などの動作異常の発生を単純にいずれか１つのモータの挙動や状態データだけで判定することは困難であった。

これに対し本実施形態では、パラレルリンクロボット４の機構部５０における衝突および脱臼の少なくともいずれか一方を、複数のモータの状態データ（本実施形態の例のモータトルク指令、モータ検出位置、及びモータ検出速度）に基づいて判定する異常判定部３２、を有している。

これにより、パラレルリンクロボット４の機構部５０を駆動する複数のモータ４４ａ〜４４ｃ全ての状態データに基づいて、機構部５０における外部との衝突や、各球面軸受４８，４９における脱臼の発生を高い精度で判定できる。以上の機能を実現するために必要な手法について、以下に順次説明する。

＜１−６．衝突判定の基本的手法について＞
まず、図５に示す１軸駆動機構をモデル例として参照し、本実施形態における衝突判定の基本的手法について説明する。図５に示す１軸駆動機構７０は、床面から垂直に立設したボールネジ７１を１つのモータ７２で回転させてリンク７３を昇降移動させるよう構成されている。この１軸駆動機構７０において衝突に対処する制御シーケンスとして、図５（ａ）に示すようにリンク７３を降下移動させている途中で、図５（ｂ）に示すようにリンク７３に予期しない異物７４が衝突した際にその移動を停止させ、その後に図５（ｃ）に示すようにリンク７３を異物７４から退避させるよう上昇移動させる制御シーケンスを実行させる。図６は、そのような制御シーケンスでの衝突前後における位置指令速度、モータトルク指令、衝突トルクの時間変化を表している。なお衝突トルクは、異物７４に衝突したリンク７３に付加される抗力としての接触外力を外乱トルクとして換算した値に相当する。

図６に示すように、リンク７３の降下移動中において安定していたモータトルク指令は、異物７４と接触して衝突した直後に大きく下がり、それに追随して衝突トルクも大幅に変動する。これは、モータ７２を制御するサーボのフィードバックループにおいて、衝突後に入力される位置指令や速度指令を実現しようとした結果による変動である。本制御シーケンスでは、このようなモータトルク指令と衝突トルクの変動を検知して衝突を判定する。また本制御シーケンスでは特に、その衝突判定後すぐに衝突方向に向けてモータトルク指令を制限（トルク制限）しつつ降下移動を停止することにより、異物７４に対する衝突の影響を抑えることができ、続いてリンク７３を上昇移動させて退避する。なお、この退避の上昇移動中における引き戻し方向に対してはトルク制限する必要はない。

ここで、図５（ｂ）に示すように、リンク７３が異物７４に接触した直後に受ける抗力Ｆ１と異物７４における影響は比較的小さいものの、そのまま下降移動し続けると受ける抗力Ｆ２が増大して異物７４における影響も大きくなる。このため、リンク７３が実際に異物７４に接触してからできるだけ迅速に衝突判定してその下降移動を止めることが望まれる。そのためにはモータトルク指令に対してできるだけ絶対値の小さいしきい値との比較により衝突判定する手法が考えられるが、その場合には通常時におけるリンク７３の昇降移動の安定性を考慮するとそのしきい値の設定が難しくなる。

つまり、リンク７３自体の質量が十分大きい場合や急峻な昇降動作を行わせる場合には、その昇降移動における加速期間と減速期間のそれぞれにおいて大きなモータトルクが必要であり、それよりも低くしきい値を設定することができない。また、例えばワークを移送する複数軸駆動の多関節ロボットなどに適用する場合では、ワークの把持による質量の増減やリンク部分の配置や姿勢によっては、必要となるモータトルクが複雑に変化する。特に本実施形態のパラレルリンクロボット４のような閉リンク機構の動作を制御する場合には、各リンク機構部４３の配置や姿勢によってはその状態を安定させるために複数のモータ４４が互いに異なるトルクを各リンク機構部４３に付加して相殺させる場合があり、それらトルクの間の関係は複雑となる。以上から、モータ駆動機構における衝突の判定を、単純にモータトルク指令と、固定的に設定された１つのしきい値との比較だけで行うことは困難である。

これに対して本実施形態の異常判定部３２は、衝突時に異物７４から機構部５０に作用する抗力としての接触外力Ｆ自体を推定し、その接触外力Ｆと十分低く設定したしきい値との比較によって迅速かつ高い精度での衝突判定を行う。その接触外力Ｆの推定は、複数のモータ４４のモータ状態データ（本実施形態の例のモータトルク指令、モータ検出位置、及びモータ検出速度）に基づく演算処理によって行う。

＜１−７．接触外力の推定演算処理について＞
以下、パラレルリンクロボット４の機構部５０に作用する接触外力Ｆを推定するための演算処理内容について詳しく説明する。本実施形態の例では、図７に示すパラレルリンクロボット４の機構モデルで定義した諸元パラメータに基づいてラグランジュの運動方程式を導出し、この運動方程式から機構部５０に作用した接触外力Ｆを算出する。

まず、エンドエフェクタの運動エネルギーは、次式で記述できる。

同様に駆動リンク４６と受動リンク４７の運動エネルギーは、次式で記述できる。

また、エンドエフェクタのポテンシャルエネルギーは、次式で記述できる。

同様に駆動リンク４６と受動リンク４７のポテンシャルエネルギーは、次式で記述できる。

なお、上記（式１）〜（式６）において、図７中に記載のない諸元パラメータについては以下の通りである。

ここで、

である。ただし、

であり、δｘ、δｙは受動リンク４７のＸ軸、Ｙ軸まわりの回転角度である。また、

である。

そして、上記（式１）〜（式６）より、次式のようなラグランジュ関数を得ることができる。

ここで、一般化座標を

とおき、機構部５０に付加される一般化力を

とおくと、運動方程式は次式を演算することによって導出できる。

（ただし、ｊ＝１〜６）

上記（式８）より、ＸＹＺ軸方向のベクトルで表される接触外力Ｆ（Ｆｘ，Ｆｙ，Ｆｚ）と各モータ４４のシャフトトルク（Ｔｒｑ_１，Ｔｒｑ_２，Ｔｒｑ_３）による運動方程式を得ることができる。なお、機構部５０が異物に接触していない通常状態では接触外力Ｆが０であるため、Ｆｘ＝Ｆｙ＝Ｆｚ＝０とする。また各シャフトトルク（Ｔｒｑ_１，Ｔｒｑ_２，Ｔｒｑ_３）は、各モータ４４の減速機１５のシャフトＳＨが出力して駆動リンク４６を揺動駆動するトルクであり、上記減速機１５の減速比と減速効率の積をηとした場合のモータトルク指令×ηに相当する値である。そして本実施形態の例では、上記（式８）の運動方程式を変形して得られる微分方程式に、各モータ４４のモータ状態データ（モータトルク指令、モータ検出位置、モータ検出速度）とともにエンドエフェクタの位置（Ｐｘ，Ｐｙ，Ｐｚ）、速度、加速度を代入して接触外力Ｆを求めることができる。なお、上記のエンドエフェクタの位置、速度、加速度については、異常判定部３２が図７の機構モデル等で示した機械諸元と、各サーボ３５から入力されたモータ状態データに基づいて順キネマティック演算により算出すればよい。

＜１−８．衝突に対する各軸方向別の対処制御内容について＞
一般的に多くのパラレルリンクロボット４に実行させるピックアンドプレースの動作は、その制御上、水平方向（ＸＹ軸方向）での移動動作と、鉛直方向（Ｚ軸方向）での移動動作とに区別され、必要に応じてそれらを組み合わせて行わせる場合が多い。そして上記プックアンドプレース動作が行われる作業環境では、機構部５０がワーク以外の異物７４等に対し水平方向で衝突する場合と、鉛直方向で衝突する場合ではそれぞれ必要とされる対処制御の内容が異なる場合がある。このため本実施形態では、機構部５０に衝突が生じた際に水平方向（ＸＹ軸方向）と鉛直方向（Ｚ軸方向）でそれぞれ異なる対処動作を行わせるように駆動制御部３１の作業制御部３３が複数のモータ４４を連携制御する。

＜１−８−１．ＸＹ軸方向での衝突に対する対処制御内容＞
図８（ａ）は、パラレルリンクロボット４の可動部４２が異物７４に対して主にＸＹ軸方向（水平方向）で衝突した状態を側面から示しており、図８（ｂ）は、そのようなＸＹ軸方向の衝突に対応して作業制御部３３が可動部４２を退避移動するよう制御した状態を示している。上述したように、異常判定部３２は、運動方程式から得られた所定の算出式に各モータ４４のモータ状態データ（モータトルク指令、モータ検出位置、モータ検出速度）及びエンドエフェクタの位置、速度、加速度を代入することで、その時点で可動部４２に付加されている接触外力Ｆを逐次算出し、異常判定情報として作業制御部３３へ出力することができる。この接触外力Ｆは、ロボット座標系のＸ軸方向、Ｙ軸方向、Ｚ軸方向のそれぞれの分力Ｆｘ，Ｆｙ，Ｆｚで算出される。

そこで、接触外力ＦのＸ軸方向とＹ軸方向の分力Ｆｘ，Ｆｙを合成した合成分力Ｆｘｙの絶対値を、Ｆｘｙ＝（Ｆｘ^２＋Ｆｙ^２）^１／２で求めることができる。また、各分力Ｆｘ，Ｆｙそれぞれの符号に基づいて、合成分力Ｆｘｙの方向も求めることができる。そして異常判定部３２は、算出した合成分力Ｆｘｙの絶対値が所定のしきい値を超えた場合に、ＸＹ軸方向での衝突が発生したものと判定する。また、接触外力ＦのＺ軸方向の分力Ｆｚの絶対値が別途のしきい値を超えた場合には、単純にＺ軸方向での衝突が発生したものと判定できる。以上の２つの衝突判定においては、それぞれしきい値と比較する対象が単純に機構部５０に付加される接触外力であるため、当該しきい値を十分小さく設定しても確度と感度の高い衝突判定が可能となる。

図８（ａ）に示す例では、算出された接触外力Ｆの分力Ｆｚが対応するしきい値と比較して十分に小さく、その一方で合成分力Ｆｘｙが対応するしきい値と比較して十分に大きいことから、異常判定部３２は機構部５０（可動部４２）にＸＹ軸方向での衝突だけが発生したと判定する。一般的なパラレルリンクロボット４の作業現場では、このように機構部５０に対して水平方向に異物７４が衝突した場合、当該機構部５０をその衝突点Ｐから水平方向で衝突方向に離間するよう移動させるだけで、機構部５０及び異物７４の両方でその衝突による影響を比較的軽くできる場合が多い。このため本実施形態では、異常判定部３２がＸＹ軸方向の衝突が発生したとする判定結果と合成分力Ｆｘｙを異常判定情報として出力した場合、それを受け取った作業制御部３３は、図８（ｂ）に示すように、可動部４２を合成分力Ｆｘｙの方向であらかじめ設定されている所定の離間距離Ｄｅだけ水平に離間した位置（所定位置）へ退避移動するよう作業指令を出力する。

なお、特に図示しないが、上記の合成分力Ｆｘｙではなく、Ｘ軸方向の分力ＦｘとＹ軸方向の分力Ｆｙのそれぞれで個別にしきい値と比較して軸方向別に衝突を判定し、各衝突判定に対応して対処してもよい。

＜１−８−２．Ｚ軸方向での衝突に対する対処制御内容＞
また図９（ａ）に示す例のように、算出された接触外力Ｆの合成分力Ｆｘｙが対応するしきい値と比較して十分に小さく、その一方で分力Ｆｚが対応するしきい値と比較して十分に大きい場合、機構部５０（可動部４２）にはＺ軸方向での衝突だけが発生したと判定される。一般的なパラレルリンクロボット４の作業現場では、機構部５０に対して鉛直方向での衝突が生じた場合、機構部５０が例えば図示しないベルトコンベアなどの床面との間で異物７４を挟み込み下方へ押圧してしまう場合がある。このため本実施形態では、異常判定部３２がＺ軸方向の衝突が発生したとする判定結果と分力Ｆｚを異常判定情報として出力した場合に、それを受け取った作業制御部３３は、図９（ｂ）に示すように、可動部４２をあらかじめ設定されている所定の離間距離Ｕｅだけ分力Ｆｚと同じ方向（上方、下方）に離間した位置（所定位置）へ退避移動するよう作業指令を出力する。またこのとき、作業制御部３３は、全てのサーボ３５にトルク制限信号を出力して各モータ４４の出力トルクを制限することで、機構部５０と異物７４の両方の影響を迅速かつ確実に軽減できる。

なお、機構部５０と異物７４の接触方向によっては、上記のＸＹ軸方向とＺ軸方向の両方で衝突が発生したと判定される場合もあり、その場合には両方の対処制御（退避移動）を同時に実行してもよい（特に図示せず）。

＜１−９．脱臼に対する対処制御内容について＞
例えば機構部５０が急激に異物７４と衝突した場合には、図１０（ａ）に示すように、当該機構部５０が備えるいずれかの球面軸受４８，４９からボールジョイントが外れる脱臼状態となる場合がある。このとき機構部５０に対する各モータ４４の駆動力が大きく変化することから、異常判定部３２が算出する接触外力Ｆも大きく変化する。本実施形態の例では、異常判定部３２が衝突の場合と脱臼の場合の間の接触外力Ｆ（またはその時間変化など）の違いを判別することで、衝突判定と脱臼判定を区別して行うことができる。

このような脱臼状態が発生した場合には、機構部５０の各リンク機構部４３に不要な加振力や拘束力を付与せず、図１０（ｂ）に示すように、機構部５０全体をそのまま自重での自由動作により下方へ降下させることが望ましい。このため本実施形態では、異常判定部３２が脱臼判定を異常判定情報として出力した場合に、それを受け取った作業制御部３３は、全てのサーボ３５にサーボオフ信号を出力して各モータ４４への駆動電力の給電を停止する。

＜１−１０．制御フロー＞
以上説明した異常判定部３２と作業制御部３３の機能を実現するために、ロボット制御装置３のＣＰＵ９０１においてソフトウェア的に実行される異常判定処理と作業制御処理の制御手順を図１１、図１２のフローチャートを参照しつつ説明する。図１１は、異常判定部３２をソフトウェア的に実装した場合のフローチャートを示しており、パラレルリンクロボット４の制御を開始した際にその実行を開始する。

まず、ステップＳ１０５において、ロボット制御装置３のＣＰＵ９０１は、各サーボ３５で生成されたモータトルク指令と、検出されたモータ検出位置及びモータ検出速度をモータ状態データとして取得する。

次にステップＳ１１０へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１０５で取得したモータ状態データに基づく順キネマティック演算により、その時点のエンドエフェクタ（図中では「ＥＥ」と略記）の位置、速度、加速度を算出する。

次にステップＳ１１５へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１０５で取得したモータ状態データと、上記ステップＳ１１０で算出したエンドエフェクタの位置、速度、加速度に基づいて接触外力Ｆ（Ｆｘ，Ｆｙ，Ｆｚ）を算出する。本実施形態におけるこの算出手法は、上記（式８）の運動方程式を用いた演算処理により行えばよい。

次にステップＳ１２０へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１１５で算出した接触外力ＦのＸＹ軸方向の分力Ｆｘ，Ｆｙに基づいて合成分力Ｆｘｙを算出する。

次にステップＳ１２５へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１２０で算出した合成分力Ｆｘｙが対応するしきい値以上にあるか否か、言い換えるとＸＹ軸方向での衝突が発生したか否かを判定する。合成分力Ｆｘｙがしきい値以上である場合、判定が満たされ（Ｓ１２５：ＹＥＳ）、ステップＳ１３０へ移る。

ステップＳ１３０では、ロボット制御装置３のＣＰＵ９０１は、ＸＹ軸方向で衝突が発生したとする判定結果と合成分力Ｆｘｙを異常判定情報として作業制御部３３に出力する。そしてステップＳ１３５へ移る。

一方、上記ステップＳ１２５の判定において、合成分力Ｆｘｙがしきい値未満である場合、判定は満たされず（Ｓ１２５：ＮＯ）、ステップＳ１３５へ移る。

ステップＳ１３５では、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１１５で算出した接触外力ＦのＺ軸方向の分力Ｆｚが対応するしきい値以上にあるか否か、言い換えるとＺ軸方向での衝突が発生したか否かを判定する。分力Ｆｚがしきい値以上である場合、判定が満たされ（Ｓ１３５：ＹＥＳ）、ステップＳ１４０へ移る。

ステップＳ１４０では、ロボット制御装置３のＣＰＵ９０１は、Ｚ軸方向で衝突が発生したとする判定結果と分力Ｆｚを異常判定情報として作業制御部３３に出力する。そしてステップＳ１４５へ移る。

一方、上記ステップＳ１３５の判定において、分力Ｆｚがしきい値未満である場合、判定は満たされず（Ｓ１３５：ＮＯ）、ステップＳ１４５へ移る。

ステップＳ１４５では、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ１１５で算出した接触外力Ｆ（またはその時間変化など）に基づいて脱臼が発生したか否かを判定する。脱臼が発生した場合、判定が満たされ（Ｓ１４５：ＹＥＳ）、ステップＳ１５０へ移る。

ステップＳ１５０では、ロボット制御装置３のＣＰＵ９０１は、脱臼が発生したとする判定結果を異常判定情報として作業制御部３３に出力する。そしてステップＳ１０５へ戻り、同様の手順を繰り返す。

一方、上記ステップＳ１４５の判定において、脱臼が発生していない場合、判定は満たされず（Ｓ１４５：ＮＯ）、ステップＳ１０５へ戻り、同様の手順を繰り返す。

以上のステップを繰り返すことで、異常判定部３２は、パラレルリンクロボット４の機構部５０に衝突又は脱臼が発生した際にその判定結果と関係する情報を異常判定情報として作業制御部３３に逐次出力する。

次に、図１２のフローチャートを参照して作業制御処理の制御手順について説明する。この作業制御処理は、パラレルリンクロボット４の制御を開始した際に上記図１１の異常判定処理と並行して実行される。

まず、ステップＳ２０５において、ロボット制御装置３のＣＰＵ９０１は、上位制御装置２から入力された作業指令を取得する。

次にステップＳ２１０へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ２０５で取得した作業指令に対応する制御シーケンスに従って、次のエンドエフェクタの移動先位置を座標位置指令として算出し、モーション制御部３４へ出力する。

次にステップＳ２２０へ移り、ロボット制御装置３のＣＰＵ９０１は、異常判定部３２から入力された異常判定情報を取得する。

次にステップＳ２２５へ移り、ロボット制御装置３のＣＰＵ９０１は、上記ステップＳ２２０で取得した異常判定情報に何らかの異常判定結果が含まれているか否か、言い換えると衝突または脱臼が発生したとする判定結果が含まれているか否かを判定する。異常判定結果が含まれていない場合、判定は満たされず（Ｓ２２５：ＮＯ）、ステップＳ２３０へ移る。

ステップＳ２３０では、ロボット制御装置３のＣＰＵ９０１は、その時点で実行している作業シーケンスが終了しているか否かを判定する。作業シーケンスが終了しておらず実行中である場合、判定は満たされず（Ｓ２３０：ＮＯ）、ステップＳ２１０へ戻って同様の手順を繰り返す。

一方、作業シーケンスが終了していた場合、判定が満たされ（Ｓ２３０：ＹＥＳ）、ステップＳ２０５へ戻って同様の手順を繰り返す。

また一方、上記ステップＳ２２５の判定において、異常判定情報に異常判定結果が含まれている場合、判定が満たされ（Ｓ２２５：ＹＥＳ）、ステップＳ２３５へ移る。なお、このときに座標位置指令を一旦固定して各モータ４４の出力位置を停止させるようにしてもよい。

ステップＳ２３５では、ロボット制御装置３のＣＰＵ９０１は、ＸＹ軸方向の衝突の発生が判定されたか否か、言い換えると異常判定情報にＸＹ軸方向の衝突が発生している旨の判定結果が含まれているか否かを判定する。ＸＹ軸方向の衝突の発生が判定されている場合、判定が満たされ（Ｓ２３４：ＹＥＳ）、ステップＳ２４０へ移る。

ステップＳ２４０では、ロボット制御装置３のＣＰＵ９０１は、異常判定情報に含まれている合成分力Ｆｘｙに基づいて可動部４２を退避移動させる移動先位置を算出し、それを座標位置指令としてモーション制御部３４へ出力する。そして、ステップＳ２４５へ移る。

一方、上記ステップＳ２３５の判定において、ＸＹ軸方向の衝突の発生が判定されていない場合、判定は満たされず（Ｓ２３５：ＮＯ）、ステップＳ２４５へ移る。

ステップＳ２４５では、ロボット制御装置３のＣＰＵ９０１は、Ｚ軸方向の衝突の発生が判定されたか否か、言い換えると異常判定情報にＺ軸方向の衝突が発生している旨の判定結果が含まれているか否かを判定する。Ｚ軸方向の衝突の発生が判定されている場合、判定が満たされ（Ｓ２４５：ＹＥＳ）、ステップＳ２５０へ移る。

ステップＳ２５０では、ロボット制御装置３のＣＰＵ９０１は、各サーボ３５にトルク制限信号を出力するとともに、異常判定情報に含まれている分力Ｆｚに基づいて可動部４２を上昇退避移動させる移動先位置を算出し、それを座標位置指令としてモーション制御部３４へ出力する。そしてステップＳ２５５へ移る。

一方、上記ステップＳ２４５の判定において、Ｚ軸方向の衝突の発生が判定されていない場合、判定は満たされず（Ｓ２４５：ＮＯ）、ステップＳ２５５へ移る。

ステップＳ２５５では、ロボット制御装置３のＣＰＵ９０１は、脱臼の発生が判定されたか否か、言い換えると異常判定情報に脱臼が発生している旨の判定結果が含まれているか否かを判定する。脱臼の発生が判定されている場合、判定が満たされ（Ｓ２５５：ＹＥＳ）、ステップＳ２６０へ移る。

ステップＳ２６０では、ロボット制御装置３のＣＰＵ９０１は、各サーボ３５にサーボオフ信号を出力する。そしてステップＳ２６５へ移る。

一方、上記ステップＳ２５５の判定において、脱臼の発生が判定されていない場合、判定は満たされず（Ｓ２５５：ＮＯ）、ステップＳ２６５へ移る。

ステップＳ２６５では、ロボット制御装置３のＣＰＵ９０１は、パラレルリンクロボット４の機構部５０に衝突又は脱臼の異常が発生した旨を特に図示しない表示部などを介してユーザに報知する。そして、このフローを終了する。

＜１−１１．本実施形態による効果＞
以上説明したように、本実施形態のロボット制御装置３は、パラレルリンクロボット４における複数のモータ４４を制御する駆動制御部３１と、パラレルリンクロボット４の機構部５０における衝突および脱臼の少なくともいずれか一方を、複数のモータ４４の状態データ（モータトルク指令、モータ検出位置、モータ検出速度）に基づいて判定する異常判定部３２と、を有している。これにより、パラレルリンクロボット４の機構部５０を駆動する複数のモータ４４の全ての状態データに基づいて、機構部５０における外部との衝突や、各球面軸受４８，４９における脱臼の発生を高い精度で判定できる。

なお、本実施形態の例のロボット制御システム１は、複数の回転型モータ４４ａ〜４４ｃを連携駆動することにより制御するパラレルリンクロボット４を使用しているが、複数の直動型（リニアタイプ）のモータを連携駆動して制御するパラレルリンクロボット（特に図示せず）を使用する場合にも適用可能である。この場合には、上記エンコーダ部１４は可動子の移動位置（移動速度）を検出可能なリニアスケールに置き換えられ、また上記の減速機１５は不要となる。また、上記（式８）の運動方程式の導出においては、直動型モータを用いたパラレルリンクロボットの機構モデルに基づいて導出すればよい。

また、本実施形態では特に、異常判定部３２が衝突を判定した場合、駆動制御部３１は、Ｚ軸方向が鉛直方向に対応したＸＹＺ軸直交座標系の動作空間におけるＸＹ軸方向での衝突とＺ軸方向での衝突とで機構部５０の動作が異なるように複数のモータ４４を制御する。これにより、水平方向（ＸＹ軸方向）での移動動作と、鉛直方向（Ｚ軸方向）での移動動作とを組み合わせて実行されるパラレルリンクロボット４のピックアンドプレースの動作において、衝突が生じた場合の機能的な対処が可能となる。

また、本実施形態では特に、異常判定部３２がＸＹ軸方向での衝突を判定した場合、駆動制御部３１は、機構部５０が備えるエンドエフェクタを衝突点ＰからＸＹ軸方向で所定距離に離間した相対位置に移動させるように、複数のモータ４４を制御する。例えば機構部５０に対して水平方向に衝突が生じても、当該機構部５０をその衝突方向から水平方向に離間するよう移動させるだけで、機構部５０及び異物７４の両方でその衝突による影響を比較的軽くできる場合が多い。このように水平方向の衝突に対しては、所定位置への移動だけを行うように駆動制御部３１が複数のモータ４４を連携制御することで迅速かつ確実に適切な対処が可能となる。なお、衝突判定時の移動先位置は、上述したような衝突点Ｐからの相対位置に限られず、ロボット座標系中にあらかじめ設定しているいわゆるホームポジションなどの絶対位置へ移動させてもよい。

また、ＸＹ軸方向での衝突に対しては、ＸＹ軸方向におけるエンドエフェクタの移動をすぐに停止させてもよい。すなわち、異常判定部３２がＸＹ軸方向での衝突を判定した場合、駆動制御部３１は、機構部５０が備えるエンドエフェクタの移動を停止させるように、複数のモータ４４を制御してもよい。例えば機構部５０に対して水平方向に衝突が生じた場合、機構部５０及び異物７４の両方でその衝突による影響を増大させないために、最低限、ＸＹ軸方向の移動を停止させる必要がある。このように水平方向の衝突に対して、ＸＹ軸方向の移動を停止させるように駆動制御部３１が複数のモータ４４を連携制御することで、所定位置へ移動させるよりも当該駆動制御部３１における演算処理の負担を省略でき、より迅速かつ確実に適切な対処が可能となる。なお、このときの移動停止制御については、作業制御部３３が同じ座標位置指令を出力し続けてエンドエフェクタの位置をクランプさせてもよい。もしくは、作業制御部３３がサーボオフ信号を出力して各モータ４４へのモータ駆動電力の給電を停止するサーボオフを行うとともに、ブレーキ信号を出力して各モータ４４の位置を制動させてもよい。

また、本実施形態では特に、異常判定部３２がＺ軸方向での衝突を判定した場合、駆動制御部３１は、複数のモータ４４における出力トルクを制限し、かつ、機構部５０が備えるエンドエフェクタを衝突点ＰからＺ軸方向で所定位置に離間した相対位置に移動させるように、複数のモータ４４を制御する。例えば機構部５０に対して鉛直方向での衝突が生じた場合には、機構部５０が例えばベルトコンベアなどの床面との間で異物７４を挟み込み下方へ押圧してしまう場合がある。これに対し、機構部５０をその衝突方向から鉛直方向（上昇方向、下降方向）に移動させるだけでなく、各モータ４４の出力トルクを制限することで、機構部５０と異物７４の両方の影響を迅速かつ確実に軽減できる。なお、衝突判定時の移動先位置は、上述したような衝突点Ｐからの相対位置に限られず、ロボット座標系中にあらかじめ設定しているいわゆるホームポジションなどの絶対位置へ移動させてもよい。

また、本実施形態では特に、異常判定部３２が脱臼を判定した場合、駆動制御部３１は、複数のモータ４４の制御を停止する。これにより、例えば球面軸受４８，４９に脱臼が発生して機構部５０が分解した場合でも、作業制御部３３がサーボオフ信号を出力して各モータ４４へのモータ駆動電力の給電を停止するサーボオフにより制御を停止することで、各リンク機構に不要な加振力や拘束力を付与せずに自由動作を許容させることができるため、周囲への干渉を軽減させることができる。

＜１−１２．変形例＞
なお、以上説明した実施形態は、その趣旨及び技術的思想を逸脱しない範囲内で種々の変形が可能である。

上記実施形態では、異常判定部３２が各モータ４４の状態データ（モータトルク指令、モータ検出位置、モータ検出速度）に基づいて機構部５０に付与されたと推定される接触外力Ｆを算出し、その接触外力Ｆから直接的に機構部５０の衝突と脱臼の発生を判定していたが、これに限られない。他にも、算出した接触外力Ｆからさらに各モータ４４の外乱トルクを推定し、それら外乱トルクに基づいて衝突と脱臼を判定してもよい。この場合、上記の外乱トルクは、接触外力Ｆに対する抗力分として各モータ４４のモータトルク指令に含まれる潜在トルク値に相当する。この外乱トルクを推定するためには、上記図７に示した機構モデルを考慮して各モータ４４ごとに適宜設計された外乱オブザーバにより、接触外力Ｆを各モータ４４の推定外乱に変換すればよい。この外乱オブザーバについては、上記（式８）における一般化力Ｑの接触外力ＦとシャフトトルクＴｒｑの関係に基づいて設計すればよい（特に図示せず）。

このように推定された各モータ４４の外乱トルクは、単純に機構部５０に付与された接触外力Ｆに相当するトルク値であるため、十分低い値で設定されたしきい値と比較することで確度と感度の高い衝突判定が可能となる。また、この外乱トルクに基づいて行うことで、精度の高い脱臼判定も可能となる。また、上記の接触外力Ｆ及び外乱トルクをより高い精度で推定するためには、モータ４４のエンコーダ部１４がモータ４４の回転子ではなく、減速機１５の出力軸であるシャフトＳＨの回転位置をモータ検出位置として検出し、対応するモータトルク指令、モータ検出速度を参照してエンコーダの位置、速度、加速度、接触外力Ｆ、及び外乱トルクを推定してもよい。

＜２．第２実施形態＞
上記第１実施形態では、衝突や脱臼の判定に用いる接触外力Ｆを各モータ４４の状態データに基づく運動方程式から算出していたが、これに限られない。他にも、機構部５０に設けたセンサの検出値とモータ状態データとの比較に基づいて接触外力Ｆを求めてもよく、そのような構成の第２実施形態について以下に説明する。なお、上記第１実施形態と同等の構成や処理については、図示及び説明を省略する。

上記図１に対応する図１３は、本実施形態におけるロボット制御システム１Ａの概略構成を示している。この図１３において、パラレルリンクロボット４は可動部４２の上面に３次元加速度センサ１６を備えている。この３次元加速度センサ１６は、ロボット座標系のＸ軸方向、Ｙ軸方向、Ｚ軸方向のそれぞれに対応した可動部４２の加速度を座標検出加速度として検出し、ロボット制御装置３へ出力している。上記図３に対応する図１４は、本実施形態におけるロボット制御装置３の内部構成とその周辺で送受される各種情報を表している。この図１４において、上記３次元加速度センサ１６が検出した座標検出加速度は、ロボット制御装置３の異常判定部３２に入力される。

以上の構成のロボット制御システム１Ａにおいて、異常判定部３２は、各サーボ３５から入力されるモータ状態データに基づいてエンドエフェクタの加速度（及び位置、速度、）を算出し、それら算出値と３次元加速度センサ１６から入力される座標検出加速度とに基づいて機構部５０に作用する接触外力Ｆを推定できる。例えば、ＸＹＺ軸の各軸方向のそれぞれで、エンドエフェクタの算出加速度と座標検出加速度との間の差分が接触外力Ｆの分力Ｆｘ，Ｆｙ，Ｆｚに対して比例関係（Ｆ＝ａ・ｍ）にあると考えられる。この検討に基づいて、上記図７に示した機構モデルの各諸元パラメータを考慮し、ＸＹＺ軸の各軸方向別に接触外力Ｆの分力Ｆｘ，Ｆｙ，Ｆｚを求めることができる。また、このようにして求めた接触外力Ｆからさらに各モータ４４の外乱トルクも推定できる。

これによりこの第２実施形態では、運動方程式を利用した上記第１実施形態と比較して、機構部５０に作用した各軸方向の接触外力Ｆをより負担の少ない演算処理によって簡易に算出できる。さらに、上記（式８）に基づいて設計した外乱オブザーバで各サーボ３５から入力されるモータ状態データを参照して外乱トルクも算出できる。

＜３．ロボット制御装置のハードウェア構成例＞
次に、図１５を参照しつつ、上記で説明したＣＰＵ９０１が実行するプログラムによりソフトウェア的に実装された駆動制御部３１（作業制御部３３、モーション制御部３４、サーボ３５）、異常判定部３２等による処理を実現するロボット制御装置３のハードウェア構成例について説明する。

図１５に示すように、ロボット制御装置３は、例えば、ＣＰＵ９０１と、ＲＯＭ９０３と、ＲＡＭ９０５と、ＡＳＩＣ又はＦＰＧＡ等の特定の用途向けに構築された専用集積回路９０７と、入力装置９１３と、出力装置９１５と、記録装置９１７と、ドライブ９１９と、接続ポート９２１と、通信装置９２３とを有する。これらの構成は、バス９０９や入出力インターフェース９１１を介し相互に信号を伝達可能に接続されている。

プログラムは、例えば、ＲＯＭ９０３やＲＡＭ９０５、記録装置９１７等に記録しておくことができる。

また、プログラムは、例えば、フレキシブルディスクなどの磁気ディスク、各種のＣＤ・ＭＯディスク・ＤＶＤ等の光ディスク、半導体メモリ等のリムーバブルな記録媒体９２５に、一時的又は永続的に記録しておくこともできる。このような記録媒体９２５は、いわゆるパッケージソフトウエアとして提供することもできる。この場合、これらの記録媒体９２５に記録されたプログラムは、ドライブ９１９により読み出されて、入出力インターフェース９１１やバス９０９等を介し上記記録装置９１７に記録されてもよい。

また、プログラムは、例えば、ダウンロードサイト・他のコンピュータ・他の記録装置等（図示せず）に記録しておくこともできる。この場合、プログラムは、ＬＡＮやインターネット等のネットワークＮＷを介し転送され、通信装置９２３がこのプログラムを受信する。そして、通信装置９２３が受信したプログラムは、入出力インターフェース９１１やバス９０９等を介し上記記録装置９１７に記録されてもよい。

また、プログラムは、例えば、適宜の外部接続機器９２７に記録しておくこともできる。この場合、プログラムは、適宜の接続ポート９２１を介し転送され、入出力インターフェース９１１やバス９０９等を介し上記記録装置９１７に記録されてもよい。

そして、ＣＰＵ９０１が、上記記録装置９１７に記録されたプログラムに従い各種の処理を実行することにより、上記の駆動制御部３１（作業制御部３３、モーション制御部３４、サーボ３５）、異常判定部３２等による処理が実現される。この際、ＣＰＵ９０１は、例えば、上記記録装置９１７からプログラムを直接読み出して実行してもよいし、ＲＡＭ９０５に一旦ロードした上で実行してもよい。更にＣＰＵ９０１は、例えば、プログラムを通信装置９２３やドライブ９１９、接続ポート９２１を介し受信する場合、受信したプログラムを記録装置９１７に記録せずに直接実行してもよい。

また、ＣＰＵ９０１は、必要に応じて、例えばマウス・キーボード・マイク（図示せず）等の入力装置９１３から入力する信号や情報に基づいて各種の処理を行ってもよい。

そして、ＣＰＵ９０１は、上記の処理を実行した結果を、例えば表示装置や音声出力装置等の出力装置９１５から出力してもよく、さらにＣＰＵ９０１は、必要に応じてこの処理結果を通信装置９２３や接続ポート９２１を介し送信してもよく、上記記録装置９１７や記録媒体９２５に記録させてもよい。

なお、以上の説明において、「垂直」「平行」「平面」等の記載がある場合には、当該記載は厳密な意味ではない。すなわち、それら「垂直」「平行」「平面」とは、設計上、製造上の公差、誤差が許容され、「実質的に垂直」「実質的に平行」「実質的に平面」という意味である。

また、以上の説明において、外観上の寸法や大きさ、形状、位置等が「同一」「同じ」「等しい」「異なる」等の記載がある場合は、当該記載は厳密な意味ではない。すなわち、それら「同一」「等しい」「異なる」とは、設計上、製造上の公差、誤差が許容され、「実質的に同一」「実質的に同じ」「実質的に等しい」「実質的に異なる」という意味である。

また、以上既に述べた以外にも、上記実施形態や各変形例による手法を適宜組み合わせて利用しても良い。その他、一々例示はしないが、上記実施形態や各変形例は、その趣旨を逸脱しない範囲内において、種々の変更が加えられて実施されるものである。

１，１Ａ ロボット制御システム
２ 上位制御装置
３ ロボット制御装置
４ パラレルリンクロボット
１２ モータ本体部
１３ ブレーキ部
１４ エンコーダ部
１５ 減速機
１６ ３次元加速度センサ
３１ 駆動制御部
３２ 異常判定部
３３ 作業制御部
３４ モーション制御部
３５ サーボ
４１ 基礎部
４２ 可動部
４３ リンク機構部
４４ モータ（駆動軸）
４５ 取付部材
４６ 駆動リンク
４７ 受動リンク
４８，４９ 球面軸受
５０ 機構部
ＳＨ シャフト

Claims (8)

1. パラレルリンクロボットを制御するロボット制御装置であって、
   前記パラレルリンクロボットにおける複数の駆動軸を制御する駆動制御部と、
   前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記複数の駆動軸の状態データに基づいて判定する異常判定部と、
   を有することを特徴とするロボット制御装置。
2. 前記異常判定部が前記衝突を判定した場合、
   前記駆動制御部は、Ｚ軸方向が鉛直方向に対応したＸＹＺ軸直交座標系の動作空間におけるＸＹ軸方向での衝突とＺ軸方向での衝突とで前記機構部の動作が異なるように前記複数の駆動軸を制御する
   ことを特徴とする請求項１記載のロボット制御装置。
3. 前記異常判定部が前記ＸＹ軸方向での衝突を判定した場合、
   前記駆動制御部は、前記機構部が備えるエンドエフェクタを所定位置に移動させるように、前記複数の駆動軸を制御する
   ことを特徴とする請求項２記載のロボット制御装置。
4. 前記異常判定部が前記ＸＹ軸方向での衝突を判定した場合
   前記駆動制御部は、前記機構部が備えるエンドエフェクタの移動を停止させるように、前記複数の駆動軸を制御する
   ことを特徴とする請求項２記載のロボット制御装置。
5. 前記異常判定部が前記Ｚ軸方向での衝突を判定した場合、
   前記駆動制御部は、前記複数の駆動軸における出力トルクを制限し、かつ、前記機構部が備えるエンドエフェクタを所定位置に移動させるように、前記駆動軸を制御する
   ことを特徴とする請求項２乃至４のいずれか１項に記載のロボット制御装置。
6. 前記異常判定部が前記脱臼を判定した場合、
   前記駆動制御部は、前記複数の駆動軸の制御を停止する
   ことを特徴とする請求項１乃至５のいずれか１項に記載のロボット制御装置。
7. パラレルリンクロボットと、前記パラレルリンクロボットを制御するロボット制御装置とを有するロボット制御システムであって、
   前記ロボット制御装置は、
   前記パラレルリンクロボットにおける複数の駆動軸を制御する駆動制御部と、
   前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記複数の駆動軸の状態データに基づいて判定する異常判定部と、
   を有することを特徴とするロボット制御システム。
8. パラレルリンクロボットを制御するロボット制御装置が備える演算装置が実行するロボット制御方法であって、
   前記パラレルリンクロボットの機構部における衝突および脱臼の少なくともいずれか一方を、前記パラレルリンクロボットにおける複数の駆動軸の状態データに基づいて判定し、
   前記衝突を判定した場合、前記パラレルリンクロボットにおける複数の駆動軸に対して所定の制御を実行し、
   前記脱臼を判定した場合、前記複数の駆動軸の制御を停止する
   ことを特徴とするロボット制御方法。

Images