JP6040002B2 - 教示情報修正装置、制御装置、及び教示情報修正方法 - Google Patents

Description

本発明は、２以上のハンド部を有するマニピュレータの教示情報を修正する教示情報修正装置等に関する。

従来、ダブルハンドの水平多関節マニピュレータの制御において、一方のハンド部が被搬送物を収容装置等に搬出入する際に、他方のハンド部が収容装置等と干渉しないように退避させることが行われていた（例えば、特許文献１参照）。

特開２００９−１３６９８１号公報

しかしながら、退避させるハンド部の教示位置が不適切であると、退避が不十分となり、そのハンド部と収容装置等との干渉の発生する危険性があった。

本発明は、上記課題を解決するためになされたものであり、２以上のハンド部を有する水平多関節マニピュレータについて、一のハンド部が退避位置から伸長位置に移動した際に、他のハンド部が干渉しないように教示情報を修正する教示情報修正装置等を提供することを目的とする。

上記目的を達成するため、本発明による教示情報修正装置は、モータによって駆動される関節によって連結された複数のアームと複数のアームの先端に設けられた第１及び第２のハンド部とを有する水平多関節マニピュレータの第１及び第２のハンド部の各教示位置を示す教示情報が記憶される教示情報記憶部と、第１のハンド部が退避位置から伸長位置まで移動した場合に、第２のハンド部の教示位置が、第２のハンド部の干渉を防止するためにあらかじめ設定された退避線から安全側の位置であるかどうかを、教示情報を用いて判断する判断部と、第２のハンド部の教示位置が退避線から安全側の位置でないと判断部が判断した場合に、第２のハンド部の教示位置が、退避線から安全側の位置となるように教示情報を修正する修正部と、を備えたものである。
このような構成により、第２のハンド部の教示位置を、干渉の危険性の低い安全な位置に移動させることができる。その結果、干渉の発生を防止することができる。

また、本発明による教示情報修正装置では、修正部は、第２のハンド部の教示位置が退避線から安全側の位置でないと判断部が判断した場合に、第２のハンド部の修正後の教示位置が退避線上となるように教示情報を修正してもよい。
このような構成により、第２のハンド部の教示位置が退避線よりも安全側に大きく移動されることを防止できる。その結果、教示情報の修正に応じた想定外の干渉の発生等を防止することができる。

また、本発明による教示情報修正装置では、修正部は、第２のハンド部の教示位置が退避線から安全側の位置でないと判断部が判断した場合であり、第２のハンド部の退避線上の修正先の教示位置が２個ある場合には、第２のハンド部の修正後の教示位置が、２個の修正先の教示位置のうち、修正前の教示位置から近い方となるように教示情報を修正してもよい。
このような構成により、教示情報の修正に応じた教示位置の移動を最小限に抑えることができる。

また、本発明による教示情報修正装置では、判断部は、第１のハンド部が退避位置から伸長位置まで移動した場合に、第２のハンド部の教示位置が、第２のハンド部の干渉が発生するかどうかの境界となる境界線の安全側であるかどうかをも判断し、修正部は、第２のハンド部の教示位置が境界線と退避線との間の位置であると判断部が判断した場合には、修正を行わなくてもよい。
このような構成により、干渉が発生しないのであれば、第２のハンド部の教示位置を変更しないことになり、教示情報が変更されることを防止できる。一方、教示位置を変更する場合には、より安全な位置にまで変更することによって、安全性を高めることができる。

また、本発明による制御装置は、教示情報修正装置と、教示情報修正装置が修正した教示情報を用いて、モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えたものである。
このような構成により、修正後の教示情報に応じてマニピュレータを動作させることによって、干渉の発生を防止することができる。

本発明による教示情報修正装置等によれば、第２のハンド部の教示位置を、干渉の危険性の低い安全な位置に移動させることができる。その結果、干渉の発生を防止することができる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図 同実施の形態によるマニピュレータの動作の一例を示す図 同実施の形態における教示位置に関する判断について説明するための図 同実施の形態における教示位置の修正について説明するための図 同実施の形態におけるハンド部について説明するための図 同実施の形態における修正後の教示位置の算出について説明するための図 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態における教示位置と境界線・退避線との関係の一例を示す図 同実施の形態における教示位置と境界線・退避線との関係の一例を示す図 同実施の形態によるマニピュレータの他の一例を示す図

以下、本発明によるマニピュレータの制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、干渉の発生を防止するために教示情報を修正するものである。

図１は、本実施の形態による産業用ロボットの構成を示すブロック図である。本実施の形態による産業用ロボットは、制御装置１と、サーボコントローラ２と、マニピュレータ３とを備える。マニピュレータ３は、直列に接続された複数のアーム３１，３２と、その複数のアーム３１，３２の先端に設けられた第１及び第２のハンド部３３，３４とを備えるダブルハンドの水平多関節マニピュレータである。なお、複数のアーム３１，３２と、第１及び第２のハンド部３３，３４とは、モータによって駆動される関節によって連結されている。そのマニピュレータ３において、各モータとアームやハンド部とは減速器を介して接続されている。また、第１及び第２のハンド部３３，３４は、アーム３２の一端に、それぞれ独立して回動可能なように設けられているものとする。サーボコントローラ２は、制御装置１による制御に応じて、マニピュレータ３の各軸のモータを制御する。制御装置１は、ティーチングプレイバック方式により、後述する教示情報に応じてマニピュレータ３が動作するようにサーボコントローラ２を制御する。本実施の形態による産業用ロボットは、例えば、搬送ロボットであり、第１及び第２のハンド部３３，３４が保持するウェハやガラス基板等の搬送対象を収容装置等に搬入したり、収容装置等から搬出したりしてもよい。その収容装置は、搬送対象が少なくとも搬入されたり、搬出されたりする装置であり、例えば、搬送対象が収容されるストッカやカセット等であってもよく、または、搬送対象に対して何らかの処理やプロセスを実行する処理装置であってもよい。また、ハンド部による搬送対象の保持は、ハンド部に搬送対象が載置されることであってもよく、ハンド部が搬送対象を挟持することであってもよく、ハンド部が搬送対象を吸着することであってもよく、または、ハンド部による搬送対象のその他の保持であってもよい。なお、本実施の形態では、マニピュレータ３が２個のハンド部３３，３４を有する場合について説明するが、そうでなくてもよい。マニピュレータ３は、３個以上のハンド部を有してもよい。また、本実施の形態では、マニピュレータ３が２個のアーム３１，３２を有する場合について説明するが、そうでなくてもよい。マニピュレータ３の有するアームの個数や軸数は問わない。

図１で示されるように、本実施の形態による制御装置１は、教示情報を修正する教示情報修正装置１１と、修正された教示情報を用いて、マニピュレータ３が教示位置を通過するように制御する軌道制御部１２とを備える。また、教示情報修正装置１１は、教示情報記憶部２１と、記憶部２２と、判断部２３と、修正部２４とを備える。

教示情報記憶部２１では、教示情報が記憶される。教示情報は、マニピュレータ３の教示位置を示す情報である。その教示情報には、マニピュレータ３の有する第１及び第２のハンド部３３，３４の各教示位置が少なくとも含まれているものとする。すなわち、その教示情報によって、第１及び第２のハンド部３３，３４の移動経路が時系列に沿って示されることになる。なお、教示情報には、マニピュレータ３のアーム３１，３２の各関節のモータの位置も示されてもよく、または、そうでなくてもよい。後者の場合には、教示情報を用いたマニピュレータ３の制御が行われるよりも前に、またはその制御中に、第１及び第２のハンド部３３，３４の教示位置を用いて、各モータの位置が算出されることになる。

教示情報記憶部２１に教示情報が記憶される過程は問わない。例えば、記録媒体を介して教示情報が教示情報記憶部２１で記憶されるようになってもよく、通信回線等を介して送信された教示情報が教示情報記憶部２１で記憶されるようになってもよく、あるいは、入力デバイスを介して入力された教示情報が教示情報記憶部２１で記憶されるようになってもよい。また、教示情報記憶部２１では、例えば、マニピュレータ３の一連の動作に関するすべての教示位置を示す教示情報が記憶されてもよく、あるいは、その一連の動作に関する一部の教示位置を示す教示情報が記憶されてもよい。後者の場合としては、例えば、後述する判断と修正との処理がリアルタイム処理として実行される際に、一連の動作に関するすべての教示位置を示す教示情報のうち、一部の教示情報のみが順次、読み出されて教示情報記憶部２１に記憶される場合がある。また、教示情報記憶部２１での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。教示情報記憶部２１は、所定の記録媒体（例えば、半導体メモリや磁気ディスク、光ディスクなど）によって実現されうる。

記憶部２２では、判断部２３や修正部２４が用いる退避線を示す情報が記憶される。その退避線は、干渉を防止するために設けられるものである。すなわち、退避線から安全側にハンド部の教示位置がある場合には、そのハンド部と収容装置等との干渉の可能性が低く、退避線よりも非安全側にハンド部の教示位置がある場合には、そのハンド部と収容装置等との干渉の可能性が高くなるように退避線が設定されることが好適である。なお、ハンド部と収容装置等との干渉には、そのハンド部が保持している搬送対象と収容装置等との干渉が含まれてもよい。他の説明においても同様であるとする。退避線の安全側とは、通常、退避線よりもマニピュレータ３の基端に近い側であり、退避線の非安全側とは、通常、退避線よりも収容装置等に近い側であり、安全側の反対側である。ここで、マニピュレータ３の基端とは、連結された複数のアーム３１，３２において、第１及び第２のハンド部３３，３４と反対側の端のことである。また、ハンド部の教示位置が退避線上に存在する場合にも、そのハンド部と収容装置等との干渉の可能性が低くなるように退避線の位置が設定されていることが好適である。その退避線は、通常、直線であるが、そうでなくてもよい。なお、その退避線は、ある水平面において設定される線であるため、マニピュレータ３が鉛直方向に昇降可能である場合には、各高さの水平面において退避線が設定されることが好適である。その退避線は、各水平面において同じであってもよく、または、そうでなくてもよい。そのように、複数の水平面において退避線が設定される場合には、退避面が設定されてもよい。その退避面は、例えば、平面であってもよく、または、曲面であってもよい。そして、ある水平面においてマニピュレータ３が動作する場合には、その水平面と退避面との交線を、その水平面における退避線として用いてもよい。また、その退避線は、例えば、マニピュレータ３が搬送対象を搬出入する収容装置等の搬出入口に対してそれぞれ設定されることが好適である。また、退避線は、収容装置等の搬出入口以外の位置であっても、一方のハンド部が退避位置から伸長位置まで移動する位置に設定されてもよい。なお、退避位置や伸長位置については後述する。また、退避線を示す情報は、例えば、退避線の位置を示す関数であってもよく、または、退避線の位置を示す点の集合等であってもよい。後者の場合には、その点を接続した直線や曲線が退避線となる。また、退避線が退避面と水平面との交線によって示される場合には、記憶部２２において、退避面を示す関数や点の集合等が記憶されていてもよい。

また、記憶部２２において、退避線を示す情報以外の情報が記憶されてもよい。また、記憶部２２に情報が記憶される過程は問わない。例えば、記録媒体を介して情報が記憶部２２で記憶されるようになってもよく、通信回線等を介して送信された情報が記憶部２２で記憶されるようになってもよく、あるいは、入力デバイスを介して入力された情報が記憶部２２で記憶されるようになってもよい。また、記憶部２２での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。また、記憶部２２は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

判断部２３は、第１のハンド部３３が退避位置から伸長位置まで移動した場合に、第２のハンド部３４の教示位置が退避線から安全側の位置であるかどうかを判断する。判断部２３は、教示情報記憶部２１で記憶されている教示情報を用いて、その判断を行う。すなわち、判断部２３は、実際に動作した第１及び第２のハンド部３３，３４を用いて判断するのではなく、教示情報を用いて、その判断を行う。なお、第１のハンド部３３の退避位置から伸長位置までの移動は、例えば、搬送対象を搬入または搬出するために第１のハンド部３３が収容装置等に近づく方向の移動である。したがって、伸長位置は、通常、第１のハンド部３３が搬送対象を収容装置等の内部に設置する位置、または、第１のハンド部３３が搬送対象を収容装置等の内部から取得する位置である。また、退避位置は、その伸長位置に移動する手前の位置、すなわち、通常、収容装置等の搬出入口の外側の位置である。本実施の形態では、その第１のハンド部３３が直線移動する場合において、第１のハンド部３３が前進する前の位置が退避位置であり、前進した後の位置が伸長位置であるとする。図２は、第１のハンド部３３が退避位置から伸長位置まで移動する場合のマニピュレータ３の動作の一例を示す図である。図２において、第１のハンド部３３が退避位置であるマニピュレータ３を破線で示しており、第１のハンド部３３が伸長位置であるマニピュレータ３を実線で示している。なお、図２では、第１のハンド部３３が退避位置から伸長位置に移動する際に、第１及び第２のハンド部３３，３４が搬送対象を保持していない場合について示しているが、第１及び第２のハンド部３３，３４は、搬送対象を保持していてもよい。図３Ａは、図２に対応する第１及び第２のハンド部３３，３４の教示位置３３ａ，３４ａと、ハンドホルダ３６の位置とを示す図である。なお、図３Ａでは、ハンドホルダ３６と、教示位置３３ａ，３４ａとを結ぶ線によって、第１及び第２のハンド部３３，３４を代表させている。他の図においても同様である。また、ハンドホルダ３６は、第１及び第２のハンド部３３，３４を支持するものであり、そのハンドホルダ３６の中心が第１及び第２のハンド部３３，３４の回動中心となる。図３Ａでも、第１のハンド部３３が退避位置である場合が破線で示されており、第１のハンド部３３が伸長位置である場合が実線で示されている。なお、教示位置３３ａ，３４ａは、例えば、図３Ｃで示されるように、第１及び第２のハンド部３３，３４が適切に保持している搬送対象７の中心点に対応する位置であってもよいが、それに限定されるものではない。また、図２、図３Ａでは、図中の退避線よりも下側、すなわち、マニピュレータ３の基端側が退避線に対する安全側となる。

判断部２３は、通常、上述した判断を２つの判断処理によって行う。まず、判断部２３は、教示情報記憶部２１で記憶されている教示情報を用いて、第１のハンド部３３が、退避位置から伸長位置まで移動したかどうかを判断する。例えば、判断部２３は、ある時点における第１のハンド部３３の教示位置が収容装置等の内部に存在する場合に、第１のハンド部３３が退避位置から伸長位置まで移動したと判断してもよい。なお、その場合には、例えば、図２で示されるように、収容装置６等の内部に存在する教示位置が伸長位置となり、その教示位置よりも１個前の教示位置が退避位置となる。次に、判断部２３は、第１のハンド部３３の伸長位置である教示位置と同じ時点の第２のハンド部３４の教示位置が、退避線から安全側の位置であるかどうか判断する。その退避線は、記憶部２２で記憶されている情報で示される退避線である。なお、本実施の形態では、「退避線から安全側の位置」は、退避線上を含むものとする。したがって、その第２のハンド部３４の教示位置が退避線上に位置する場合にも、判断部２３は、退避線から安全側の位置であると判断するものとする。例えば、図３Ａで示されるように、第２のハンド部３４の教示位置３４ａが退避線よりも非安全側に存在する場合には、判断部２３は、第２のハンド部３４の教示位置が退避線から安全側の位置でないと判断する。

修正部２４は、第２のハンド部３４の教示位置が退避線から安全側の位置でないと判断部２３が判断した場合に、その第２のハンド部３４の教示位置が、退避線から安全側の位置となるように教示情報を修正する。なお、修正部２４は、その修正時に、伸長位置である第１のハンド部３３の教示位置が変更されないように、第２のハンド部３４の教示位置を修正するものとする。その第２のハンド部３４は、前述のように、第１のハンド部３３と同軸の回動中心を有している。したがって、第１のハンド部３３の教示位置を変更しないで第２のハンド部３４の教示位置を修正するためには、第２のハンド部３４を回動させることしかできない。そのため、修正部２４は、回動軸を中心に第２のハンド部３４を回動させることによって、第２のハンド部３４の教示位置が退避線から安全側の位置となるように教示情報を修正するものとする。なお、「退避線から安全側の位置」は、前述のように、退避線上を含んでいる。したがって、教示位置の修正を行う場合に、修正部２４は、第２のハンド部３４の修正後の教示位置が、退避線上となるように教示情報を修正してもよく、または、退避線よりも安全側の位置となるように教示情報を修正してもよい。なお、第２のハンド部３４の教示位置が退避線よりも安全側の位置となるように教示情報が修正される場合には、修正後の教示位置が、あらかじめ設定されていた教示位置から遠くなり、想定外の干渉が発生する可能性もある。そのため、修正後の教示位置は、退避線上であることが好適である。本実施の形態では、修正部２４が、修正後の第２のハンド部３４の教示位置が退避線上となるように教示情報を修正する場合について説明する。また、第２のハンド部３４の修正後の教示位置が退避線上である場合でも、修正先の教示位置が退避線上に２点存在することが一般的である。回動軸を中心に第２のハンド部３４を回動させた場合に、退避線と、第２のハンド部３４の教示位置３４ａとの交点は、通常、２点あるからである。そのため、修正部２４は、第２のハンド部３４の教示位置が退避線から安全側の位置でないと判断部２３が判断した場合であり、第２のハンド部３４の退避線上の修正先の教示位置が２個ある場合には、第２のハンド部３４の修正後の教示位置が、２個の修正先の教示位置のうち、修正前の教示位置から近い方となるように教示情報を修正してもよい。そのようにすることで、修正後の教示位置が修正前の教示位置に近くなるように教示情報を修正することができ、その結果として、想定外の干渉の発生を低減させることができ、また、その修正後の教示位置からの第２のハンド部３４の移動に関する問題の発生も低減させることができる。例えば、修正前の第２のハンド部３４の位置が図３Ｂの破線で示される場合には、修正部２４は、図中の矢印Ｂで示されるように、第２のハンド部３４の教示位置３４ａが退避線上となるように教示情報を修正してもよい。

なお、ここでは第１のハンド部３３が伸長位置に移動した際の、第２のハンド部３４の教示位置について判断を行い、必要であれば第２のハンド部３４の教示位置の修正を行う場合について説明したが、第２のハンド部３４が伸長位置に移動した際の、第１のハンド部３３の教示位置についても同様の判断を行い、必要であれば第１のハンド部３３の教示位置の修正を行ってもよい。また、この判断と修正の処理は、教示情報記憶部２１で記憶されている教示情報についてバッチ処理として行ってもよく、または、軌道制御部１２が教示情報を順次、読み出してサーボコントローラ２の制御を行っている際に、リアルタイム処理として行ってもよい。本実施の形態では、前者の場合、すなわち、バッチ処理として判断と修正との処理を行う場合について主に説明する。なお、判断部２３による判断と、修正部２４による修正との具体的な処理については、後述する。

軌道制御部１２は、教示情報修正装置１１が修正した教示情報を用いて、サーボコントローラ２を制御する。すなわち、軌道制御部１２は、第１及び第２のハンド部３３，３４の位置が、修正後の教示情報の示す教示位置となるようにサーボコントローラ２を制御する。なお、軌道制御部１２は、教示情報によって示される教示位置の間を補間した軌道をマニピュレータ３の各軸が通過するように、サーボコントローラ２を制御してもよい。また、教示情報にマニピュレータ３の第１及び第２のハンド部３３，３４のみの教示位置が含まれている場合には、軌道制御部１２は、その教示位置を用いて各軸の位置を算出してもよい。軌道制御部１２による処理は、従来のロボットにおけるサーボコントローラの制御と同じであり、その詳細な説明を省略する。

なお、教示情報記憶部２１と、記憶部２２とは、同一の記録媒体によって実現されてもよく、あるいは、別々の記録媒体によって実現されてもよい。前者の場合には、教示情報を記憶している領域が教示情報記憶部２１となり、退避線を示す情報を記憶している領域が記憶部２２となる。

ここで、サーボコントローラ２について簡単に説明する。サーボコントローラ２は、制御装置１から、マニピュレータ３の各軸の位置の指令を受け取る。サーボコントローラ２は、その各軸の位置の指令に応じて、フィードバック制御により各軸のモータに対応するトルク指令値を生成する。マニピュレータ３の各モータは、このトルク指令値によって動作する。なお、このサーボコントローラ２によるフィードバック制御はすでに公知であり、その詳細な説明を省略する。

次に、判断部２３による判断と、修正部２４による修正との具体的な処理について、図４を用いて説明する。図４において、図中の下側の第１及び第２のハンド部３３，３４は、第１のハンド部３３が退避位置である場合の位置である。また、図中の上側の第１及び第２のハンド部３３，３４は、第１のハンド部３３が伸長位置である場合の位置である。また、修正後の第２のハンド部３４の位置が破線で示されている。また、図４において、Ｘ軸は、第１のハンド部３３が退避位置から伸長位置まで移動する際の移動方向と一致するように設定されている。すなわち、Ｘ軸は図中の矢印Ａと同じ方向に設定されている。また、Ｙ軸は、Ｘ軸に直交するように設定されている。また、記憶部２２では、退避線を示す関数
Ｘ＝ａ＋ｂＹ
が記憶されているものとする。なお、ａ，ｂは、それぞれ実数である。また、第１のハンド部３３が伸長位置である際の第２のハンド部３４の教示位置をＥ_ａとし、その教示位置Ｅ_ａの座標を（Ｘ_ａ，Ｙ_ａ）とする。この教示位置は、教示情報に含まれている。また、修正後の第２のハンド部３４の教示位置をＥ_ａ'とし、その教示位置Ｅ_ａ'の座標を（Ｘ_ａ'，Ｙ_ａ'）とする。また、第２のハンド部３４の回動中心から教示位置までの長さをＬ_ａとする。また、第１のハンド部３３が伸長位置である際の第１及び第２のハンド部３３，３４の回動中心の位置をＥ_ｈとし、その教示位置Ｅ_ｈの座標を（Ｘ_ｈ，Ｙ_ｈ）とする。なお、（Ｘ_ｈ，Ｙ_ｈ）は、教示情報に含まれていてもよく、または、教示情報に含まれている第１及び第２のハンド部３３，３４の教示位置から算出されてもよい。また、Ｅ_ｈと退避線との距離をＬ_ｈとする。また、退避位置における第１のハンド部３３から第２のハンド部３４までの角度をθ_ｒａとし、伸長位置における第１のハンド部３３から第２のハンド部３４までの角度をθ_ｅａとする。なお、各角度は、反時計回りが正であり、時計回りが負であるとする。すなわち、図４の場合には、θ_ｒａ、θ_ｅａはともに正の値である。ここで、θ_ｒａは、
−１８０°＜θ_ｒａ≦１８０°
であるとする。

判断部２３は、記憶部２２から退避線の式を読み出し、それに第２のハンド部３４の教示位置をＥ_ａの座標（Ｘ_ａ，Ｙ_ａ）を代入し、
Ｘ_ａ＞ａ＋ｂＹ_ａ （１）
がみたされるかどうか判断する。そして、その（１）式がみたされる場合には、判断部２３は、第２のハンド部３４の教示位置が退避線より非安全側であると判断する。一方、
Ｘ_ａ≦ａ＋ｂＹ_ａ （２）
がみたされる場合には、判断部２３は、第２のハンド部３４の教示位置が退避線から安全側の位置に存在すると判断する。なお、判断部２３は、上記（１）式がみたされない場合に、第２のハンド部３４の教示位置が退避線から安全側の位置に存在すると判断してもよく、上記（２）式がみたされない場合に、第２のハンド部３４の教示位置が退避線より非安全側であると判断してもよい。

修正部２４は、第２のハンド部３４の教示位置が退避線から安全側の位置に存在する場合には、教示情報の修正の処理を行わない。一方、第２のハンド部３４の教示位置が退避線より非安全側であると判断された場合には、修正部２４は、修正先の教示位置を決定する。まず、修正部２４は、Ｅ_ｈの座標（Ｘ_ｈ，Ｙ_ｈ）と退避線の式Ｘ＝ａ＋ｂＹとを用いてＬ_ｈを計算し、そのＬ_ｈとＬ_ａとの大小関係に応じて、次のように修正先の教示位置を決定する。

１．Ｌ_ｈ＞Ｌ_ａの場合
この場合には、第２のハンド部３４の教示位置を退避線から安全側の位置に修正することはできない。したがって、エラーとする。

２．Ｌ_ｈ＝Ｌ_ａの場合
この場合には、Ｅ_ｈを中心とする半径Ｌ_ａの円は、退避線と１点で交わる。したがって、その交点が第２のハンド部３４の修正後の教示位置Ｅ_ａ'となる。

３．Ｌ_ｈ＜Ｌ_ａの場合
この場合には、Ｅ_ｈを中心とする半径Ｌ_ａの円は、退避線と２点で交わる。その２点のうち、修正前の教示位置Ｅ_ａを、θ_ｒａの符号と同じ方向にＥ_ｈを中心として回転させた場合に、修正前の教示位置Ｅ_ａに近い方の点が第２のハンド部３４の修正後の教示位置Ｅ_ａ'となる。なお、０°は、＋０°である、すなわち正の符号であるとしてもよく、または、そうでなくてもよい。

ここで、退避線の式がＸ＝ａ（すなわち、ｂ＝０）であり、Ｘ_ａ＞ａ（すなわち、上記（１）式がみたされる）であり、Ｌ_ｈ＜Ｌ_ａである場合に、修正部２４が教示位置Ｅ_ａ'の座標（Ｘ_ａ'，Ｙ_ａ'）を算出する方法について説明する。また、説明の便宜上、０°＜θ_ｒａ≦１８０°であり、０°＜θ_ｅａ≦９０°であるとする。

上述した前提から、上記「３．Ｌ_ｈ＜Ｌ_ａの場合」となり、Ｅ_ｈを中心とする半径Ｌ_ａの円は、退避線と２点で交わる。その２点を、Ａ_１（Ｘ_ａ１，Ｙ_ａ１）、Ａ_２（Ｘ_ａ２，Ｙ_ａ２）とする。また、線分Ｅ_ａＥ_ｈと線分Ａ_１Ｅ_ｈとのなす角度をθ_１とし、線分Ｅ_ａＥ_ｈと線分Ａ_２Ｅ_ｈとのなす角度をθ_２とする。また、−１８０°＜θ_１≦１８０°、−１８０°＜θ_２≦１８０°とすると、θ_１，θ_２は次のようになる。

なお、Ａ_１、Ａ_２は、Ｅ_ｈを中心とする半径Ｌ_ａの円と退避線との交点であるため、Ｘ＝ａ、及び（Ｘ−Ｘ_ｈ）^２＋（Ｙ−Ｙ_ｈ）^２＝Ｌ_ａ ^２を解くことによって求められる。また、０°＜θ_ｅａ≦９０°であるため、Ｙ_ａ１≦２Ｙ_ｈ−Ｙ_ａの場合には、θ_１＞０となり、そうでない場合には、θ_１＜０となる。また、同様にして、Ｙ_ａ２≦２Ｙ_ｈ−Ｙ_ａの場合には、θ_２＞０となり、そうでない場合には、θ_２＜０となる。なお、θ_１，θ_２が負である場合には、３６０°を加算して正の値にする。そして、修正部２４は、θ_１＜θ_２であれば、Ａ_１を修正後の教示位置Ｅ_ａ'とし、教示情報における修正前の教示位置Ｅ_ａを、その修正後の教示位置Ｅ_ａ'に修正する。また、修正部２４は、θ_１＞θ_２であれば、Ａ_２を修正後の教示位置Ｅ_ａ'とし、教示情報における修正前の教示位置Ｅ_ａを、その修正後の教示位置Ｅ_ａ'に修正する。なお、上記θ_１，θ_２の式が記憶部２２で記憶されており、修正部２４は、それを読み出して用いることによって、上述の計算等を行ってもよい。また、ここでは説明の便宜上、ｂ＝０、０°＜θ_ｒａ≦１８０°、０°＜θ_ｅａ≦９０°の場合について説明したが、それ以外の場合についても、同様にして修正前の教示位置Ｅ_ａに近い修正後の教示位置Ｅ_ａ'を算出することができる。

次に、制御装置１の動作について図５のフローチャートを用いて説明する。
（ステップＳ１０１）判断部２３は、カウンタｉを１に設定する。

（ステップＳ１０２）判断部２３は、教示情報記憶部２１で記憶されている教示情報を参照し、第１のハンド部３３または第２のハンド部３４のｉ番目の教示位置が伸長位置であるかどうか判断する。そして、伸長位置である場合には、ステップＳ１０３に進み、そうでない場合には、ステップＳ１０５に進む。なお、第１のハンド部３３のｉ番目の教示位置と、第２のハンド部３４のｉ番目の教示位置とは、同時点における教示位置であるとする。

（ステップＳ１０３）判断部２３は、教示情報記憶部２１で記憶されている教示情報を参照し、伸長位置でない第１のハンド部３３または第２のハンド部３４のｉ番目の教示位置が退避線から安全側の位置であるかどうか判断する。そして、安全側である場合には、ステップＳ１０５に進み、そうでない場合には、ステップＳ１０４に進む。

（ステップＳ１０４）修正部２４は、伸長位置でない第１のハンド部３３または第２のハンド部３４のｉ番目の教示位置が、退避線から安全側の位置となるように教示情報を修正する。

（ステップＳ１０５）判断部２３は、カウンタｉを１だけインクリメントする。

（ステップＳ１０６）判断部２３は、教示情報にｉ番目の教示位置が存在するかどうか判断する。そして、ｉ番目の教示位置が存在する場合には、ステップＳ１０２に戻り、そうでない場合には、ステップＳ１０７に進む。

（ステップＳ１０７）軌道制御部１２は、修正後の教示情報を用いて、マニピュレータ３の各モータの制御を行うサーボコントローラ２を制御する。具体的には、軌道制御部１２は、補間周期ごとに第１及び第２のハンド部３３，３４を回動させる各軸の位置や、複数のアーム３１，３２の先端の位置（例えば、ハンドホルダ３６の位置等）を算出する。また、軌道制御部１２は、その算出した位置に対して逆キネマティクス処理を行うことによって、複数のアーム３１，３２の各軸の位置を算出する。そして、軌道制御部１２は、その補間周期ごとに各軸の位置を示す位置指令値をサーボコントローラ２に出力する。その結果、マニピュレータ３は、サーボコントローラ２による制御によって、教示情報が示す教示位置を通過するように順次、動作する。その動作において、修正後の教示情報に応じた動作が行われるため、第１のハンド部３３や第２のハンド部３４と収容装置等との干渉の発生を防止することができる。

なお、図５のフローチャートでは、バッチ処理として教示情報の修正を行った上で、軌道の制御を行う場合について説明したが、前述のように、軌道の制御時に、リアルタイム処理として教示情報の修正を行ってもよいことは言うまでもない。その場合には、軌道の制御に教示位置を用いる直前に、ステップＳ１０２〜Ｓ１０４の処理を行ってもよい。

次に、本実施の形態による制御装置１の動作について、具体例を用いて説明する。この具体例において、教示情報記憶部２１に、次の教示情報が記憶されているものとする。
Ｐｏｓｉｔｉｏｎ（１−１）、Ｐｏｓｉｔｉｏｎ（２−１）
Ｐｏｓｉｔｉｏｎ（１−２）、Ｐｏｓｉｔｉｏｎ（２−２）
：
：
Ｐｏｓｉｔｉｏｎ（１−７）、Ｐｏｓｉｔｉｏｎ（２−７）
Ｐｏｓｉｔｉｏｎ（１−８）、Ｐｏｓｉｔｉｏｎ（２−８）
Ｐｏｓｉｔｉｏｎ（１−９）、Ｐｏｓｉｔｉｏｎ（２−９）
：
：

この教示情報において、Ｐｏｓｉｔｉｏｎ（１−Ｋ）は、第１のハンド部３３のＫ番目の教示位置を示す情報であり、Ｐｏｓｉｔｉｏｎ（２−Ｋ）は、第２のハンド部３４のＫ番目の教示位置を示す情報である。なお、Ｋは、１以上の整数であり、教示情報における各ステップに対応している。なお、教示情報には、ハンドホルダ３６の教示位置Ｐｏｓｉｔｉｏｎ（３−Ｋ）や、第１及び第２のハンド部３３，３４の姿勢を示す情報Ｐｏｓｅ（１−Ｋ）、Ｐｏｓｅ（２−Ｋ）等が含まれていてもよいが、ここでは説明の便宜のため省略している。また、Ｐｏｓｉｔｉｏｎ（１−７）、Ｐｏｓｉｔｉｏｎ（２−７）は、図３Ａの破線で示される第１及び第２のハンド部３３，３４の教示位置に対応し、Ｐｏｓｉｔｉｏｎ（１−８）、Ｐｏｓｉｔｉｏｎ（２−８）は、図３Ａの実線で示される第１及び第２のハンド部３３，３４の教示位置に対応するものとする。

判断部２３は、Ｐｏｓｉｔｉｏｎ（１−１）、Ｐｏｓｉｔｉｏｎ（２−１）から順次、教示位置を読み出して、どちらかのＰｏｓｉｔｉｏｎが伸長位置かどうか判断する（ステップＳ１０１，Ｓ１０２）。なお、伸長位置かどうかの判断に１個前のＰｏｓｉｔｉｏｎを用いてもよく、あるいは、用いなくてもよい。前者の場合には、判断部２３は、そのＰｏｓｉｔｉｏｎをも読み出して用いてもよい。なお、Ｋ＝１〜７のＰｏｓｉｔｉｏｎ（１−Ｋ）、Ｐｏｓｉｔｉｏｎ（２−Ｋ）に伸長位置は含まれていなかったとする。そのため、Ｋ＝７までは、教示位置が伸長位置かどうかの判断のみが繰り返し実行される（ステップＳ１０２，Ｓ１０５，Ｓ１０６）。

その後、判断部２３は、Ｐｏｓｉｔｉｏｎ（１−８）、Ｐｏｓｉｔｉｏｎ（２−８）を読み出し、どちらかが伸長位置であるかどうか判断する（ステップＳ１０２）。この場合には、前述のように、Ｐｏｓｉｔｉｏｎ（１−８）が伸長位置であるため、判断部２３は、伸長位置でない方の教示位置Ｐｏｓｉｔｉｏｎ（２−８）が安全側であるかどうか判断する（ステップＳ１０３）。この場合には、前述のように、Ｐｏｓｉｔｉｏｎ（２−８）は非安全側に存在するため、判断部２３は、Ｐｏｓｉｔｉｏｎ（２−８）を修正する旨の指示を修正部２４に渡す。修正部２４は、その指示を受け取ると、前述のようにして、修正後の教示位置Ｐｏｓｉｔｉｏｎ（２−８'）を生成し、教示情報記憶部２１で記憶されているＰｏｓｉｔｉｏｎ（２−８）に上書きで蓄積する（ステップＳ１０４）。その結果、教示情報記憶部２１で記憶されている教示情報は，次のようになり、図３Ｂで示されるように、第２のハンド部３４の教示位置が退避線上に移動される。
：
：
Ｐｏｓｉｔｉｏｎ（１−７）、Ｐｏｓｉｔｉｏｎ（２−７）
Ｐｏｓｉｔｉｏｎ（１−８）、Ｐｏｓｉｔｉｏｎ（２−８'）
：
：

なお、このような処理が順次、繰り返されることによって、教示情報の修正が行われる（ステップＳ１０２〜Ｓ１０６）。そして、教示情報の修正が終了すると、軌道制御部１２は、教示情報記憶部２１から教示位置を順次、読み出し、補間周期ごとの各軸の位置を計算してサーボコントローラ２に出力する（ステップＳ１０７）。その結果、マニピュレータ３が修正後の教示情報に応じて動作することになる。

なお、この具体例では、修正部２４が修正後の教示位置を上書きで蓄積する場合について説明したが、そうでなくてもよい。修正部２４は、修正後の教示位置を上書きでなく蓄積してもよい。その場合には、教示情報記憶部２１において、修正後の教示位置と、修正前の教示位置とが区別可能なようになっていることが好適である。軌道制御部１２が、修正後の教示位置を読み出すことができるようにするためである。

以上のように、本実施の形態による制御装置１によれば、第１及び第２のハンド部３３，３４のうち、搬送を行うハンド部が伸長位置である際に、退避を行うハンド部が非安全側に存在する場合には、その非安全側に存在するハンド部の教示位置が安全側となるように修正することができる。そのため、その退避を行うハンド部の干渉の発生を防止することができる。また、その教示位置の修正を行う場合に、修正前の教示位置から近い退避線上の位置を修正後の教示位置とすることによって、教示位置の移動を最小限に抑えることができ、別の干渉が発生する可能性を低減させることができる。また、教示位置の移動を最小限に抑えることは、種々の要因を考慮して作成された教示情報をできる限り変更しないようにするという観点からも好適である。

なお、本実施の形態において、マニピュレータ３が３個以上のハンド部を有する場合には、判断部２３は、搬送を行うハンド部が退避位置から伸長位置まで移動した際に、他の２以上の退避を行うハンド部の教示位置が退避線から安全側の位置であるかどうかを、教示情報を用いてそれぞれ判断する。そして、非安全側に存在する教示位置がある場合には、修正部２４は、その教示位置が、退避線から安全側の位置となるように修正する。

また、本実施の形態では、「退避線から安全側の位置」が、退避線上を含む場合について説明したが、そうでなくてもよい。すなわち、退避線上は安全側でなくてもよい。例えば、教示位置を修正する際に、修正部２４が、退避線よりも安全側の位置であって、退避線上ではない位置に教示位置を修正する場合には、「退避線から安全側の位置」が、退避線上を含んでいなくてもよい。

また、本実施の形態では、第１及び第２のハンド部３３，３４のうち、一方が伸長位置である際に、他方の教示位置が退避線より非安全側に位置する場合には、その教示位置の修正を行う場合について説明したが、そうでなくてもよい。退避線よりも非安全側に境界線をさらに設け、教示位置が境界線と退避線との間に位置する場合には、教示位置の修正を行わないようにしてもよい。そのような教示情報修正装置１１について簡単に説明する。記憶部２２において、境界線を示す情報も記憶されているものとする。境界線は、図６Ａ，図６Ｂで示されるように、退避線よりも非安全側に設定される。境界線は、ハンド部の干渉が発生するかどうかの境界となる線である。したがって、通常、境界線よりも非安全側にハンド部が位置する場合には、干渉が発生することになる。すなわち、干渉が発生するかどうかの観点からは、教示位置が退避線から安全側の位置であると非常に安全であり、教示位置が境界線と退避線との間の位置であるとやや安全であり、教示位置が境界線の非安全側の位置であると危険であるということになる。なお、境界線は、退避線と独立して設定されてもよく、または、そうでなくてもよい。後者の場合には、例えば、退避線と境界線とは平行であり、退避線よりもあらかじめ決められた距離だけ非安全側の位置に境界線が設定されてもよい。また、境界線の安全側とは、境界線の退避線側であり、境界線の非安全側とは、その安全側の反対側である。境界線は、設定される位置が異なる以外は、退避線と同様のものであるとする。次に、判断部２３と修正部２４との処理について説明する。判断部２３は、第１のハンド部３３が退避位置から伸長位置まで移動した場合に、第２のハンド部３４の教示位置が、境界線の安全側であるかどうかをも教示情報を用いて判断する。そして、修正部２４は、第２のハンド部３４の教示位置が境界線と退避線との間の位置であると判断部２３が判断した場合には、修正を行わない。なお、ここでは第１のハンド部３３が伸長位置に移動した場合に、第２のハンド部３４の教示位置について判断を行い、必要であれば第２のハンド部３４の教示位置の修正を行う場合について説明したが、第１及び第２のハンド部３３，３４が逆の場合にも同様である。また、ここで説明した以外の処理は、前述の通りである。すなわち、第１及び第２のハンド部３３，３４のうち、一方が伸長位置である際に、他方の教示位置が境界線より非安全側に位置すると判断部２３が判断した場合には、修正部２４は、その教示位置が退避線から安全側の位置となるように教示情報の修正を行うことになる。なお、教示位置が境界線より非安全側である場合には、その教示位置は、当然、退避線より非安全側に存在していることになる。ここで、境界線も用いて判断や修正を行う場合の具体的な処理について説明する。図６Ａ，図６Ｂにおいて、図中の下方の第１及び第２のハンド部３３，３４が、図中の上方の第１及び第２のハンド部３３，３４まで、矢印Ａで示されるように移動したとする。その移動によって第１のハンド部３３が退避位置から伸長位置まで移動したとすると、判断部２３は、第２のハンド部３４の教示位置３４ａと、退避線及び境界線との位置関係を判断する。図６Ａの場合には、教示位置３４ａは、退避線より非安全側であるが、境界線より安全側であるため、教示位置３４ａの修正は行われない。一方、図６Ｂの場合には、教示位置３４ａは、退避線より非安全側であり、境界線より非安全側であるため、教示位置３４ａは、破線の第２のハンド部３４で示される退避線上の位置に修正される。このように境界線と、退避線とを用いた教示位置の修正を行う理由について簡単に説明する。前述のように、教示情報は種々の要因を考慮して作成されたものであるため、できるだけ変更しない方がよい。そのため、干渉がぎりぎり発生しない場合には、教示位置を修正しないことが好適である。一方、教示位置を修正する場合には、干渉がぎりぎり発生しない位置に修正するのではなく、少しの余裕を考慮した位置に修正することが好適である。したがって、上述の説明のように、教示位置が境界線の安全側である場合には、修正部２４は、その教示位置を修正せず、教示位置が境界線の非安全側である場合には、修正部２４は、その教示位置を退避線から安全側の位置に修正する。なお、伸長位置でないハンド部の教示位置が境界線上に存在する場合には、その教示位置が退避線から安全側の位置に修正されてもよく、あるいは、修正されなくてもよい。後者の場合には、ハンド部の教示位置が境界線上に存在する場合には、干渉が起きないように境界線が設定されていることが好適である。ここで、教示位置が境界線と退避線との間の位置であり、教示情報の修正を行わなかった場合には、その教示位置は、退避線よりも非安全側に存在することになる。したがって、例えば、教示位置が退避線の非安全側であると停止するロボットの場合には、境界線と退避線との間に位置する教示位置についてのみ、教示位置が退避線上となるように退避線を移動させてもよく、あるいは、その教示位置についてのみ、教示位置が退避線の非安全側であってもロボットを停止させないように制御してもよい。なお、退避線を移動させる場合には、例えば、移動前の退避線と移動後の退避線とが平行になるように移動させてもよい。

また、本実施の形態では、マニピュレータ３が有する第１及び第２のハンド部３３，３４の回動中心が同軸上にある場合について説明したが、そうでなくてもよい。図７で示されるように、第１及び第２のハンド部３３，３４の回動中心は、それぞれ異なる軸上にあってもよい。マニピュレータ３が３個以上のハンド部を有する場合にも同様である。なお、２以上のハンド部の回動中心が異なる軸上にあったとしても、それらの軸は通常、直列に接続された複数のアーム３１，３２の先端に存在する。また、２以上のハンド部の回動中心が異なる軸上にあり、いずれかのハンド部が退避位置から伸長位置まで移動した場合に、他のハンド部の教示位置が退避線の非安全側や境界線の非安全側であるときには、その教示位置が修正される。その教示位置の修正は、前述のように、伸長位置にあるハンド部が移動しないように行われることが好適である。なお、２以上のハンド部の回動中心が異なる軸上にある場合には、伸長位置にあるハンド部が移動しないように他のハンド部の教示位置を修正する際に、連結されたアーム３１，３２を移動させてもよく、あるいは、移動させなくてもよいが、教示情報をできるだけ変更したくないという考えからは、後者の方が好適である。

また、本実施の形態では、制御装置１が教示情報修正装置１１を含んでいる場合について説明したが、そうでなくてもよい。教示情報修正装置１１と、マニピュレータ３を制御する制御装置とは、異なる装置であってもよい。その場合には、例えば、教示情報修正装置１１は、修正後の教示情報を教示情報記憶部２１に蓄積してもよく、または、修正後の教示情報を、マニピュレータ３を制御する制御装置に渡してもよい。前者の場合には、教示情報記憶部２１で記憶されている修正後の教示情報が、マニピュレータ３を制御する制御装置に渡されてもよい。このように、教示情報修正装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合には、教示情報修正装置１１は、修正後の教示情報を、直接的にまたは間接的に制御装置に渡す図示しない出力部を備えていてもよい。また、教示情報修正装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合には、教示情報修正装置１１の有する教示情報記憶部２１は、制御装置で記憶されている教示情報が一時的に記憶される記憶部であってもよく、あるいは、そうでなくてもよい。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、線、面等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値、線、面等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。また、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、そのプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による教示情報修正装置等によれば、干渉の発生を防止することができ、水平多関節マニピュレータの教示情報を修正する装置等として有用である。

１ 制御装置
２ サーボコントローラ
３ マニピュレータ
１１ 教示情報修正装置
１２ 軌道制御部
２１ 教示情報記憶部
２２ 記憶部
２３ 判断部
２４ 修正部
３１、３２ アーム
３３ 第１のハンド部
３４ 第２のハンド部

Claims (5)

1. モータによって駆動される関節によって連結された複数のアームと当該複数のアームの先端に設けられた第１及び第２のハンド部とを有する水平多関節マニピュレータの当該第１及び第２のハンド部の各教示位置を示す教示情報が記憶される教示情報記憶部と、
   前記第１のハンド部が退避位置から伸長位置まで移動した場合に、前記第２のハンド部の教示位置が、当該第２のハンド部の干渉を防止するためにあらかじめ設定された退避線から安全側の位置であるかどうかを、前記教示情報を用いて判断する判断部と、
   前記第２のハンド部の教示位置が前記退避線から安全側の位置でないと前記判断部が判断した場合に、当該第２のハンド部の教示位置が、前記退避線から安全側の位置となるように前記教示情報を修正する修正部と、を備え、
   前記判断部は、前記第１のハンド部が退避位置から伸長位置まで移動した場合に、前記第２のハンド部の教示位置が、前記第２のハンド部の干渉が発生するかどうかの境界となる境界線の安全側であるかどうかをも判断し、
   前記修正部は、前記第２のハンド部の教示位置が境界線と退避線との間の位置であると前記判断部が判断した場合には、前記修正を行わない、教示情報修正装置。
2. 前記修正部は、前記教示情報を修正する場合に、前記第２のハンド部の修正後の教示位置が前記退避線上となるように前記教示情報を修正する、請求項１記載の教示情報修正装置。
3. 前記修正部は、前記教示情報を修正する場合であり、前記第２のハンド部の前記退避線上の修正先の教示位置が２個ある場合には、前記第２のハンド部の修正後の教示位置が、当該２個の修正先の教示位置のうち、修正前の教示位置から近い方となるように前記教示情報を修正する、請求項２記載の教示情報修正装置。
4. 請求項１から請求項３のいずれか記載の教示情報修正装置と、
   前記教示情報修正装置が修正した教示情報を用いて、前記モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えた制御装置。
5. モータによって駆動される関節によって連結された複数のアームと当該複数のアームの先端に設けられた第１及び第２のハンド部とを有する水平多関節マニピュレータの当該第１及び第２のハンド部の各教示位置を示す教示情報が記憶される教示情報記憶部で記憶されている教示情報を用いて、前記第１のハンド部が退避位置から伸長位置まで移動した場合に、前記第２のハンド部の教示位置が、当該第２のハンド部の干渉を防止するためにあらかじめ設定された退避線から安全側の位置であるかどうかを判断する判断ステップと、
   前記第２のハンド部の教示位置が前記退避線から安全側の位置でないと前記判断部が判断した場合に、当該第２のハンド部の教示位置が、前記退避線から安全側の位置となるように前記教示情報を修正する修正ステップと、を備え、
   前記判断ステップでは、前記第１のハンド部が退避位置から伸長位置まで移動した場合に、前記第２のハンド部の教示位置が、前記第２のハンド部の干渉が発生するかどうかの境界となる境界線の安全側であるかどうかをも判断し、
   前記修正ステップでは、前記第２のハンド部の教示位置が境界線と退避線との間の位置であると前記判断ステップで判断された場合には、前記修正を行わない、教示情報修正方法。

Images