JP6805812B2 - マニプレータの動作判定装置 - Google Patents

Description

本発明は、マニプレータの動作可否を判定する動作判定装置に関する。

従来、ロボットのアーム（マニプレータ）の先端を移動させる位置である動作点を教示（設定）して、教示した動作点をアームの先端が通るように、アームを動作させるものがある（特許文献１参照）。

特開２０１６−１４４８６１号公報

ところで、動作点の教示を行った時のアームの温度と、ロボットにより作業を行う時のアームの温度とが異なることがある。その場合、教示した通りにアームを動作させることができない場合があることに、本願発明者は着目した。

本発明は、こうした課題を解決するためになされたものであり、所定温度において、設定した通りにマニプレータを動作させることが可能か否か判定することのできるマニプレータの動作判定装置を提供することにある。

上記課題を解決するための第１の手段は、
第１リンクと、第２リンクと、前記第１リンク及び前記第２リンクを相対回転可能に接続する関節と、を備えるマニプレータの動作可否を判定する動作判定装置であって、
所定温度における前記第１リンク及び前記第２リンクの長さ、前記第１リンク及び前記第２リンクの熱膨張による長さ変化を補正する補正係数、及び前記関節の角度を設定する時の設定温度と前記所定温度との温度差に基づいて、前記所定温度において前記関節を完全に伸ばした状態の前記マニプレータの所定点の位置である第１位置に、前記設定温度において前記所定点を移動させる時の前記関節の角度である限界角度を算出する限界角度算出部と、
前記設定された前記関節の角度である設定角度を検出する設定角度検出部と、
前記限界角度算出部により算出された前記限界角度と、前記設定角度検出部により検出された前記設定角度とに基づいて、前記所定温度において前記第１位置に前記所定点を移動させることが可能であるか否か判定する判定部と、
を備える。

上記構成によれば、マニプレータにおいて、第１リンク及び第２リンクが関節により相対回転可能に接続されている。ここで、第１リンクと第２リンクとが相対回転することにより、リンクの温度が上昇する。リンクは温度の上昇に伴って熱膨張するため、リンクの長さは温度に応じて変化する。このため、例えば高温時に関節を完全に伸ばした状態でマニプレータの所定点の位置（動作点）を設定（教示）すると、低温時にリンクの長さが足りず、設定した位置に所定点を移動させることができない。

この点、限界角度算出部により、所定温度における第１リンク及び第２リンクの長さ、第１リンク及び第２リンクの熱膨張による長さ変化を補正する補正係数、及び関節の角度を設定する時の設定温度と所定温度との温度差に基づいて、所定温度において関節を完全に伸ばした状態のマニプレータの所定点の位置である第１位置に、設定温度において所定点を移動させる時の関節の角度である限界角度が算出される。この限界角度は、設定温度において関節を完全に伸ばした状態に近い状態で関節の角度を設定する場合に、これ以上関節の角度を小さく設定すると、所定温度（＜設定温度）において関節を完全に伸ばしても設定温度での所定点の位置まで所定点を移動させることができなくなる角度である。

そこで、設定角度検出部により、設定された関節の角度である設定角度が検出される。したがって、限界角度算出部により算出された限界角度と、設定角度検出部により検出された設定角度とに基づいて、所定温度において第１位置に所定点を移動させることが可能であるか否か判定することができる。その結果、所定温度において、設定した通りにマニプレータを動作させることが可能か否か判定することができる。なお、所定温度（作業時の温度）が設定温度（教示時の温度）よりも高い場合は、設定温度において関節を完全に伸ばした状態のマニプレータの所定点の位置に、所定温度において所定点を問題なく移動させることができる。

ここで、第１位置が関節を完全に伸ばした状態のマニプレータの所定点の位置でない場合、マニプレータの姿勢を含めた演算が必要となり、その結果動作不可能と判定されると、改めて動作点を設定し直して同様の演算を繰り返し行うことになる。第1位置によっては、上記のような演算を極めて多くこなすことになる場合があり、その場合最終的には動作可能という回答が得られるにしても、それを得るためにかなりの演算時間を必要として非効率である。それに対して、上記構成のように関節を完全に伸ばした状態を対象にして動作可否の判定を行えば、1回の演算により最終的な回答に到達できるので、極めて演算時間を短縮できる。例えばロボット（マニプレータ）において、一番手間が掛かる（時間や費用の効率が悪い）のは、導入初期の教示やキャリブレーションといった必ず人手を介さないとできない段階である。本願は元々、そのタイミングで行われるものであるので、上記構成により演算時間を短縮できれば、手間の改善につながり効率的なロボット導入も可能になることから、ロボットにとっては極めて意味のあるものである。

具体的には、第２の手段のように、前記判定部は、前記限界角度算出部により算出された前記限界角度よりも、前記設定角度検出部により検出された前記設定角度が小さい場合に、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定するといった構成を採用することができる。

第３の手段では、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であることを通知する不可通知部を備える。

上記構成によれば、所定温度において第１位置に所定点を移動させることが不可能であると判定された場合に、所定温度において第１位置に所定点を移動させることが不可能であることが通知される。このため、ユーザは、現在の動作点（すなわち関節の角度）の設定では、所定温度において第１位置に所定点を移動させることが不可能となることを知ることができる。その結果、関節の角度の設定終了後に動作点の設定をやり直す必要が生じることや、マニプレータの納入後に動作不能になることを抑制することができる。

第４の手段では、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記第１位置に前記所定点を移動させることが不可能となる温度を通知する温度通知部を備える。

上記構成によれば、所定温度において第１位置に所定点を移動させることが不可能であると判定された場合に、第１位置に所定点を移動させることが不可能となる温度が通知される。このため、ユーザは、マニプレータを使用する環境の温度やマニプレータの使用状態を考慮して、第１位置に所定点を移動させることが可能であるか否か判断することができる。

第５の手段では、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記関節の角度の設定時に前記関節の角度を修正すべき量を通知する角度通知部を備える。

上記構成によれば、所定温度において第１位置に所定点を移動させることが不可能であると判定された場合に、関節の角度の設定時に関節の角度を修正すべき量が通知される。このため、ユーザは、所定温度において第１位置に所定点を移動させることを可能とするために、関節の角度の設定時に関節の角度を修正する量を容易に知ることができる。

第６の手段では、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記関節の角度の設定時に前記所定点の位置を修正すべき量を通知する位置通知部を備える。

上記構成によれば、所定温度において第１位置に所定点を移動させることが不可能であると判定された場合に、関節の角度の設定時に所定点の位置を修正すべき量が通知される。このため、ユーザは、所定温度において第１位置に所定点を移動させることを可能とするために、関節の角度の設定時に所定点の位置を修正する量を容易に知ることができる。

また、具体的には第７の手段のように、前記判定部は、前記限界角度算出部により算出された前記限界角度よりも、前記設定角度検出部により検出された前記設定角度が大きい場合に、前記所定温度において前記第１位置に前記所定点を移動させることが可能であると判定するといった構成を採用することができる。

余弦定理、ｃｏｓ関数のテイラー展開を用い、所定温度において関節を完全に伸ばした状態を考慮すると、限界角度θ１を以下の式で近似することができる。すなわち、第８の手段のように、前記限界角度算出部は、所定温度における前記第１リンク及び前記第２リンクの長さをそれぞれａ０，ｂ０とし、前記第１リンク及び前記第２リンクの熱膨張による長さ変化を補正する補正係数をそれぞれＭ１，Ｍ２とし、前記温度差をΔとし、前記限界角度をθ１とすると、θ１＝√｛２（ａ０＋ｂ０）×（Ｍ１＋Ｍ２）×Δ／（ａ０×ｂ０）｝により、前記限界角度θ１を算出するといった構成を採用することができる。こうした構成によれば、簡易な式により限界角度θ１を算出することができ、演算負荷を軽減することができる。

アーム及びコントローラを示す模式図。 電源ＯＮ直後の伸び状態のアームを示す模式図。 暖機動作後の伸び状態のアームを示す模式図。 設定温度における限界角度と所定温度での動作可否を示す模式図。

以下、垂直多関節ロボットの第２軸及び第３軸を構成する複数のリンク及び関節に具現化した一実施形態について、図面を参照して説明する。

図１に示すように、アーム１０（マニプレータに相当）は、リンクＬ１，Ｌ２、及び関節Ｊ１，Ｊ２を備えている。リンクＬ１（第１リンクに相当）は、関節Ｊ１により回転可能に支持されている。関節Ｊ２は、リンクＬ１とリンクＬ２（第２リンクに相当）とを相対回転可能に接続している。関節Ｊ１，Ｊ２は、前段のリンクに固定された軸受けと、この軸受けを介して回転可能に支持された回転軸とを備えている。この回転軸は、次段のリンクに接続されており、モータ及び減速機等を備える駆動装置により回転させられる。リンクＬ１において、関節Ｊ２と反対側の端部の中心（１点）は、リンクＬ１，Ｌ２の動作の基点Ｂに設定されている。リンクＬ２において、関節Ｊ２と反対側の端部の中心は、アーム１０の位置を表す所定点Ｃに設定されている。

関節Ｊ１，Ｊ２には、それぞれエンコーダ１１，１２が設けられている。エンコーダ１１，１２は、それぞれの関節において回転軸の回転角度を検出する。また、エンコーダ１１，１２は、それぞれ温度センサを備えている。温度センサは、エンコーダの温度、ひいてはモータの温度を検出する。エンコーダ１１，１２により検出された各回転軸の回転角度、及び温度センサにより検出された各エンコーダの温度は、コントローラ２０へ出力される。なお、エンコーダ１２は設定角度検出部を構成している。

アーム１０の動作状態は、コントローラ２０により制御される。コントローラ２０（動作判定装置に相当）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、入出力インターフェース等を備えるマイクロコンピュータである。コントローラ２０は、エンコーダ１１，１２及び温度センサによる検出値に基づいて、所定点Ｃを目標位置に移動させる。

詳しくは、コントローラ２０は、各温度センサにより検出される温度に基づいて、リンクＬ１，Ｌ２の温度を算出する。コントローラ２０は、基準温度（例えば２０℃）におけるリンクＬ１，Ｌ２の長さ、基準温度とリンクＬ１，Ｌ２の温度との差、及びリンクＬ１，Ｌ２の熱膨張係数に基づいて、リンクＬ１，Ｌ２の長さを算出する。コントローラ２０は、所定点Ｃの目標位置及びリンクＬ１，Ｌ２の長さに基づいて、関節Ｊ１，Ｊ２の目標回転角度を算出する（目標位置から目標回転角度への逆変換）。そして、コントローラ２０は、エンコーダ１１，１２によりそれぞれ検出される回転角度が、関節Ｊ１，Ｊ２のそれぞれの目標回転角度になるように、関節Ｊ１，Ｊ２のモータをそれぞれフィードバック制御する。なお、コントローラ２０は、関節Ｊ１，Ｊ２の回転角度及びリンクＬ１，Ｌ２の長さに基づいて、所定点Ｃの位置を算出する（回転角度から位置への変換）。

ここで、図２に示すように、コントローラ２０の電源ＯＮ直後（アーム１０の暖機動作前）は、アーム１０の温度が低く、リンクＬ１，Ｌ２の長さが短くなっている。標準温度（例えば４０℃）においてリンクＬ１，Ｌ２の長さが５００．０ｍｍであるのに対して、電源ＯＮ直後（例えば２０℃）においてリンクＬ１，Ｌ２の長さが４９９．８ｍｍになっている。

一方、図３に示すように、アーム１０暖機動作後は、アーム１０の温度が高く、リンクＬ１，Ｌ２の長さが長くなっている。標準温度（例えば４０℃）においてリンクＬ１，Ｌ２の長さが５００．０ｍｍであるのに対して、暖機動作後（例えば６０℃）においてリンクＬ１，Ｌ２の長さが５００．２ｍｍになっている。すなわち、リンクＬ１とリンクＬ２とが相対回転することにより、リンクＬ１，Ｌ２の温度が上昇する。リンクＬ１，Ｌ２は温度の上昇に伴って熱膨張するため、リンクＬ１，Ｌ２の長さは温度に応じて変化する。

このため、図３に示す暖機動作後にアーム１０の関節Ｊ２を完全に伸ばした状態で、所定点Ｃを移動させる位置である動作点を教示（設定）すると、図２に示す電源ＯＮ直後にアーム１０の関節Ｊ２を完全に伸ばしたとしても、教示した動作点（以下、「教示点」という）に所定点Ｃを移動させることができない。この場合、コントローラ２０は、所定点Ｃの目標位置としての教示点及びリンクＬ１，Ｌ２の長さに基づいて、関節Ｊ１，Ｊ２の目標回転角度を算出しようとしても、目標回転角度を算出することができない（目標回転角度の解なし）。

なお、暖機動作の前後でアーム１０の温度が異なる場合だけでなく、ロボットの動作環境の温度が場所や季節により変化した場合や、アーム１０の動作状態（発熱状態）が変化した場合も、同様の問題が生じ得る。

そこで、本実施形態では、コントローラ２０（限界角度算出部に相当）は、図４（ｂ）に示すように、所定温度（例えば２０℃）におけるリンクＬ１，Ｌ２の長さａ０，ｂ０、リンクＬ１，Ｌ２の熱膨張による長さ変化をそれぞれ補正する補正係数Ｍ１，Ｍ２、及び関節Ｊ２の角度を設定（教示）する時の設定温度（例えば６０℃）と所定温度との温度差Δに基づいて、図４（ｂ）に示すように所定温度において関節Ｊ２を完全に伸ばした状態のアーム１０の所定点Ｃの位置である第１位置に、図４（ａ）に示すように設定温度において所定点Ｃを移動させる時の関節Ｊ２の角度である限界角度θ１を算出する。

ここで、図４（ａ）に示すように、設定温度においてリンクＬ１の長さをａ１、リンクＬ２の長さをｂ１、基点Ｂから所定点Ｃまでの長さをｃ１とする。余弦定理により、以下の（１）式が成立する。なお、ｃ１＾２は、ｃ１の二乗を表す。

ｃ１＾２＝ａ１＾２＋ｂ１＾２−２×ａ１×ｂ１×ｃｏｓ（π−θ１） ・・・（１）
ｃｏｓθ１をテイラー展開して、θ１を０に近い微小角度として第３項以降を省略すると、（１）式を以下の（２）式のように変形することができる。

ｃ１＾２＝ａ１＾２＋ｂ１＾２＋２×ａ１×ｂ１×（１−θ１＾２／２）・・・（２）
（２）式をθ１＾２について解くと、以下の（３）式で表すことができる。

θ１＾２＝｛（ａ１＋ｂ１）＾２−ｃ１＾２｝／（ａ１×ｂ１） ・・・（３）
また、設定温度におけるリンクＬ１の長さａ１は、所定温度におけるリンクＬ１の長さａ０、上記補正係数Ｍ１、及び設定温度と所定温度との温度差Δを用いて、ａ１＝ａ０＋Ｍ１×Δで表すことができる。同様に、設定温度におけるリンクＬ２の長さｂ１は、ｂ１＝ｂ０＋Ｍ２×Δで表すことができる。そして、図４（ｂ）に示すように、所定温度において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さｃ０＝ａ０＋ｂ０と、図４（ａ）に示すように、設定温度における基点Ｂから所定点Ｃまでの長さｃ１とが等しいとする（ｃ１＝ｃ０）。すなわち、図４（ａ）の所定点Ｃの位置と、図４（ｂ）の所定点Ｃの位置とが等しいとする。これらの式を上記（３）式に代入すると、以下の（４）式のように表すことができる。

θ１＾２＝｛（ａ０＋Ｍ１×Δ＋ｂ０＋Ｍ２×Δ）＾２−（ａ０＋ｂ０）＾２｝／｛（ａ０＋Ｍ１×Δ）×（ｂ０＋Ｍ２×Δ）｝ ・・・（４）
（４）式を展開して微小値となる項を省略すると、以下の（５）式のように表すことができる。

θ１＾２＝２×（ａ０＋ｂ０）×（Ｍ１＋Ｍ２）×Δ／（ａ０×ｂ０） ・・・（５）
したがって、限界角度θ１は、以下の（６）式で表すことができる。

θ１＝√｛２×（ａ０＋ｂ０）×（Ｍ１＋Ｍ２）×Δ／（ａ０×ｂ０）｝・・・（６）
この限界角度θ１は、図４（ａ）に示すように設定温度において関節Ｊ２を完全に伸ばした状態に近い状態で関節Ｊ２の角度θを設定する場合に、これ以上関節Ｊ２の設定角度θを小さく設定すると、図４（ｂ）に示すように所定温度（＜設定温度）において関節Ｊ２を完全に伸ばしても設定温度での所定点Ｃの位置まで所定点Ｃを移動させることができなくなる角度である。なお、所定温度（作業時の温度）が設定温度（教示時の温度）よりも高い場合は、設定温度において関節Ｊ２を完全に伸ばした状態のアーム１０の所定点Ｃの位置に、所定温度において所定点Ｃを問題なく移動させることができる。

このため、コントローラ２０（判定部に相当）は、算出した限界角度θ１よりも、エンコーダ１２により検出された設定角度θが小さい場合に、図４（ｂ）に示すように所定温度において上記第１位置（図４（ａ）の所定点Ｃの位置）に所定点Ｃを移動させることが不可能であると判定する。また、コントローラ２０は、算出した限界角度θ１よりも、エンコーダ１２により検出された設定角度θが大きい場合に、所定温度において第１位置に所定点Ｃを移動させることが可能であると判定する。すなわち、コントローラ２０は、算出した限界角度θ１と、エンコーダ１２により検出された設定角度θとに基づいて、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定する。

さらに、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、コントローラ２０（不可通知部に相当）は、所定温度において第１位置に所定点Ｃを移動させることが不可能であることをユーザに通知する。具体的には、コントローラ２０に接続されたモニタやティーチングペンダントにより、所定温度において第１位置に所定点Ｃを移動させることが不可能であることを表示させたり、音声で通知させたりする。

加えて、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、コントローラ２０（温度通知部に相当）は、第１位置に所定点Ｃを移動させることが不可能となる温度をユーザに通知する。具体的には、コントローラ２０は、設定角度θ＝限界角度θ１となる温度差Δを算出する。そして、コントローラ２０は、所定温度に温度差Δを足した温度を、第１位置に所定点Ｃを移動させることが不可能となる温度として算出する。そして、コントローラ２０は、算出した温度をユーザに通知する。

また、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、コントローラ２０（角度通知部に相当）は、動作点の教示時に関節Ｊ２の角度を修正すべき量をユーザに通知する。具体的には、コントローラ２０は、設定角度θと限界角度θ１との差（θ−θ１）を、関節Ｊ２の角度を修正すべき量としてユーザに通知する。

あるいは、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、コントローラ２０（位置通知部に相当）は、動作点の教示時に教示点（動作点）の位置を修正すべき量をユーザに通知してもよい。具体的には、コントローラ２０は、設定角度θにおける基点Ｂと所定点Ｃとの距離と、限界角度θ１における基点Ｂと所定点Ｃとの距離との差を、教示点の位置を修正すべき量としてユーザに通知する。

以上詳述した本実施形態は、以下の利点を有する。

・エンコーダ１２により、動作点の教示時に設定された関節Ｊ２の角度である設定角度θが検出される。そして、算出された限界角度θ１と、エンコーダ１２により検出された設定角度θとに基づいて、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定される。その結果、所定温度において、設定した通りにアーム１０を動作させることが可能か否か判定することができる。

・コントローラ２０は、算出した限界角度θ１よりも、エンコーダ１２により検出された設定角度θが小さい場合に、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定することができる。

・所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、所定温度において第１位置に所定点Ｃを移動させることが不可能であることが通知される。このため、ユーザは、現在の動作点の設定（関節Ｊ２の角度の設定）では、所定温度において第１位置に所定点Ｃを移動させることが不可能となることを知ることができる。その結果、動作点の教示終了後に動作点の教示をやり直す必要が生じることや、ロボットの納入後に動作不能になることを抑制することができる。

・所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、第１位置に所定点Ｃを移動させることが不可能となる温度が通知される。このため、ユーザは、アーム１０を使用する環境の温度やアーム１０の使用状態を考慮して、第１位置に所定点Ｃを移動させることが可能であるか否か判断することができる。

・所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、関節Ｊ２の角度の設定時に関節Ｊ２の角度を修正すべき量が通知される。このため、ユーザは、所定温度において第１位置に所定点Ｃを移動させることを可能とするために、関節Ｊ２の角度の設定時に関節Ｊ２の角度を修正する量を容易に知ることができる。

・所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定された場合に、関節Ｊ２の角度の設定時に所定点Ｃの位置を修正すべき量が通知される。このため、ユーザは、所定温度において第１位置に所定点Ｃを移動させることを可能とするために、関節Ｊ２の角度の設定時に所定点Ｃの位置を修正する量を容易に知ることができる。

・コントローラ２０は、算出した限界角度θ１よりも、エンコーダ１２により検出された設定角度θが大きい場合に、所定温度において第１位置に所定点Ｃを移動させることが可能であると判定することができる。

・余弦定理、ｃｏｓ関数のテイラー展開を用い、所定温度において関節Ｊ２を完全に伸ばした状態を考慮すると、限界角度θ１を（６）式で表すことができる。したがって、簡易な式により限界角度θ１を算出することができ、コントローラ２０の演算負荷を軽減することができる。

なお、上記実施形態を、以下のように変更して実施することもできる。

・ｃｏｓθ１をテイラー展開して、θ１を０に近い微小角度として微小値となる項を省略する際に、第３項を省略せず、第４項以降又は第５項以降を省略してもよい。また、（４）式を展開して微小値となる項を省略する際に、分母においてはΔ＾２の項のみを省略してもよい。

・設定温度におけるリンクＬ１の長さａ１を、所定温度におけるリンクＬ１の長さａ０、補正係数Ｍ３、及び設定温度と所定温度との温度差Δを用いて、ａ１＝ａ０×（１＋Ｍ３×Δ）で表すように、補正係数Ｍ３を定義することもできる。同様に、ｂ１＝ｂ０×（１＋Ｍ４×Δ）で表すように、補正係数Ｍ４を定義することもできる。その場合、リンクＬ１，Ｌ２の長さａ１，ｂ１を上記式に変更した上で、（４）式以降の式を導出すればよい。

・上記の各通知のうち少なくとも一部を省略してもよい。

・ユーザによる動作点の教示時に限らず、シミュレーションによるアーム１０の動作確認時に、第１位置に所定点Ｃを移動させることが可能であるか否か判定してもよい。その場合、コントローラ２０（限界角度算出部、設定角度検出部、及び判定部に相当）はシミュレーションにおいて、シミュレーション時の設定に基づいて限界角度θ１を算出し、設定された関節Ｊ２の角度を設定角度θとして検出し、限界角度θ１と設定角度θとに基づいて、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定すればよい。

・アーム１０の動作可否を判定する温度である所定温度は、アーム１０を動作させる環境の温度やアーム１０の動作状態（発熱状態）に応じて、任意に設定することができる。

・各温度におけるリンクＬ１，Ｌ２の長さを予め算出して記憶しておき、コントローラ２０は、設定温度や所定温度に応じて、記憶されたリンクＬ１，Ｌ２の長さを読み込んで用いてもよい。

・垂直多関節ロボットの第２軸及び第３軸を構成するリンクＬ１，Ｌ２及び関節Ｊ１，Ｊ２に限らず、水平多関節ロボットの第１軸及び第２軸を構成する複数のリンク及び関節に具現化することもできる。さらに、ロボットのアーム１０に限らず、第１リンクと、第２リンクと、第１リンク及び第２リンクを相対回転可能に接続する関節と、を備えるマニプレータに具現化することもできる。

１０…アーム（マニプレータ）、１１…エンコーダ、１２…エンコーダ（設定角度検出部）、２０…コントローラ（動作判定装置）、Ｊ１…関節、Ｊ２…関節、Ｌ１…リンク、Ｌ２…リンク。

Claims (8)

1. 第１リンクと、第２リンクと、前記第１リンク及び前記第２リンクを相対回転可能に接続する関節と、を備えるマニプレータの動作可否を判定する動作判定装置であって、
   所定温度における前記第１リンク及び前記第２リンクの長さ、前記第１リンク及び前記第２リンクの熱膨張による長さ変化を補正する補正係数、及び前記関節の角度を設定する時の設定温度と前記所定温度との温度差に基づいて、前記所定温度において前記関節を完全に伸ばした状態の前記マニプレータの所定点の位置である第１位置に、前記設定温度において前記所定点を移動させる時の前記関節の角度である限界角度を算出する限界角度算出部と、
   前記設定された前記関節の角度である設定角度を検出する設定角度検出部と、
   前記限界角度算出部により算出された前記限界角度と、前記設定角度検出部により検出された前記設定角度とに基づいて、前記所定温度において前記第１位置に前記所定点を移動させることが可能であるか否か判定する判定部と、
   を備えるマニプレータの動作判定装置。
2. 前記判定部は、前記限界角度算出部により算出された前記限界角度よりも、前記設定角度検出部により検出された前記設定角度が小さい場合に、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定する請求項１に記載のマニプレータの動作判定装置。
3. 前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であることを通知する不可通知部を備える請求項２に記載のマニプレータの動作判定装置。
4. 前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記第１位置に前記所定点を移動させることが不可能となる温度を通知する温度通知部を備える請求項２又は３に記載のマニプレータの動作判定装置。
5. 前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記関節の角度の設定時に前記関節の角度を修正すべき量を通知する角度通知部を備える請求項２〜４のいずれか１項に記載のマニプレータの動作判定装置。
6. 前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定された場合に、前記関節の角度の設定時に前記所定点の位置を修正すべき量を通知する位置通知部を備える請求項２〜５のいずれか１項に記載のマニプレータの動作判定装置。
7. 前記判定部は、前記限界角度算出部により算出された前記限界角度よりも、前記設定角度検出部により検出された前記設定角度が大きい場合に、前記所定温度において前記第１位置に前記所定点を移動させることが可能であると判定する請求項１〜６のいずれか１項に記載のマニプレータの動作判定装置。
8. 前記限界角度算出部は、所定温度における前記第１リンク及び前記第２リンクの長さをそれぞれａ０，ｂ０とし、前記第１リンク及び前記第２リンクの熱膨張による長さ変化を補正する補正係数をそれぞれＭ１，Ｍ２とし、前記温度差をΔとし、前記限界角度をθ１とすると、
   θ１＝√｛２（ａ０＋ｂ０）×（Ｍ１＋Ｍ２）×Δ／（ａ０×ｂ０）｝により、限界角度θ１を算出する請求項１〜７のいずれか１項に記載のマニプレータの動作判定装置。

Images