JP5370128B2 - ロボットの故障検出装置 - Google Patents

Description

本発明は、ロボットアームの撮影画像による所要のアームの現在位置とモータの回転角による所要のアームの現在位置との比較によって故障を検出するロボットの故障検出装置に関する。

ロボットが使用される生産設備では、人とロボットとの接触を防止するために、ロボットを防護柵によって囲むことが一般に行われている。しかしながら、近年、設備の小型化、設備の変更の容易化などのために、防護柵を撤去することが要請されている。

この要請に対処するためには、例えば、人の現在位置とロボットの現在位置を検出し、人とロボットとの離間距離が所定距離以下になったとき、ロボットを緊急停止させてロボットと人とが接触することを回避するシステム構成を採用し、安全を図ることが好ましい。この場合において、ロボットの位置制御が正常に行われないと、ロボットが動作プログラムから外れた予期しない動作をして人と接触する恐れが生じてくるため、ロボットの現在位置検出を二重化、つまりロボットの現在位置を２通りの方法で検出して位置制御が正常に行われているか否かを監視し、位置制御に異常が生じた場合には、直ちにロボットを停止させてロボットが予期しない動作をすることのないようにする必要がある。

位置検出の二重化のために、特許文献１には、宇宙活動においてさまざまな作業を実行させるための遠隔操作型宇宙用ロボットを対象に、そのロボット先端に３次元マーカを取り付け、このマーカを２台のカメラで撮影することによってロボット先端の位置を検出し、検出した位置を予め与えられた監視データから取得される位置と比較して不一致の場合には異常発生と判断してロボットを停止させることが開示されている。

また、通常の産業用ロボットにおいて、位置検出を二重化するために、特許文献２では、アブソリュートエンコーダなどの絶対位置検出手段による検出値と、インクリメンタルエンコーダなどの相対位置検出手段による検出値とを比較し、その比較結果によってロボットの位置制御の正常異常を判断するようにしている。

特開平７−２３７１６０号公報 特開平４−２３５６１０号公報

特許文献１では、撮影画像の解析についての具体的方法は開示されていないが、３次元マーカを２台のカメラで撮影し、その２台のカメラの撮影画像からロボット先端の位置を３次元で検出するには相当高度な計算が要求されるので、極めて高価な制御装置を使用することになる。このため、宇宙開発用ロボットにおいてはまだしも、投資効率を重視する工場などの産業用ロボットへの適用は現実的でない。

また、特許文献２のものでは、アームの駆動源としてのモータに、アブソリュートエンコーダとインクリメンタルエンコーダの２台のエンコーダを組み込む必要があり、モータが大型化し、且つ高価格のものとなってロボットの大型化、高価格化を招く。

本発明は上記の事情に鑑みてなされたもので、その目的は、撮影画像からアームの回転角を求めることによってモータに位置検出手段を２台設けずとも済むようにし、しかも、撮影画像からアームの回転角を求める場合、簡易な画像解析で済むロボットの故障検出装置を提供することにある。

本発明では、所要のアームに付されたマーカを撮像手段により撮影し、マーカの撮影画像の形状、大きさ、または向きからアームの回転角および／または直動位置を推定し、そして、推定したアームの回転角および／または直動位置を、アームを駆動するモータの回転角から演算により求めたアームの回転角および／または直動位置と比較し、或はアームを駆動するモータの回転角を、推定したアームの回転角および／または直動位置から演算により求めたモータの回転角と比較し、その比較結果により故障の有無を判断するので、位置制御が正常に行われているか否かを容易に検出できる。しかも、マーカの撮影画像の形状、大きさ、または向きからアームの回転角および／または直動位置を演算するので、画像解析のために高度な演算をせずとも済み、安価な故障検出装置とすることができる。

この場合において、マーカを、アームの回転方向に沿って幅寸法が異なる三角形または台形とし、アーム位置推定手段は、三角形または台形のマーカのうち撮像手段により撮像された部分の重心を求め、当該重心を通る幅寸法線の長さを求めることによってマーカの付されたアームの回転角および／または直動位置を検出するように構成することができる。

また、マーカは、アームの回転方向に沿って幅寸法が異なる三角形または台形であり、アーム位置推定手段は、三角形または台形のマーカのうち撮像手段により撮像された部分の面積を求め、当該面積によってマーカの付されたアームの回転角および／または直動位置を検出するように構成することができる。

更に、マーカは、半円形または弦月状であり、アーム位置推定手段は、半円形または弦月状のマーカの直線部分の傾き角からアームの回転角を検出する構成とすることもできる。

本発明の第１の実施形態を示すロボットの斜視図 図１において後側から見たロボットの部分的な斜視図 ロボットの制御構成を示すブロック図 故障検出のためのフローチャート 下アームを垂直、第１の上アームを水平にして示すロボットの平面図 下アームおよび第１の上アームを水平にして示すロボットの平面図 ショルダ部のマーカを示し、（ａ）はショルダ部の基準位置での撮影画像図、（ｂ）は基準位置から時計回り方向にある角度回転したときの撮影画像図、（ｃ）は基準位置から反時計回り方向にある角度回転したときの撮影画像図 下アームのマーカを示し、（ａ）は展開図、（ｂ）は下アームが垂直位置から後側へある角度回転した位置での撮影画像図、（ｃ）は下アームが垂直位置から前側へある角度回転した位置での撮影画像図 第１の上アームのマーカを示し、（ａ）は展開図、（ｂ）は第１の上アームが水平位置から図１の後側へある角度回転した位置での撮影画像図、（ｃ）は第１の上アームが図１の前側へある角度回転した位置での撮影画像図 本発明の第２の実施形態を示すロボットの斜視図ロボットの平面図 本発明の第３の実施形態を示すロボットの斜視図 ロボットの平面図

以下、本発明を実施形態により具体的に説明する。
［第１の実施形態］
図１〜図９は本発明の第１の実施形態を示す。図１には、６軸の垂直多関節型ロボット１が示されている。このロボット１のロボットアーム２は、ベース３と、このベース３に第１軸Ｌｃ−１を中心に水平方向に回転可能に支持されたショルダ部４と、このショルダ部４に第２軸Ｌｃ−２を中心に上下方向に回転可能に支持された下アーム５と、この下アーム５に第３軸Ｌｃ−３を中心に上下方向に回転可能に支持された第１の上アーム６と、この第１の上アーム６の先端部に第４軸Ｌｃ−４を中心に捻り回転可能に支持された第２の上アーム７と、この第２の上アーム７に第５軸Ｌｃ−５を中心に上下方向に回転可能に支持された手首８と、この手首８に第６軸Ｌｃ−６を中心に捻り回転可能に支持されたフランジ９とから構成されている。なお、アーム先端であるフランジ９には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。

ベース３、ショルダ部４、下アーム５、第１の上アーム６、第２の上アーム７、手首８、フランジ９は、ロボットアーム２におけるアームとして機能し、固定アームであるベース３を除く各アーム４〜９は、前段のアームに回転可能に支持された回転軸（回転関節：図示せず）に固定連結されている。そして、各アーム４〜９の回転軸は、それぞれ図３に示すサーボモータ１０により減速装置１１を介して回転されるようになっている。なお、各回転軸の回転中心軸線は、上記第１軸Ｌｃ−１〜第６軸Ｌｃ−６に一致している。

ロボットアーム２の動作を制御する制御手段としての制御装置１２は、マイコンからなり、図３に示すように、ＣＰＵ１３、ＲＯＭ１４およびＲＡＭ１５を備えている。この制御装置１２には、サーボモータ１０の駆動回路１６、サーボモータ１０の回転角を検出する回転角検出回路（モータ回転角検出手段）１７が接続されている。この回転角検出回路１７は、サーボモータ１０の回転軸（図示せず）に連結されたロータリエンコーダ（回転センサ）１８から入力される回転検出信号に基づいてサーボモータ１０の回転角を検出し、その回転角情報を制御装置１２と駆動回路１６に与える。なお、図２では、サーボモータ１０は１台しか示されていないが、実際には、各アーム４〜９に１台ずつ設けられているものである。

上記ロータリエンコーダ１８は、アブソリュート型のもので、サーボモータ１０の回転軸の現在位置を示す信号を出力する。従って、回転角検出回路１７により検出される回転角はサーボモータ１０の基準位置を０度としたとき、基準位置からの正方向への回転角（３６０度以上を含む）および逆方向への回転角（３６０度以上を含む）を検出できるようになっている。

ここで、ベース３および各アーム４〜９には、３次元の座標が規定されている。このうち、床面に据え付けられるベース３の座標系（図１に座標軸ＸＹＺで示す。）は、不動の座標系で基準座標（ロボット座標）とされる。ＲＯＭ（記憶手段）１４には、ロボット座標上におけるショルダ部４の座標位置、ショルダ部４の座標上における下アーム５の座標位置、下アーム５の座標上における第１の上アーム６の座標位置、第１の上アーム６の座標上における第２の上アーム７の座標位置、第２の上アーム７の座標上における手首８の座標位置、手首８の座標上におけるフランジ９の座標位置、各アーム４〜９の長さ（座標間の長さ）などの各種のパラメータが記憶されている。

ＣＰＵ１３は、座標変換の計算機能を有し、ロボット先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボット先端が取るような各アーム４〜８の回転角を演算（逆変換）できるようになっており、また、各アーム４〜９の回転角が与えられると、各アーム４〜９の位置を演算（順変換）できるようにもなっている。

そして、ＣＰＵ１３は、ＲＡＭ１５に記憶された動作プログラムに基づいて、ロボット先端の移動軌跡を演算し、ロボットアーム２の動作制御中、サンプリングタイム毎に、各サーボモータ１０の目標回転角を駆動回路１６に出力する。駆動回路１６は、ロボット制御装置１２から与えられた目標回転角と、回転角検出回路１７からフィードバックされた各サーボモータ１０の回転角とを比較し、その差分に応じた電流を各サーボモータ１０に供給する。これによりサーボモータ１０が目標回転角に回転するように制御される。このような制御がサンプリングタイム毎に行われることによって、ロボット先端が動作プログラムに示された移動軌跡上を移動するようになる。

以上のようなロボット１において、ロボットアーム２のアーム４〜９のうちの所要のアーム、この実施形態では、ショルダ部４、下アーム５および第１の上アーム６に、マーカ１９〜２１が付されている。これらマーカ１９〜２１は、それぞれショルダ部４、下アーム５および第１の上アーム６の地色とは異なる色に着色されている。

マーカ１９〜２１が付された部分とマーカ１９〜２１の形状について説明するに、まず、ショルダ部４のマーカ１９は、当該ショルダ部４の下部外周寄りの平坦面４ａに弦月状に描かれている。弦月状とは、円弧の両端を直線で結んだ形状をいい、その弦月状のマーカ１９の直線部１９ａは第２軸Ｌｃ−２、第３軸Ｌｃ−２および第５軸Ｌｃ−５と平行になっている。

下アーム５のマーカ２０は、当該下アーム２０の回転中心側端部である基端部の半円状外周面に、１８０度の角度範囲にわたって付されている。第１の上アーム６のマーカ２１は、第２の上アーム７側を前としたとき第１の上アーム６の後端部の半円状外周面に、同じく１８０度の角度範囲にわたって付されている。これらマーカ２０，２１は、下アーム５、第１の上アーム６の回転方向に沿って幅寸法が異なる例えば三角形、更に言えば、図８（ａ）および図９（ａ）に展開して示すように直角三角形に描かれ、その直角三角形の直交２辺のうちの一方の辺（短辺）が第２軸Ｌｃ−２、Ｌｃ−３に平行で、他方の辺（長辺）が円周方向に延びている。従って、マーカ２０，２１の幅方向（直交２辺のうちの短辺に平行な直線の方向）は、ショルダ部４のマーカ１９の直線部１９ａと平行になっている。

さて、ロボットアーム２の全体を見得る定位置、例えばロボットアーム２の上方、具体的には、ロボット座標の垂直のＺ座標軸の延長上に、図３に示す撮像手段としてのＣＣＤカメラ２２が設置されている。このＣＣＤカメラ２２により、ショルダ部４を撮影すると、当該ショルダ部４に付されたマーカ１９は、ショルダ部４の回転角に応じて向きが変化する。つまり、ショルダ部４が回転すると、図７（ａ），（ｂ）および（ｃ）弦月状のマーカ１９の直線部１９ａの傾きが変化するとともに、直線部１９ａから膨らみ出る円弧曲線部１９ｂの頂点１９ｃの位置が変化する。

ここで、ショルダ部４の回転角度範囲とＣＣＤカメラ２２の撮影画像上に設定されたカメラ座標との関係を述べるに、まず、ショルダ部４の回転角度範囲が０〜θ（例えば３２０度）であるとすると、その中間の回転角θ／２（１６０度）をショルダ部４の基準位置とし、基準位置から例えば時計回り方向にθ／２回転した位置を０度の位置、反時計回り方向にθ／２回転した位置をθ度（３２０度）の位置とする。

そして、カメラ座標は、その原点がショルダ部４の回転軸の回転中心軸線（Ｌｃ−１）に一致するように定められ、ショルダ部４が基準位置にあるとき、カメラ座標のＸｍ座標軸およびＹｍ座標軸のうち、Ｙｍ座標軸がマーカ１９の直線部１９ａと平行で、Ｘｍ座標軸がマーカ１９の直線部１９ａに直交し、且つＸｍ座標軸のプラス側の軸線が円弧曲線部１９ｂの頂点１９ｃを通るようにカメラ座標が設定されている。なお、ショルダ部４の基準位置は回転角度範囲の中間に限られない。

カメラ座標を上記のように設定すると、マーカ１９の直線部１９ａのＹｍ座標軸との交差角度（９０度以下）を検出すると共に、円弧曲線部１９ｂの頂点１９ｃがＸｍ座標軸およびＹｍ座標軸のプラス側に有るかマイナス側に有るかを検出することによってショルダ部４の回転角を検出することができる。具体的には、図７（ｂ）のように、直線部１９ａのＹｍ座標軸との交差角がα（≦９０度）であり、頂点１９ｃがＸｍ座標軸のプラス側でＹｍ座標軸のマイナス側にあれば、このときのショルダ部４の回転角は（θ／２−α）度と決定する。また、図７（ｃ）のように、直線部１９ａのＹｍ座標軸との交差角がβ（≦９０度）であり、頂点１９ｃがＸｍ座標軸およびＹｍ座標軸で共にプラス側にあれば、このときのショルダ部４の回転角は（θ／２＋β）度と決定する。

更に、図示はしないが、頂点１９ｃがＸｍ座標軸のマイナス側でＹｍ座標軸のマイナス側にあって直線部１９ａとＹｍ座標軸とのなす角がδ（≦９０度）である場合、ショルダ部の回転角は（θ／２−９０＋δ）度であり、頂点１９ｃがＸｍ座標軸のマイナス側でＹｍ座標軸のプラス側にあって直線部１９ａとＹｍ座標軸とのなす角がε（≦９０度）である場合、ショルダ部の回転角は（θ／２＋９０−ε）度である。

下アーム５は垂直上向きの状態から図１において時計回り方向（後側）および反時計回り方向（前側）に所定の角度範囲内で回転可能になっている。この下アーム５をＣＣＤカメラ２２によって撮影すると、下アーム５が垂直上向きの状態にあるとき、図５に示すように、下アーム５のマーカ２０は当該下アーム５に隠されて撮影されない。下アーム５が垂直上向きの状態から、図１において時計回り方向に回転してゆくと、図８（ａ）に展開して示すマーカ２０の図示左端部分が図８（ｂ）に示すように台形状に撮影され始め、回転角の増大と共に、台形状のマーカ２０が次第に大きくなってゆく。逆に、下アーム５が垂直上向きの状態から、図１において反時計回り方向に回転してゆくと、図８（ａ）に示すマーカ２０の図示右端部分が図８（ｃ）に示すように三角形に撮影され始め、回転角の増大と共に三角形のマーカ２０が次第に大きくなってゆく。

また、第１の上アーム６は図１において水平前向きの状態から下方へ所定角度だけ回転可能になっている。この第１の上アーム６をＣＣＤカメラ２２によって撮影すると、第１の上アーム６が水平の状態にあるとき、第１の上アーム６のマーカ２１は、図９（ａ）に展開して示すマーカ２１の左端部分が三角形に撮影される。そして、第１の上アーム６が水平の状態から、図１において反時計回り方向（下方向）に回転してゆくと、回転角の増大と共に、三角形のマーカ２０が次第に大きく撮影されてゆく。更に、第１の上アーム６が回転して図１において垂直下向きから更に後側を向くようになると、マーカ２０が図９（ｃ）に示すように台形に撮影され、回転角の増大と共に、台形のマーカ２１が次第に大きく撮影されてゆく。

以上のように下アーム５のマーカ２０および第１の上アーム６のマーカ２１は、共に下アーム５および第１の上アーム６の回転に伴って形状や大きさが変化する。従って、マーカ２０，２１の形状や大きさを計測すれば、下アーム５および第１の上アーム６の回転角を特定することができる。本実施形態では、マーカ２０，２１の撮影画像上の大きさによって回転角を特定するものとし、マーカ２０，２１の大きさを、撮影画像上のマーカ２０，２１の重心Ｇを通る幅方向の直線（幅寸法線）Ｌの長さに置換して把握するものとする。撮影画像の重心Ｇは図形中心として計算により求めることができる。重心Ｇを通る撮影画像の幅方向に延びる幅寸法線Ｌは、ショルダ部４、下アーム５および第１の上アーム６の回転角とは関係なく、マーカ１９の直線部１９ａが常にマーカ２０，２１の幅方向の直線と平行であるから、重心Ｇが求まれば、容易に求めることができる。なお、撮影画像上での長さは、画素数をカウントすることによって容易に求めることができる。

ただ、マーカ２０が付された下アーム５の基端部外周面の高さ位置は下アーム５が回転しても殆ど変わらないが、マーカ２０が付された第１の上アーム６の基端部の高さ位置は、下アーム５の回転角に応じて変化するため、マーカ２１が同じ大きさに撮像されても、高さ位置が分からなければ、マーカ２１の大きさから第１の上アーム６の回転角を特定することはできない。なお、第１の上アーム６の基端部外周面はＬｃ−３と同心ではないので、第１の上アーム６自身の回転角によっても大きさが変化するが、その程度は僅かであるから、本実施形態では、第１の上アーム６自身の回転角についての大きさの変化は考慮しない。

そこで、下アーム５のマーカ２０の撮影画像については、幅寸法線Ｌの長さと下アーム５の回転角を関連付けてＲＯＭ１４にテーブル化して記憶させ、第１の上アーム６のマーカ２１については、第１の上アーム６の基端部の高さ別に、幅寸法線Ｌの長さと第１の上アーム６の回転角を関連付けてＲＯＭ１４にテーブル化して記憶させている。これにより、幅寸法線Ｌの長さを計測すれば、下アーム５、第１の上アーム６の回転角はＲＯＭ１４のテーブルを検索することにより、容易に検出できる。なお、第１の上アーム６の基端部の高さ位置は、下アーム５の先端部の高さ位置と同等であるから、下アーム５の回転角と下アーム５の長さとから計算により求めることができる。

ＲＯＭ１４には、上記マーカ２０について、幅寸法線Ｌの長さと下アーム５の回転角との関係、マーカ２１について、高さ別の幅寸法線Ｌの長さと下アーム５の回転角との関係がテーブル化して記憶されている他、下アーム５の長さ、Ｌｃ−２軸のＬｃ−１軸（ロボット座標の原点）からのＹ座標軸方向へのオフセット長さＦ１および同じくＬｃ−２軸のＬｃ−１軸からのＸ座標軸方向へのオフセット長さＦ２が記憶されている。

次に上記構成において、位置検出を二重化して故障の有無を検出するための制御装置１２の制御内容を図４のフローチャートをも参照しながら説明する。まず、位置検出の二重化のために、ＣＣＤカメラ２２の座標とロボット１の座標とのキャリブレーションを行う（ステップＳ１）。このキャリブレーションのうちには、マーカ１９〜２１の向きや大きさにから検出したショルダ部４、下アーム５および第１の上アーム６の回転角と、ロボット制御上の回転角との整合を取ることも含まれる。例えば、マーカ１９により検出されたショルダ部４の回転角がθ／２度が制御上は０度、θ度が制御上は＋θ／２度、０度が制御上は−θ／２度というように両者の整合が取られる。その後、制御装置１２のＣＰＵ１３は、ＣＣＤカメラ２に撮影指令を出す。この撮影指令により、ＣＣＤカメラ２２は、ロボットアーム２を撮影する（ステップＳ２）。なお、撮影指令は、一定の時間毎に出すことが好ましいが、ロボット１側が必要とするならば、任意のタイミングに出しても良い。

今、下アーム５および第１の上アーム６が共にほぼ水平の状態になった時点でＣＣＤカメラ２２に撮影指令が出されたとすると、ＣＣＤカメラ２２の撮影画像は、図６のようになる。この撮影画像（データ）は、制御装置１２に送られる。そして、ＣＰＵ１３は、この撮影画像から、まず、ショルダ部４のマーカ１９を検索する。このとき、マーカ１９は、カメラ座標の原点を中心にして所定の半径の円を描いたとき、その円の上に存在する（マーカ位置推測手段）ので、マーカ１９は容易に検出することができる。マーカ１９を検出し、形状を認識する（直線部１９ａ、円弧曲線部１９ｂの頂点１９ｃの認識：形状認識手段）と、ＣＰＵ１３は、当該マーカ１９の直線部１９ａがＹｍ座標軸となす角度（≦９０度）と、頂点１９ｃの位置とから、ショルダ部４の回転角を演算する（ステップＳ４：アーム位置推定手段）。

続いて、ＣＰＵ１３は、ショルダ部４の回転角に基づいて下アーム５のマーカ２０の位置を推測する（ステップＳ５：マーカ位置推測手段）。この場合、ＲＯＭ１４には、マーカ１９の直線部１９ａに直交し第１軸Ｌｃ−１を通る直線Ｃを仮定したとき、Ｌｃ−１から第２軸Ｌｃ−２までの直線Ｃに沿った方向のオフセット量Ｆ１と、直線Ｃからマーカ２０の第２軸に沿った方向のオフセット量Ｆ２とが記憶されているので、そのオフセット量Ｆ１，Ｆ２からマーカ２０の概略位置を推測することがきる。そして、推測した位置を基にしてマーカ２０を容易に検出することができる。マーカ２０の形状を認識（ステップＳ６：形状認識手段）したら、ＣＰＵ１３は、当該マーカ２０の撮影画像の重心Ｇの位置を求め、その重心Ｇを通るマーカ２０の幅寸法線Ｌの長さを求める（大きさ演算手段）。そして、この幅寸法線Ｌの長さからＲＯＭ１４のテーブルを参照して下アーム５の回転角を求める（ステップＳ７：アーム位置推定手段）。

ここで、前述したように、下アーム５が垂直上向きの状態にあるとき、下アーム５のマーカ２０は当該下アーム５に隠されて撮影されない。このような場合には、ＣＣＤカメラ２２の位置を変えたり、マーカ２０の位置を変えたりすることで対処したり、或は、ＣＣＤカメラ２２によってマーカ２０が撮影されない場合、下アーム５は垂直上向きにあると推定するようにしたりするようにプログラムすることで対処したりすることが考えられる。

その後、ＣＰＵ１３は、第１の上アーム６のマーカ２１の位置を推測する（ステップＳ８）。このマーカ２１は、上記直線Ｃ上であって第１の上アーム６の軸Ｌｃ−３の近辺に存在するので、下アーム５の回転角と当該下アーム５の長さとで軸Ｌｃ−２から軸Ｌｃ−３までの水平距離を求めれば、マーカ２１の位置を推測することができる（マーカ位置推測手段）。なお、マーカ２１は第１の上アーム６の回転中心である第３軸Ｌｃ−３の近辺にあるから、ショルダ部４、下アーム５の回転角を基にした順変換の演算によって第１の上アーム６の座標の位置（ロボット座標上の位置）を求めることで、マーカ２１の位置を推測する構成としても良い。

ＣＰＵ１３は、マーカ２１を検出すると、その検出したマーカ２１の形状を認識し（ステップＳ９：形状認識手段）、マーカ２１の撮影画像の重心Ｇを通る幅寸法線Ｌの長さを求める（大きさ演算手段）。更に、ＣＰＵ１３は、下アーム５の回転角と当該下アーム５の長さから第１の上アーム６の基端部の高さ位置を求め、ＲＯＭ１４に記憶されている高さ毎の幅寸法線Ｌの長さと回転角とのテーブルデータを参照し、第１の上アーム６の回転角を求める（ステップＳ１０：アーム位置推定手段）。

以上のようにしてショルダ部４、下アーム５および第１の上アーム６の回転角を求めると、次に、ＣＰＵ１３は、ショルダ部４、下アーム５および第１の上アーム６を駆動するサーボモータ１０の回転角を回転角検出回路１７から取得し（ステップＳ１１）、このサーボモータ１０の回転角を減速装置１１の減速比で除してショルダ部４、下アーム５および第１の上アーム６の回転角を演算する（アーム回転角演算手段）。この場合、逆に、以上のようにして求めたアーム４〜９の回転角に各減速装置１１の減速比を乗じて各サーボモータ１０の回転角を演算により求めても良い（モータ回転角演算手段）。

ところで、マーカ１９〜２１の撮影画像から求めたショルダ部４、下アーム５および第１の上アーム６の回転角は、ロボット座標を基準にした回転角（ロボット座標基準の絶対的回転角）であるのに対し、サーボモータ１０の回転角から求めたショルダ部４の回転角はロボット座標、下アーム５の回転角はショルダ部４の座標、第１の上アーム６の回転角は下アーム５の座標をそれぞれ基準にした回転角（アーム相互間の相対的回転角）であるから、基準を同じにするために、角度変換を行う（ステップＳ１２：回転角変換手段）。

例えば、ロボット座標基準の絶対的回転角をアーム相互間の相対的回転角に変換するために、マーカ２１の撮影画像を基にして求めた第１の上アーム６の回転角からマーカ２０の撮影画像を基にして求めた下アーム５の回転角を差し引いて、下アーム５の座標に対する第１の上アーム６の回転角を求め、マーカ２０の撮影画像を基にして求めた下アーム５の回転角はそのままをショルダ部４の座標に対する回転角とし、マーカ１９から求めた回転角ショルダ部４の回転角も同様にそのままをロボット座標に対する回転角とする。

逆に、アーム相互間の相対的回転角をロボット座標基準の絶対的回転角に変換するために、サーボモータ１０から求めたショルダ部４の回転角はそのままをロボット座標基準の絶対的回転角、サーボモータ１０から求めた下アーム５の回転角もそのままをロボット座標基準の絶対的回転角、サーボモータ１０から求めた第１の上アーム６の回転角は下アーム５の回転角と合計してロボット座標基準の絶対的回転角としても良い。

続いて、ＣＰＵ１３は、マーカ１９〜２１から求めたショルダ部４、下アーム５および第１の上アーム６の回転角と、サーボモータ１０の回転角から求めたショルダ部４、下アーム５および第１の上アーム６の回転角とを比較、具体的には両回転角の差を求める（ステップＳ１４：比較手段）。この場合、基準を同じにした回転角どうしで比較することは勿論である。そして、ＣＰＵ１３は、比較したショルダ部４、下アーム５および第１の上アーム６の各々についての回転角の差が所定範囲内であるか否かを判断する（ステップＳ１４：判断手段）。

回転角差が所定範囲内であった場合、ＣＰＵ１３は、ロータリエンコーダ１８、回転角検出回路１７は正常に動作して位置検出を正常に行っているとし（ステップＳ１４で「ＹＥＳ」）、ステップＳ２に戻って上記した動作を繰り返す。回転角差が所定範囲を超えている場合、ＣＰＵ１３は、位置検出が正常に行われていないとし（ステップＳ１４で「ＮＯ」）、ロボットアーム２を緊急停止させる（ステップＳ１５：停止制御手段）。なお、緊急停止は、サーボモータ１０に最大の逆トルクを発生させて急速にロボットアーム２を停止させることが考えられるが、緊急停止手段としては、これに限られず、機械的ブレーキを強く掛けることであっても良い。

このように本実施形態によれば、位置検出が正常に行われていない場合、ロボットアーム２を緊急停止させるので、位置検出に異常が起きたことにより、ロボットアーム２が予期しない動作を行うといった事態の発生を未然に防止することができる。

また、位置検出のためにマーカ１９〜２１の撮影画像からショルダ部４、下アーム５および第１の上アーム６の回転角を検出する場合、ＣＰＵ１３はマーカ１９〜２１の画像解析を行うが、この画像解析はマーカ１９〜２１の形状の検出、重心Ｇの検出、幅寸法線Ｌの長さ検出といった程度の比較的簡単なものであるから、それ程高度な演算をせずとも済む。従って、制御装置１２としては、比較的安価なものを使用することができる。

［第２の実施形態］
図１０は本発明の第２の実施形態を示すものである。この実施形態では、第１の実施形態と同様のＣＣＤカメラ２２をロボットアーム２の横方向に設置している。横方向の定位置からロボットアーム２を撮影してアームの回転角を取得できるようにするために、ショルダ部４の円周側面に三角形のマーカ２３、下アーム５の基端部側面に弦月状のマーカ２４、第１の上アーム６の基端部側面に弦月状のマーカ２５、第２の上アーム７の外周面に沿って三角形または台形のマーカ２６を付している。

上記各マーカ２３〜２６の向き、大きさによってショルダ部４、下アーム５、第１の上アーム６および第２の上アーム７の回転角を取得する構成は第１の実施形態において説明した通りである。

［第３の実施形態］
図１１および図１２は本発明の第３の実施形態を示す。この実施形態は、水平多関節型のロボット２７に適用したものであり、そのロボットアーム２８は、ベース２９、ベース２９に第１軸Ｌｃ−１を中心に水平方向に回転可能に支持された第１の水平旋回アーム３０、第１の水平旋回アーム３０に第１軸Ｌｃ−２を中心に水平方向に回転可能に支持された第２の水平旋回アーム３１、第２の水平旋回アーム３１に第３軸Ｌｃ−３に沿って上下に直動可能で且つ回転可能に支持された手首軸３２からなる。なお、エンドエフェクタは手首軸３２の下端に取り付けられるようになっている。

ベース２９、第１の水平旋回アーム３０、第２の水平旋回アーム３１、手首軸３２および手首軸３２は、ロボットアーム２８におけるアームとして機能し、第１の水平旋回アーム３０および第２の水平旋回アーム３１は、それぞれ前段のアームであるベース２９および第１の水平旋回アーム３０に支持された回転軸（回転関節：図示せず）に固定連結されている。そして、その各回転軸はサーボモータにより減速装置を介して回転されるようになっている。

図示はしないが、第２の水平旋回アーム３１の先端部には、回転関節機構と直動機構とが設けられ、手首軸３２は、それら回転関節および直動機構により回転可能に且つ上下に直線的に移動可能になっている。そして、手首軸３２は、サーボモータによって減速装置を介して回転され、また、他のサーボモータによって減速装置を介して上下に直線移動される。

ＣＣＤカメラとロボットの制御装置は図示されていないが、ここでは、第１の実施形態におけると同じ符号を用いて説明することとする。ＣＣＤカメラ２２は、第１の水平旋回アーム３０の回転中心軸線（Ｌｃ−１）の延長上に固定設置され、ロボットアーム２８を上方から撮影する。上方からロボットアーム２を撮影した画像から第１の水平旋回アーム３０および第２の水平旋回アーム３１の回転角を取得できるようにするために、第１の水平旋回アーム３０の基端部上面および第２の水平旋回アーム３１の基端部上面に、それぞれ弦月状のマーカ３３および３４が付されている。これらマーカ３３および３４は、第１の水平旋回アーム３０および第２の水平旋回アーム３１の回転によって直線部３３ａおよび３４ａの傾斜角度と円弧曲線部３３ｂおよび３４ｂの突出する向きと変化する。

また、撮影画像から手首軸３２の回転角および上下位置を取得できるようにするために、手首軸３２の上端部に弦月状のマーカ３５が付されている。このマーカ３５は、手首軸３２の回転によって直線部３５ａの傾斜角度と円弧曲線部３５ｂの突出する向きと変化し、手首軸３２の上下動によって撮影画像の大きさが変化する。そして、マーカ３５の大きさ、例えば面積と高さ位置との関係がテーブル化して制御装置１２のＲＯＭ１４に記憶されている。

ＣＣＤカメラ２２により図１２に示す画像が撮影されたとする。すると、制御装置１２のＣＰＵ１３は、カメラ座標の原点近くにある第１の水平旋回アーム３０のマーカ３３を認識し、その直線部３３ａのカメラ座標のＹｍ座標軸に対する傾斜角を第１の実施形態と同様に求め、そして、Ｙｍ座標軸に対する傾斜角と円弧曲線部３３ｂの頂点３３ｃの位置とから第１の実施形態と同様にして第１の水平旋回アーム３０の回転角αを求める。

次に、求めた第１の水平旋回アーム３０の回転角と第１の水平旋回アーム３０のアーム長さ（ＲＯＭ１４に記憶）とから第２軸Ｌｃ−２の位置を求め、その周辺に存在するマーカ３４を探索し、第２の水平旋回アーム３１のマーカ３４を認識する。そして、ＣＰＵ１３は、上記したと同様にして第２の水平旋回アーム３１の回転角βを求める。この場合、カメラ座標を第１軸Ｌｃ−１から第２軸Ｌｃ−２まで並行移動させた状態でマーカ３４の回転角を求めるものである。

第２の水平旋回アーム３１の回転角を求めると、ＣＰＵ１３は、求めた第１の水平旋回アーム３０の回転角とアーム長さ、および第２の水平旋回アーム３１の回転角とアーム長さから第３軸Ｌｃ−３の位置を求め、その周辺に存する手首軸３２のマーカ３５を検索し、当該マーカ３５を認識する。そして、ＣＰＵ１３は、前に述べたと同様にして手首軸３５の回転角γを求める。この場合も、カメラ座標を第１軸Ｌｃ−１から第３軸Ｌｃ−３まで並行移動させた状態でマーカ３５の回転角を求めるものである。また、マーカ３５については、ＣＰＵ１３は、当該マーカ３５の撮影画像上での面積を演算する。そして、ＲＯＭ１４に記憶されている面積と高さ位置とのテーブルデータから手首軸３２の高さ位置を求める。

以上のようにして求めた第１の水平旋回アーム３０、第２の水平旋回アーム３１および手首軸３２の回転角α、βおよびγはカメラ座標（ベース２９）に対するものであるから、（β−α）によって第１の水平旋回アーム３０に対する第２の水平旋回アーム３１の回転角を求め、（γ−β）によって第２の水平旋回アーム３１に対する手首軸３２の回転角を求める。そして、第１の水平旋回アーム３０をαだけ回転させるためのサーボモータ１０の回転角、第２の水平旋回アーム３１を（β−α）だけ回転させるためのサーボモータ１０の回転角、手首軸３２を（γ−β）だけ回転させるためのサーボモータ１０の回転角、手首軸３２の高さ位置ｈに移動させるためのサーボモータ１０の回転角を演算する。

その後、ＣＰＵ１３は、回転角検出回路１７から各サーボモータ１０の回転角を取得し、これと撮影画像を基にして求めた各サーボモータ１０の回転角とを比較し、両者の差が許容範囲内にあれば、位置検出に故障はないとしてそのまま動作を続行させ、両者の差が許容範囲を超えていた場合には、位置検出に故障が発生したとして緊急停止する。

［その他の実施形態］
第１の実施形態において、各マーカ１９〜２１の大きさは、面積で求めるようにしても良い。特に、アームの回転角によって形状の変化がなく大きさだけが変化するような場合には、マーカの大きさを面積によって計測し、面積と回転角との関連を示すテーブルからアームの回転角を求めることが可能である。
マーカを付すアームは、第１の実施形態においてはショルダ部４、下アーム５、第１の上アーム６以外のアームであっても良い。
三角形のマーカや台形に限られない。
ロータリエンコーダ１８として、アブソリュート型のものを用いたが、インクリメント型のものであっても良い。インクリメント型のものは、３６０度以上の回転位置を検出できないが、回転角検出回路１７に回転数カウンタを設け、サーボモータ１０が１回転する毎に回転数カウンタを１ずつ加算或は減算してゆくことでサーボモータ１０の回転位置を検出することができる。
撮影されたマーカの大きさ（例えば幅寸法線の長さ、面積）からアームの回転角を求める方法としては、撮影されたマーカの大きさとアームの回転角とをテーブル化して記憶手段に記憶させておき、実際に測定したマーカの幅寸法線の長さや面積から、テーブルを参照してアームの回転角を検索する構成に限られず、計算式による演算により求める構成であっても良い。

第１の実施形態では、撮影画像を基にしたショルダ部４、下アーム５、第１の上アーム６の回転角と、各サーボモータ１０を基にしたショルダ部４、下アーム５、第１の上アーム６の回転角とを比較したが、撮影画像を基にしたショルダ部４、下アーム５、第１の上アーム６の回転角を各サーボモータ１０の回転角に変換し、回転角検出回路１７から求めたショルダ部４、下アーム５、第１の上アーム６の各サーボモータ１０の回転角とを比較しても良い。

第３の実施形態では、撮影画像から求めた第１の水平旋回アーム３０、第２の水平旋回アーム３１、手首軸３２の回転角および高さ位置から求めた各サーボモータ１０と回転角検出回路１７から求めた第１の水平旋回アーム３０、第２の水平旋回アーム３１、手首軸３２の各サーボモータ１０の回転角とを比較したが、回転角検出回路１７から求めた各サーボモータ１０の回転角を第１の水平旋回アーム３０、第２の水平旋回アーム３１、手首軸３２の各サーボモータ１０の回転角に変換し、撮影画像から求めた第１の水平旋回アーム３０、第２の水平旋回アーム３１、手首軸３２の回転角および高さ位置と比較するようにしても良い。

図面中、１はロボット、２はロボットアーム、４はショルダ部（所要のアーム）、５は下アーム（所要のアーム）、６は第１の上アーム（所要のアーム）、１０はサーボモータ、１１は減速装置、１２は制御装置、１３はＣＰＵ（アーム位置推定手段、演算手段、判断手段、回転角変換手段、停止制御手段）、１７は回転角検出回路（モータ回転角検出手段）、１８はロータリエンコーダ、１９〜２１はマーカ、２２はＣＣＤカメラ（撮像手段）、２３〜２６はマーカ、２７はロボット、２８はロボットアーム、３０は第１の水平旋回アーム（所要のアーム）、３１は第２の水平旋回アーム（所要のアーム）、３２は手首軸（所要のアーム）、３３〜３５はマーカを示す。

Claims (6)

1. 回転関節および／または直動関節により複数のアームを連結して構成されたロボットアームの故障を検出する装置において、
   前記複数のアームのうち所要のアームに付されたマーカであってアームの回転および／または直動によって定位置から見たときの形状、大きさ、または向きが変化するように形と付された位置が設定されたマーカと、
   前記所要のアームに付された前記マーカを前記定位置から撮影する撮像手段と、
   前記撮像手段により撮像した前記マーカの形状、大きさ、または向きから前記所要のアームの回転角または直動位置を推定するアーム位置推定手段と、
   前記所要のアームを駆動するモータの回転角を検出するモータ回転角検出手段と、
   前記モータ回転角検出手段が検出した前記モータの回転角から前記所要のアームの回転角および／または直動位置を演算し、または、前記アーム位置推定手段が推定した前記所要のアームの回転角または直動位置から前記モータの回転角を演算する回転角変換手段と、
   前記回転角変換手段が演算した前記所要のアームの回転角および／または直動位置と前記アーム位置推定手段により推定された前記所要のアームの回転角および／または直動位置とを比較し、或は前記回転角変換手段が演算した前記モータの回転角と前記モータ回転角検出手段が検出した前記モータの回転角とを比較し、その比較結果により故障の有無を判断する判断手段と、
   前記判断手段が故障有りと判断したとき前記ロボットを停止させる制御を行う停止制御手段と、を備え、
   前記マーカは、前記アームの回転方向に沿って幅寸法が異なる三角形または台形であり、前記アーム位置推定手段は、前記三角形または台形のマーカのうち前記撮像手段により撮像された部分画像の重心を求め、当該重心を通る前記部分画像の幅寸法線の長さによって前記所要のアームの回転角または直動位置を推定することを特徴とするロボットの故障検出装置。
2. 前記マーカは、前記アームの回転方向に沿って幅寸法が異なる三角形または台形であり、前記アーム位置推定手段は、前記三角形または台形のマーカのうち前記撮像手段により撮像された部分画像の面積を求め、当該面積によって前記所要のアームの回転角または直動位置を推定することを特徴とする請求項１記載のロボットの故障検出装置。
3. 前記マーカは、半円形または弦月状であり、前記アーム位置推定手段は、前記半円形または弦月状のマーカの直線部分の傾きによって前記所要のアームの回転角または直動位置を推定することを特徴とする請求項１または２記載のロボットの故障検出装置。
4. 回転関節および／または直動関節により複数のアームを連結して構成されたロボットアームの故障を検出する装置において、
   前記複数のアームのうち所要のアームに付されたマーカであってアームの回転および／または直動によって定位置から見たときの形状、大きさ、または向きが変化するように形と付された位置が設定されたマーカと、
   前記所要のアームに付された前記マーカを前記定位置から撮影する撮像手段と、
   前記撮像手段により撮像した前記マーカの形状、大きさ、または向きから前記所要のアームの回転角または直動位置を推定するアーム位置推定手段と、
   前記所要のアームを駆動するモータの回転角を検出するモータ回転角検出手段と、
   前記モータ回転角検出手段が検出した前記モータの回転角から前記所要のアームの回転角および／または直動位置を演算し、または、前記アーム位置推定手段が推定した前記所要のアームの回転角または直動位置から前記モータの回転角を演算する回転角変換手段と、
   前記回転角変換手段が演算した前記所要のアームの回転角および／または直動位置と前記アーム位置推定手段により推定された前記所要のアームの回転角および／または直動位置とを比較し、或は前記回転角変換手段が演算した前記モータの回転角と前記モータ回転角検出手段が検出した前記モータの回転角とを比較し、その比較結果により故障の有無を判断する判断手段と、
   前記判断手段が故障有りと判断したとき前記ロボットを停止させる制御を行う停止制御手段と、を備え、
   前記マーカは、前記アームの回転方向に沿って幅寸法が異なる三角形または台形であり、前記アーム位置推定手段は、前記三角形または台形のマーカのうち前記撮像手段により撮像された部分画像の面積を求め、当該面積によって前記所要のアームの回転角または直動位置を推定することを特徴とするロボットの故障検出装置。
5. 前記マーカは、半円形または弦月状であり、前記アーム位置推定手段は、前記半円形または弦月状のマーカの直線部分の傾きによって前記所要のアームの回転角または直動位置を推定することを特徴とする請求項４記載のロボットの故障検出装置。
6. 回転関節および／または直動関節により複数のアームを連結して構成されたロボットアームの故障を検出する装置において、
   前記複数のアームのうち所要のアームに付されたマーカであってアームの回転および／または直動によって定位置から見たときの形状、大きさ、または向きが変化するように形と付された位置が設定されたマーカと、
   前記所要のアームに付された前記マーカを前記定位置から撮影する撮像手段と、
   前記撮像手段により撮像した前記マーカの形状、大きさ、または向きから前記所要のアームの回転角または直動位置を推定するアーム位置推定手段と、
   前記所要のアームを駆動するモータの回転角を検出するモータ回転角検出手段と、
   前記モータ回転角検出手段が検出した前記モータの回転角から前記所要のアームの回転角および／または直動位置を演算し、または、前記アーム位置推定手段が推定した前記所要のアームの回転角または直動位置から前記モータの回転角を演算する回転角変換手段と、
   前記回転角変換手段が演算した前記所要のアームの回転角および／または直動位置と前記アーム位置推定手段により推定された前記所要のアームの回転角および／または直動位置とを比較し、或は前記回転角変換手段が演算した前記モータの回転角と前記モータ回転角検出手段が検出した前記モータの回転角とを比較し、その比較結果により故障の有無を判断する判断手段と、
   前記判断手段が故障有りと判断したとき前記ロボットを停止させる制御を行う停止制御手段と、を備え、
   前記マーカは、半円形または弦月状であり、前記アーム位置推定手段は、前記半円形または弦月状のマーカの直線部分の傾きによって前記所要のアームの回転角または直動位置を推定することを特徴とするロボットの故障検出装置。

Images