JP6922213B2 - マニプレータの暖機運転要否判定装置 - Google Patents

Description

本発明は、マニプレータの暖機運転の要否を判定する判定装置に関する。

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

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

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

本発明は、こうした課題を解決するためになされたものであり、設定した通りにマニプレータを動作させるために暖機運転が必要であるか否か判定することのできるマニプレータの暖機運転要否判定装置を提供することにある。

上記課題を解決するための第１の手段は、
複数のリンクと、前記複数のリンクを相対回転可能に接続する関節と、を備えるマニプレータの暖機運転の要否を判定する判定装置であって、
前記マニプレータの所定点が移動する位置として設定された第１位置、前記第１位置を設定する時の設定温度における前記複数のリンクの長さ、及び所定温度における前記複数のリンクの長さに基づいて、前記第１位置に前記所定点を移動させるために前記所定温度において前記マニプレータの暖機運転が必要であるか否か判定する。

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

ここで、マニプレータの所定点が移動する位置として設定された第１位置に所定温度において所定点を移動させることが可能か否かは、第１位置、第１位置を設定する時の設定温度における複数のリンクの長さ、及び所定温度における複数のリンクの長さにより決まる。そして、所定温度において第１位置に所定点を移動させることが不可能である場合は、リンクの長さを第１位置の設定時におけるリンクの長さまで伸ばすために、マニプレータの暖機運転が必要となる。したがって、第１位置、設定温度における複数のリンクの長さ、及び所定温度における複数のリンクの長さに基づいて、第１位置に所定点を移動させるために所定温度においてマニプレータの暖機運転が必要であるか否か判定することができる。

第２の手段では、前記暖機運転が必要であると判定された場合に、前記暖機運転が必要であることを通知する必要通知部を備える。

上記構成によれば、暖機運転が必要であると判定された場合に、暖機運転が必要であることが通知される。このため、ユーザは、マニプレータの温度が所定温度では、第１位置に所定点を移動させることが不可能であるため、マニプレータの暖機運転が必要であることを知ることができる。

第３の手段では、前記所定温度は、前記マニプレータの現在の温度であり、前記暖機運転が必要であると判定された場合に、前記第１位置に前記所定点を移動させることが可能となるまで前記マニプレータの暖機運転を実行する暖機運転実行部を備える。

上記構成によれば、第１位置に所定点を移動させるために、マニプレータの現在の温度においてマニプレータの暖機運転が必要であるか否か判定される。そして、暖機運転が必要であると判定された場合に、第１位置に所定点を移動させることが可能となるまでマニプレータの暖機運転が実行される。したがって、暖機運転が必要である場合に暖機運転が実行され、第１位置に所定点を移動させることが可能となる。

第４の手段では、前記暖機運転実行部は、前記暖機運転が必要であると判定された場合に、前記第１位置に前記所定点を移動させることが可能となる前記マニプレータの温度よりも、前記マニプレータの温度を所定の余裕温度だけ上昇させて前記暖機運転を終了する。

上記構成によれば、暖機運転が必要であると判定された場合に、第１位置に所定点を移動させることが可能となるマニプレータの温度よりも、マニプレータの温度が所定の余裕温度だけ上昇させられて暖機運転が終了される。このため、暖機運転後にマニプレータの温度が若干下がったとしても、第１位置に所定点を移動させることが不可能になることを抑制することができる。詳しくは、暖機運転したタイミングと、第1位置への移動タイミングとは一致するとは限らない。例えばロボット（マニプレータ）での作業は目標点への移動動作と目標点周辺での姿勢変更動作、他の設備との連携待ちによる待機等によって成り立っている。このため、少し間をおいた結果として冷却されてからとか、熱量加算が少し足りなくてアーム長が少し短くなってから第1位置へ移動させることも考えられる。そのような場合は、第１位置へ到達するにはアーム長が足りなくなるので、それに対する事前熱量蓄積として余裕温度分の暖機運転を行っている。さらに、マニプレータの温度が余裕温度だけ上昇させられると暖機運転が終了されるため、マニプレータの暖機運転を過剰に実行することを抑制することができ、マニプレータの作業性が低下することを抑制することができる。

第５の手段では、前記暖機運転実行部により前記暖機運転が実行される際に、前記暖機運転の推定終了時刻を通知する終了通知部を備える。

上記構成によれば、暖機運転実行部により暖機運転が実行される際に、暖機運転の推定終了時刻が通知される。このため、ユーザは、暖機運転が終了すると推定される時刻を知ることができ、マニプレータの作業予定を立て易くなる。

第６の手段では、前記マニプレータは、複数の前記関節を備えており、前記暖機運転が必要であると判定された場合に、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定する関節判定部を備える。

マニプレータが複数の関節を備えている場合、それぞれの関節の温度上昇がリンクの温度上昇に与える影響がそれぞれ異なる。このため、どの関節を暖機運転するかによって、マニプレータの暖機運転終了までの時間が異なる。

この点、暖機運転が必要であると判定された場合に、複数の関節のうちどの関節を暖機運転すべきであるかが判定される。したがって、マニプレータの暖機運転を効率的に実行させることができる。

所定温度では移動させることが不可能である第１位置へ所定点を移動させるためには、所定点から第１位置の方向へ所定点が移動するようにリンクを伸ばす必要がある。ここで、各リンクの長さに応じて、各リンクの温度が上昇した際の各リンクの伸び量が異なる。さらに、各リンクの方向と、所定点から第１位置への方向との角度に応じて、各リンクが伸びた際に所定点が第１位置の方向へ移動する割合が異なる。

この点、第７の手段では、前記関節判定部は、各リンクの長さ及び方向を表す第１ベクトルと、前記所定点から前記第１位置へのベクトルである第２ベクトルとの内積に基づいて、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定するといった構成を採用している。したがって、各リンクの長さ及び方向を考慮して、どの関節を暖機運転すべきであるかを適切に判定することができる。換言すれば、第１位置への到達とは関係ない関節の暖機は省略（通常の動作で温まることを期待）し、その分だけ暖機時間を短縮したり電力消費を抑制したりすることが可能になる。

１つの関節の温度が上昇した場合、その関節に接続されたリンクを介して、隣の関節や隣の関節に接続されたリンクまで熱が伝達されることがある。

この点、第８の手段では、前記関節判定部は、各関節の温度上昇が各リンクの熱膨張に及ぼす影響に基づいて、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定するといった構成を採用している。したがって、各関節の温度上昇が各リンクの熱膨張に及ぼす影響を考慮して、どの関節を暖機運転すべきであるかを適切に判定することができる。

第９の手段では、前記関節判定部によりどの関節を暖機運転すべきであるか判定された場合に、前記暖機運転すべきであると判定された関節を通知する関節通知部を備える。

上記構成によれば、どの関節を暖機運転すべきであるか判定された場合に、暖機運転すべきであると判定された関節が通知される。したがって、ユーザは、どの関節を暖機運転すべきであるか知ることができる。

アーム及びコントローラを示す模式図。 電源ＯＮ直後の伸び状態のアームを示す模式図。 暖機動作後の伸び状態のアームを示す模式図。 伸び状態における位置から角度への逆変換を示す模式図。 曲げ状態における位置から角度への逆変換を示す模式図。 スプライン軌道及び動作点を示す模式図。 どの関節を暖機運転すべきかを判定する態様を示す模式図。 時間とアームの温度上昇との関係を示すグラフ。 設定温度における限界角度と所定温度での動作可否を示す模式図。

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

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

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

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

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

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

一方、図３に示すように、アーム１０暖機動作後は、アーム１０の温度が高く、リンクＬ１，Ｌ２の長さが長くなっている。標準温度（例えば４０℃）においてリンクＬ１，Ｌ２の長さが５００．０ｍｍであるのに対して、暖機動作後（例えば６０℃）においてリンクＬ１，Ｌ２の長さが５００．２ｍｍになっている。

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

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

コントローラ２０（第１位置算出部に相当）は、図４（ａ）及び図５（ａ）に示すように、ユーザによる所定点Ｃの動作点の教示時に、教示時の温度（例えば６０℃）におけるリンクＬ１，Ｌ２の長さ及び教示点に基づいて、所定点Ｃが移動する位置である第１位置を算出する。各教示点に所定点Ｃを直線的に移動させる場合は、各教示点を第１位置とすればよい。

しかしながら、図６に示すように、動作点Ｐ１，Ｐ２，Ｐ４，Ｐ５の間を、所定点Ｃがスプライン軌道Ｔ（動作軌道に相当）で移動する場合は、スプライン軌道Ｔ上において教示点ではない点Ｐ３に所定点Ｃを移動させることができない場合がある。スプライン軌道Ｔは、動作点Ｐ１，Ｐ２，Ｐ４，Ｐ５を通る滑らかな曲線として規定される。例えば、スプライン軌道Ｔにおいて点Ｐ３の付近の曲率は他の部分の曲率よりも大きくなっており、点Ｐ３の両脇に、関節Ｊ２を完全に伸ばした状態で動作点Ｐ２，Ｐ４が設定されている。この場合、点Ｐ３に所定点Ｃを移動させることができない。

したがって、コントローラ２０は、スプライン軌道Ｔ上の全ての点を上記第１位置とする。すなわち、第１位置は、動作点Ｐ１，Ｐ２，Ｐ４，Ｐ５により規定されるスプライン軌道Ｔ上の点Ｐ３を含んでいる。

コントローラ２０（角度算出部に相当）は、図４（ｂ）及び図５（ｂ）に示すように、所定温度（例えば４０℃）におけるリンクＬ１，Ｌ２の長さに基づいて、算出された第１位置に所定点Ｃを移動させる時の関節Ｊ１，Ｊ２の回転角度を算出する。詳しくは、コントローラ２０は、基準温度（例えば２０℃）におけるリンクＬ１，Ｌ２の長さ（例えば４９９．８ｍｍ）、基準温度とリンクＬ１，Ｌ２の温度との差（例えば４０℃−２０℃＝２０℃）、及びリンクＬ１，Ｌ２の熱膨張係数に基づいて、４０℃でのリンクＬ１，Ｌ２の長さを算出する。そして、コントローラ２０は、第１位置及びリンクＬ１，Ｌ２の長さに基づいて、関節Ｊ１，Ｊ２の回転角度を算出する（位置から回転角度への逆変換）。

そして、コントローラ２０（角度判定部に相当）は、回転角度の算出結果に基づいて、現在の温度（所定温度）において第１位置に所定点Ｃを移動させることが可能であるか否か判定する。詳しくは、図４（ｂ）に示すように、コントローラ２０は、回転角度が算出されなかった場合に、現在の温度において第１位置に所定点Ｃを移動させることが不可能であると判定する。このとき、コントローラ２０は、第１位置に所定点Ｃを移動させるために、現在の温度においてアーム１０の暖機運転が必要であると判定する。また、図５（ｂ）に示すように、コントローラ２０は、算出された回転角度θ１２，θ２２が、それぞれ関節Ｊ１，Ｊ２により実現可能な角度であった場合に、現在の温度において第１位置に所定点Ｃを移動させることが可能であると判定する。このとき、コントローラ２０は、第１位置に所定点Ｃを移動させるために、現在の温度においてアーム１０の暖機運転が必要でないと判定する。

さらに、アーム１０の暖機運転が必要であると判定された場合に、コントローラ２０（必要通知部に相当）は、アーム１０の暖機運転が必要であることをユーザに通知する。具体的には、コントローラ２０に接続されたモニタやティーチングペンダントにより、アーム１０の暖機運転が必要であることを表示させたり、音声で通知させたりする。

アーム１０は複数の関節Ｊ１，Ｊ２を備えており、それぞれの関節Ｊ１，Ｊ２の温度上昇がリンクＬ１，Ｌ２の温度上昇に与える影響がそれぞれ異なる。このため、どの関節を暖機運転するかによって、アーム１０の暖機運転終了までの時間が異なる。

そこで、コントローラ２０（関節判定部に相当）は、暖機運転が必要であると判定された場合に、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかを判定する。なお、簡易的に関節Ｊ１，Ｊ２のみを対象として説明するが、その他の関節も対象とする場合も手順は同様である。

詳しくは、図７に示すように、基点Ｂから関節Ｊ２の中心点Ｍへのベクトルｖ１、中心点Ｍから所定点Ｃへのベクトルｖ２、所定点Ｃから例えば点Ｐ３（第１位置に相当）へのベクトルｖ３を考える。ベクトルｖ１，ｖ２は、各リンクＬ１，Ｌ２の長さ及び方向を表すベクトルである。

所定温度では移動させることが不可能である点Ｐ３へ所定点Ｃを移動させるためには、所定点Ｃから点Ｐ３の方向へ所定点Ｃが移動するようにリンクＬ１，Ｌ２を伸ばす必要がある。ここで、各リンクＬ１，Ｌ２の長さに応じて、各リンクＬ２，Ｌ２の温度が上昇した際の各リンクＬ１，Ｌ２の伸び量が異なる。さらに、リンクＬ１，Ｌ２の方向（ベクトルｖ１，ｖ２の方向）と、所定点Ｃから点Ｐ３への方向（ベクトルｖ３の方向）との角度に応じて、各リンクＬ１，Ｌ２が伸びた際に所定点Ｃが点Ｐ３の方向へ移動する割合が異なる。

そこで、コントローラ２０は、各リンクＬ１，Ｌ２の長さ及び方向を表すベクトルｖ１，ｖ２（第１ベクトルに相当）と、所定点Ｃから点Ｐ３へのベクトルであるベクトルｖ３（第２ベクトルに相当）との内積に基づいて、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかを判定する。

また、１つの関節の温度が上昇した場合、その関節に接続されたリンクを介して、隣の関節や隣の関節に接続されたリンクまで熱が伝達される。例えば、関節Ｊ１の温度が上昇した場合、関節Ｊ１に接続されたリンクＬ１を介して、隣の関節Ｊ２や関節Ｊ２に接続されたリンクＬ２まで熱が伝達される。

そこで、コントローラ２０は、各関節Ｊ１，Ｊ２の温度上昇が各リンクＬ１，Ｌ２の熱膨張に及ぼす影響に基づいて、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかを判定する。

具体的には、以下の式により、関節Ｊｎの温度上昇により所定点Ｃが点Ｐ３の方向へ移動する量である移動量Ｃｎを算出する。ｖ１ｍは、各リンクＬｍの長さ及び方向を表す第１ベクトルである。ｖ２０は、所定点Ｃから点Ｐ３へのベクトルである第２ベクトルである。ｋｍｎは、各リンクＬｍが関節Ｊｎの温度上昇により熱膨張する量（比率）を表す熱膨張係数である。なお、熱膨張係数ｋｍｎは、予め実験等に基づいて取得しておくことができる。

Ｃｎ＝（ｖ１１・ｖ２０）×ｋ１ｎ＋（ｖ１２・ｖ２０）×ｋ２ｎ＋・・・＋（ｖ１ｍ・ｖ２０）×ｋｍｎ
例えば、図７に示すように、関節Ｊ１の温度上昇により所定点Ｃが点Ｐ３の方向へ移動する量である移動量Ｃ１は次式で算出される。Ｃ１＝（ｖ１・ｖ３）×ｋ１１＋（ｖ２・ｖ３）×ｋ２１。同様にして、関節Ｊ２の温度上昇により所定点Ｃが点Ｐ３の方向へ移動する量である移動量Ｃ２は次式で算出される。Ｃ２＝（ｖ１・ｖ３）×ｋ１２＋（ｖ２・ｖ３）×ｋ２２。そして、コントローラ２０は、移動量Ｃ１と移動量Ｃ２とのうち、移動量が大きい方の関節を暖機運転すべき関節であると判定する。すなわち、コントローラ２０は、移動量Ｃｎのうち移動量が最も大きい関節を暖機運転すべき関節であると判定する。そして、コントローラ２０（関節通知部に相当）は、暖機運転すべきであると判定された関節をユーザに通知する。

加えて、暖機運転が必要であると判定された場合に、コントローラ２０（可能温度算出部に相当）は、点Ｐ３（第１位置）に所定点Ｃを移動させることが可能となるアーム１０の温度Ｔｅ１を算出する。具体的には、コントローラ２０は、上昇後の温度におけるリンクＬ１，Ｌ２の長さと熱膨張係数とに基づいて、算出された第１位置に所定点Ｃを移動させる時の関節Ｊ１，Ｊ２の回転角度を算出することができるようになる温度Ｔｅ１を算出する。このとき、コントローラ２０は、各リンクＬ１，Ｌ２の温度を個別に算出して、算出した各リンクＬ１，Ｌ２の温度に基づいて温度Ｔｅ１を算出してもよいし、リンクＬ１，Ｌ２の温度の平均値に基づいて温度Ｔｅ１を算出してもよい。そして、コントローラ２０は、温度Ｔｅ１に所定の余裕温度ΔＴを足した温度を、暖機目標温度Ｔｅ２（＝Ｔｅ１＋ΔＴ）として算出する。

コントローラ２０（暖機運転実行部に相当）は、点Ｐ３に所定点Ｃを移動させることが可能となるまでアーム１０の暖機運転を実行する。具体的には、コントローラ２０はアーム１０の温度を暖機目標温度Ｔｅ２まで上昇させて暖機運転を終了する。このとき、コントローラ２０は、上記暖機運転すべきであると判定された関節を暖機運転する。

暖機運転を実行することにより、図８に示すようにアーム１０の温度が上昇する。図８に示す関係は、ロボットの動作条件に応じて、予め取得しておくことができる。そこで、コントローラ２０（終了通知部に相当）は、暖機運転が実行される際に、図８に示す関係に基づいて、アーム１０の温度が暖機目標温度Ｔｅ２になる暖機運転の推定終了時刻ｔ１を推定してユーザに通知する。なお、コントローラ２０は、暖機運転開始後の所定期間におけるアーム１０の温度上昇態様から、図８に示す温度上昇曲線を予測し、予測した温度上昇曲線と暖機目標温度Ｔｅ２とに基づいて推定終了時刻ｔ１を推定してもよい。

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

・アーム１０の所定点Ｃが移動する位置として設定された動作点Ｐ１〜Ｐ５（第１位置）に現在の温度（所定温度）において所定点Ｃを移動させることが可能か否かは、第１位置、第１位置を教示（設定）する時の設定温度におけるリンクＬ１，Ｌ２の長さ、及び所定温度におけるリンクＬ１，Ｌ２の長さにより決まる。そして、所定温度において第１位置に所定点Ｃを移動させることが不可能である場合は、リンクＬ１，Ｌ２の長さを第１位置の設定時におけるリンクＬ１，Ｌ２の長さまで伸ばすために、アーム１０の暖機運転が必要となる。したがって、第１位置、設定温度における複数のリンクＬ１，Ｌ２の長さ、及び所定温度における複数のリンクＬ１，Ｌ２の長さに基づいて、第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定することができる。

・暖機運転が必要であると判定された場合に、暖機運転が必要であることがユーザに通知される。このため、ユーザは、アーム１０の温度が現在の温度では、第１位置に所定点Ｃを移動させることが不可能であるため、アーム１０の暖機運転が必要であることを知ることができる。

・暖機運転が必要であると判定された場合に、第１位置に所定点Ｃを移動させることが可能となるまでアーム１０の暖機運転が実行される。したがって、暖機運転が必要である場合に暖機運転が実行され、第１位置に所定点Ｃを移動させることが可能となる。

・暖機運転が必要であると判定された場合に、第１位置に所定点Ｃを移動させることが可能となるアーム１０の温度Ｔｅ１よりも、アーム１０の温度が所定の余裕温度ΔＴだけ上昇させられて暖機運転が終了される。このため、暖機運転後にアーム１０の温度が若干下がったとしても、第１位置に所定点Ｃを移動させることが不可能になることを抑制することができる。さらに、アーム１０の温度が余裕温度ΔＴだけ上昇させられると暖機運転が終了されるため、アーム１０の暖機運転を過剰に実行することを抑制することができ、アーム１０の作業性が低下することを抑制することができる。

・暖機運転が実行される際に、暖機運転の推定終了時刻ｔ１が通知される。このため、ユーザは、暖機運転が終了すると推定される時刻ｔ１を知ることができ、アーム１０の作業予定を立て易くなる。

・暖機運転が必要であると判定された場合に、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかが判定される。したがって、アーム１０の暖機運転を効率的に実行させることができる。

・コントローラ２０は、各リンクＬ１，Ｌ２の長さ及び方向を表す第１ベクトルと、所定点Ｃから第１位置へのベクトルである第２ベクトルとの内積に基づいて、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかを判定している。したがって、各リンクＬ１，Ｌ２の長さ及び方向を考慮して、どの関節を暖機運転すべきであるかを適切に判定することができる。

・コントローラ２０は、各関節Ｊ１，Ｊ２の温度上昇が各リンクＬ１，Ｌ２の熱膨張に及ぼす影響（熱膨張係数ｋｍｎ）に基づいて、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるかを判定している。したがって、各関節Ｊ１，Ｊ２の温度上昇が各リンクＬ１，Ｌ２の熱膨張に及ぼす影響を考慮して、どの関節を暖機運転すべきであるかを適切に判定することができる。

・どの関節を暖機運転すべきであるか判定された場合に、暖機運転すべきであると判定された関節がユーザに通知される。したがって、ユーザは、どの関節を暖機運転すべきであるか知ることができる。

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

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

・上記実施形態では、アーム１０の暖機運転が必要であると判定された場合に、コントローラ２０は、アーム１０の暖機運転が必要であることをユーザに通知していた。この場合に、コントローラ２０はアーム１０の暖機運転を実行せず、ユーザの指示や操作によりアーム１０の暖機運転を実行させることもできる。

・アーム１０の暖機運転が必要であると判定された場合に、コントローラ２０は、アーム１０の暖機運転が必要であることをユーザに通知せず、暖機運転を実行することもできる。

・上記実施形態では、コントローラ２０は、アーム１０の温度を暖機目標温度Ｔｅ２まで上昇させて暖機運転を終了していた。これに対して、アーム１０の温度がその後に上昇する見込みがあれば、コントローラ２０は、アーム１０の温度を温度Ｔｅ１まで上昇させて暖機運転を終了してもよい。

・上記実施形態では、暖機運転が必要であると判定された場合に、コントローラ２０は、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるか判定していた。これに対して、コントローラ２０は、複数の関節Ｊ１，Ｊ２のうちどの関節を暖機運転すべきであるか判定せず、アーム１０全体を暖機運転することもできる。

・各リンク（第１ベクトル）の長さ及び方向のうち一方のみを考慮してもよい。また、第１ベクトルと第２ベクトルとの内積、及び各関節Ｊ１，Ｊ２が各リンクＬ１，Ｌ２の熱膨張に及ぼす影響のうち、一方のみを考慮してもよい。

・アーム１０による作業時に限らず、シミュレーションによるアーム１０の動作確認時に、第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定してもよい。その場合、コントローラ２０は、シミュレーションにおいて、設定した第１位置、設定した温度におけるリンクＬ１，Ｌ２の長さ、及び所定温度におけるリンクＬ１，Ｌ２の長さに基づいて、第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定すればよい。

（第２実施形態）
以下、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定する構成、すなわち第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定する構成を変更した第２実施形態について、第１実施形態との相違点を中心に説明する。なお、第１実施形態と同一の部分については、同一の符号を付すことにより説明を省略する。

図２に示すように、所定温度（例えば２０℃）において所定点Ｃを移動させることができる基点Ｂから最も離れた位置は、関節Ｊ２を完全に伸ばした状態での所定点Ｃの位置となる。したがって、図３に示すように、動作点の教示時（例えば６０℃）に算出した第１位置がこの位置よりも外側にある場合は、所定温度において所定点Ｃを第１位置に移動させることができない。

そこで、本実施形態では、コントローラ２０（長さ判定部に相当）は、動作点の教示時の温度における基点Ｂから第１位置までの距離と、所定温度（現在の温度）において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さとに基づいて、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定する。すなわち、コントローラ２０は、第１位置に所定点Ｃを移動させるために、所定温度においてアーム１０の暖機運転が必要であるか否か判定する。

詳しくは、コントローラ２０は、動作点の教示時の温度における基点Ｂから第１位置までの距離が、所定温度において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さよりも長い場合に、所定温度において第１位置に所定点Ｃを移動させることが不可能であると判定する。そして、コントローラ２０は、第１位置に所定点Ｃを移動させるために、作業時（現在）の温度が所定温度であればアーム１０の暖機運転が必要であると判定する。また、コントローラ２０は、動作点の教示時の温度における基点Ｂから第１位置までの距離が、所定温度において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さよりも短い場合に、作業時（現在）の温度が所定温度であれば第１位置に所定点Ｃを移動させることが可能であると判定する。そして、コントローラ２０は、第１位置に所定点Ｃを移動させるために、所定温度においてアーム１０の暖機運転が必要でないと判定する。

本実施形態は、以下の利点を有する。

・動作点の教示時の温度における基点Ｂから第１位置までの距離と、所定温度において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さとに基づいて、所定温度において所定点Ｃを第１位置に移動させることが可能であるか否か判定される。そして、第１位置に所定点Ｃを移動させるために、所定温度においてアーム１０の暖機運転が必要であるか否か判定される。このため、教示した通りにアーム１０を動作させるために、暖機運転が必要であるか否かを、簡易な演算により高速で判定することができる。

・第１位置に所定点Ｃを移動させるために、所定温度においてアーム１０の暖機運転が必要であることを、簡易な演算により高速で判定することができる。

・第１位置に所定点Ｃを移動させるために、所定温度においてアーム１０の暖機運転が必要でないことを、簡易な演算により高速で判定することができる。

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

・アーム１０による作業時に限らず、シミュレーションによるアーム１０の動作確認時に、第１位置に所定点Ｃを移動させることが可能であるか否か、ひいては第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定してもよい。その場合、コントローラ２０は、シミュレーションにおいて、設定した第１位置、設定した温度におけるリンクＬ１，Ｌ２の長さ、及び作業時を想定した所定温度におけるＬ１，Ｌ２に基づいて、設定した温度における基点Ｂから第１位置までの距離と、所定温度において関節Ｊ２を完全に伸ばした状態での基点Ｂから所定点Ｃまでの長さとに基づいて、第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定すればよい。

（第３実施形態）
所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か判定する構成、すなわち第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定する構成を変更した第３実施形態について、第１実施形態との相違点を中心に説明する。なお、第１実施形態と同一の部分については、同一の符号を付すことにより説明を省略する。

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

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

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

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

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

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

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

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

このため、コントローラ２０（判定部に相当）は、算出した限界角度θ１よりも設定角度θが小さい場合に、図９（ｂ）に示すように所定温度において上記第１位置（図９（ａ）の所定点Ｃの位置）に所定点Ｃを移動させることが不可能であると判定する。すなわち、コントローラ２０は、第１位置に所定点Ｃを移動させるために、作業時（現在）の温度が所定温度であればアーム１０の暖機運転が必要であると判定する。また、コントローラ２０は、算出した限界角度θ１よりも設定角度θが大きい場合に、所定温度において第１位置に所定点Ｃを移動させることが可能であると判定する。すなわち、コントローラ２０は、第１位置に所定点Ｃを移動させるために、作業時（現在）の温度が所定温度であればアーム１０の暖機運転が必要でないと判定する。

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

・算出された限界角度θ１と、設定角度θとに基づいて、所定温度において第１位置に所定点Ｃを移動させることが可能であるか否か、すなわち第１位置に所定点Ｃを移動させるために所定温度においてアーム１０の暖機運転が必要であるか否か判定することができる。

・コントローラ２０は、算出した限界角度θ１よりも設定角度θが小さい場合に、所定温度においてアーム１０の暖機運転が必要であると判定することができる。

・コントローラ２０は、算出した限界角度θ１よりも設定角度θが大きい場合に、所定温度においてアーム１０の暖機運転が必要でないと判定することができる。

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

また、第１〜第３実施形態を、以下のように変更して実施することもできる。

・基点Ｂは、リンクＬ１において、関節Ｊ２と反対側の端部の中心（１点）に限らず、関節Ｊ１と関節Ｊ２との間に設定することもできる。

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

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

・教示点にアーム１０の所定点Ｃを移動させる動作とは別に、教示点から所定方向へ所定距離だけ所定点Ｃを移動させる動作をアーム１０に実行させる場合がある。この場合、教示点から所定方向へ所定距離の位置を、所定点Ｃが移動する位置である第１位置に含めればよい。

・垂直多関節ロボットの第２軸及び第３軸を構成するリンクＬ１，Ｌ２及び関節Ｊ１，Ｊ２に限らず、水平多関節ロボットの第１軸及び第２軸を構成する複数のリンク及び関節に具現化することもできる。さらに、ロボットのアーム１０に限らず、複数のリンクと、複数のリンクを相対回転可能に接続する関節と、を備えるマニプレータに具現化することもできる。また、リンクの数は２つに限らず、３つ以上であっても、同様の考え方によりマニプレータの暖機運転の要否を判定することができる。

１０…アーム（マニプレータ）、２０…コントローラ（暖機運転要否判定装置）、Ｊ１…関節、Ｊ２…関節、Ｌ１…リンク、Ｌ２…リンク。

Claims (9)

1. 複数のリンクと、前記複数のリンクを相対回転可能に接続する関節と、を備えるマニプレータの暖機運転の要否を判定する判定装置であって、
   前記マニプレータの所定点が移動する位置として設定された第１位置、前記第１位置を設定する時の設定温度における前記複数のリンクの長さ、及び所定温度における前記複数のリンクの長さに基づいて、前記所定温度において前記第１位置に前記所定点を移動させることが不可能であると判定した場合に前記第１位置に前記所定点を移動させるために前記所定温度において前記マニプレータの暖機運転が必要であると判定するマニプレータの暖機運転要否判定装置。
2. 前記暖機運転が必要であると判定された場合に、前記暖機運転が必要であることを通知する必要通知部を備える請求項１に記載のマニプレータの暖機運転要否判定装置。
3. 前記所定温度は、前記マニプレータの現在の温度であり、
   前記暖機運転が必要であると判定された場合に、前記第１位置に前記所定点を移動させることが可能となるまで前記マニプレータの暖機運転を実行する暖機運転実行部を備える請求項１又は２に記載のマニプレータの暖機運転要否判定装置。
4. 前記暖機運転実行部は、前記暖機運転が必要であると判定された場合に、前記第１位置に前記所定点を移動させることが可能となる前記マニプレータの温度よりも、前記マニプレータの温度を所定の余裕温度だけ上昇させて前記暖機運転を終了する請求項３に記載のマニプレータの暖機運転要否判定装置。
5. 前記暖機運転実行部により前記暖機運転が実行される際に、前記暖機運転の推定終了時刻を通知する終了通知部を備える請求項３又は４に記載のマニプレータの暖機運転要否判定装置。
6. 前記マニプレータは、複数の前記関節を備えており、
   前記暖機運転が必要であると判定された場合に、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定する関節判定部を備える請求項１〜５のいずれか１項に記載のマニプレータの暖機運転要否判定装置。
7. 前記関節判定部は、各リンクの長さ及び方向を表す第１ベクトルと、前記所定点から前記第１位置へのベクトルである第２ベクトルとの内積に基づいて、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定する請求項６に記載のマニプレータの暖機運転要否判定装置。
8. 前記関節判定部は、各関節の温度上昇が各リンクの熱膨張に及ぼす影響に基づいて、前記複数の関節のうちどの関節を暖機運転すべきであるかを判定する請求項６又は７に記載のマニプレータの暖機運転要否判定装置。
9. 前記関節判定部によりどの関節を暖機運転すべきであるか判定された場合に、前記暖機運転すべきであると判定された関節を通知する関節通知部を備える請求項６〜８のいずれか１項に記載のマニプレータの暖機運転要否判定装置。

Images