JP6023517B2

JP6023517B2 - 算出装置、制御装置、及び算出方法

Info

Publication number: JP6023517B2
Application number: JP2012195738A
Authority: JP
Inventors: 光介上田; 英利神谷
Original assignee: Daihen Corp
Current assignee: Daihen Corp
Priority date: 2012-09-06
Filing date: 2012-09-06
Publication date: 2016-11-09
Anticipated expiration: 2032-09-06
Also published as: JP2014050902A

Description

本発明は、３次元形状を用いて内回り開始位置を算出する算出装置等に関する。

従来、産業用ロボットにおいて、ある教示点と、その前後の教示点からなる２個の軌道を合成し、内回りする軌道を生成することが行われている（例えば、特許文献１参照）。そのような内回り軌道を用いる利点は、次の通りである。ある教示点において、移動方向と速さが不連続に変化した場合には、その教示点で大きな加速度が発生する。その結果、振動が引き起こされ、所要の精度が得られなかったり、無理な力が加わることによってウェハやガラス基板等の搬送対象や機械本体の破損を招いたりする。そのような破損等を回避するため、その教示点において速度が０になるようにすると、移動に余分な時間がかかることになる。一方、内回り軌道を用いることによって、それらの問題を解決することができる。

特開平８−１２３５３１号公報

しかしながら、内回り軌道の開始点を適切に設定できないという問題があった。例えば、Ａ点からＢ点までの直線軌道と、Ｂ点からＣ点までの直線軌道とが教示されており、Ｂ点について内回り軌道を用いる場合には、Ｂ点から内回り開始位置までの距離である内回り開始距離を設定することが行われていた。そのような場合に、すべての内回り軌道において同じ内回り開始距離を設定した場合には、干渉の起こる可能性がある。例えば、ウェハやガラス基板等の搬送対象の搬送先となるスロットは、入口の横幅に対して縦幅が小さくなっている。したがって、図１０Ａで示されるように、縦方向に移動する場合であって、内回り開始距離が長い場合には、ハンド３ａに載置された搬送対象５０がスロット５２と干渉する可能性がある。一方、そのような干渉が起こらないように内回り開始距離を短くすると、図１０Ｂで示されるように、横方向に移動する場合には、内回り軌道が必要以上に小さくなり、内回りによるタクトタイム短縮の効果が得られなくなる。すなわち、画一的な内回り開始距離の設定を行っていた場合には、干渉が発生しうるという問題があり、また、干渉を防止するためには移動時間が長くなるという問題があった。

本発明は、上記課題を解決するためになされたものであり、内回り軌道を用いる場合に、干渉を防止すると共に、移動時間の長期化を防止できる装置等を提供することを目的とする。

上記目的を達成するため、本発明による算出装置は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの２以上の教示点を示す教示情報が記憶される教示情報記憶部と、基準点の設定された３次元形状であり、第１の方向の長さ、及び第１の方向と異なる方向である第２の方向の長さを独立して設定可能な３次元形状である位置算出用形状を示す形状情報が記憶される形状情報記憶部と、教示情報によって示される隣接する２個の教示点に関し、２個の教示点間の線分と、終点側の教示点を基準点とする、形状情報によって示される位置算出用形状との交点である内回り開始位置を算出する算出部と、を備えたものである。
このような構成により、形状情報を適切に設定することにより、移動方向に応じて内回り開始距離を変更することができる。その結果、干渉を防止できると共に、移動時間を短縮できる。

また、本発明による算出装置では、教示情報は、教示点におけるマニピュレータの先端の姿勢をも示す情報であり、位置算出用形状には、基準となる方向である基準方向も設定されており、算出部は、終点側の教示点を基準点とし、終点側の教示点におけるマニピュレータの先端の姿勢と基準方向とを合わせた位置算出用形状と、２個の教示点間の線分との交点である内回り開始位置を算出してもよい。
このような構成により、マニピュレータの先端の姿勢も考慮するため、より適切な内回り開始位置の決定を行うことができる。

また、本発明による算出装置では、第１の方向は鉛直方向であり、第２の方向は水平方向であってもよい。
このような構成により、鉛直方向と水平方向とで内回り開始距離を異なるようにすることができる。例えば、搬送ロボットがスロットにアクセスする場合などに有用である。

また、本発明による算出装置では、位置算出用形状は、鉛直方向及び水平方向の長さが異なる直方体であってもよい。

また、本発明による制御装置は、上記算出装置と、教示情報と、算出部が算出した内回り開始位置とを用いて、内回り開始位置から内回り軌道が開始されるように、モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えたものである。
このような構成により、形状情報を適切に設定することにより、適切な内回り開始位置を用いた制御を行うことができるようになる。

本発明による算出装置等によれば、内回り開始位置の算出で用いる形状情報を適切に設定することにより、干渉の防止と、移動時間の短縮とを両立することができる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図同実施の形態におけるマニピュレータとカセットとの位置関係を示す図同実施の形態における軌道とスロットとの一例を示す図同実施の形態における軌道とスロット等を示す図同実施の形態における位置算出用形状について説明するための図同実施の形態における位置算出用形状について説明するための図同実施の形態における位置算出用形状の一例を示す図同実施の形態における内回り軌道の一例を示す図同実施の形態における内回り軌道の一例を示す図同実施の形態における直線軌道に応じた速度の時間変化の一例を示す図同実施の形態による算出装置の動作を示すフローチャート従来の内回り軌道の一例を示す図従来の内回り軌道の一例を示す図

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

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、３次元形状を用いて内回り開始位置を算出するものである。

図１は、本実施の形態による産業用ロボットの構成を示すブロック図である。本実施の形態による産業用ロボットは、制御装置１と、サーボコントローラ２と、マニピュレータ３とを備える。マニピュレータ３は、モータによって駆動される関節によって連結された複数のアームを有する。そのマニピュレータ３において、各モータとアームとは減速器を介して接続されている。また、直列に接続されたアームの先端には手先効果器（エンドエフェクタ）が設けられていてもよい。マニピュレータ３は、例えば、水平多関節ロボットのマニピュレータであってもよく、または、垂直多関節ロボットのマニピュレータであってもよい。サーボコントローラ２は、制御装置１による制御に応じて、マニピュレータ３の各軸のモータを制御する。制御装置１は、ティーチングプレイバック方式により、後述する教示情報に応じてマニピュレータ３が動作するようにサーボコントローラ２を制御する。本実施の形態による産業用ロボットは、例えば、搬送ロボットであってもよく、溶接ロボットであってもよく、組立ロボットであってもよく、塗装ロボットであってもよく、あるいは、その他の用途のロボットであってもよい。本実施の形態では、水平多関節ロボットであり、搬送ロボットである産業用ロボットについて主に説明する。すなわち、図２で示されるように、手先効果器が、搬送対象を搬送するハンド３ａであり、カセット４１〜４３の各スロットに、ウェハやガラス基板等の搬送対象を設置したり、各スロットから搬送対象を取得したりする場合について主に説明する。

図１で示されるように、本実施の形態による制御装置１は、内回り開始位置を算出する算出装置１１と、その算出された内回り開始位置を用いてサーボコントローラ２を制御する軌道制御部１２とを備える。また、算出装置１１は、教示情報記憶部２１と、形状情報記憶部２２と、算出部２３とを備える。

教示情報記憶部２１では、教示情報が記憶される。教示情報は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータ３の２以上の教示点を示す情報である。その教示点は、例えば、マニピュレータ３の各軸の教示点であってもよく、または、マニピュレータ３の先端の教示点であってもよい。また、教示情報は、教示点におけるマニピュレータ３の先端の姿勢をも示す情報であってもよい。すなわち、教示情報にマニピュレータ３の手先効果器の姿勢を示す情報が含まれていてもよい。その姿勢を示す情報は、例えば、水平方向における手先効果器の向きを示す角度（例えば、方位角）であってもよく、球面座標系における手先効果器の向きを示す角度（例えば、方位角と仰俯角）であってもよく、球面座標系における手先効果器の向きを示す角度と、手先効果器の方向を中心とする回転の角度であるねじり角度（例えば、手首のねじりに対応する角度）とであってもよい。例えば、産業用ロボットが搬送ロボットであり、手先効果器であるハンドのチルト等を考慮しない場合には、その姿勢を示す情報は、ハンドの向きを示す方位角のみであってもよい。また、各教示点には、その教示点において内回りをするかどうかを示すフラグ等の情報が設定されていてもよい。また、時間的に隣接する２個の教示点を結ぶ軌道に対応する速さを示す情報が教示情報に含まれていてもよい。その速さを示す情報は、例えば、５０％や１００％等のように、最大の速さに対する割合で示されてもよい。

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

形状情報記憶部２２では、位置算出用形状を示す情報である形状情報が記憶される。その位置算出用形状は、第１の方向の長さ、及びその第１の方向と異なる方向である第２の方向の長さを独立して設定可能な３次元形状である。すなわち、位置算出用形状は、少なくとも２方向について独立して長さを設定可能であるものであればよい。したがって、一方向の長さを決めれば他の方向の長さも決まる球や立方体は位置算出用形状に含まれない。また、その位置算出用形状は、基準点の設定された３次元形状である。その基準点は、内回りを行う教示点に対する位置算出用形状の位置決めのために用いられる。また、位置算出用形状には、基準となる方向である基準方向も設定されていてもよい。その基準方向は、内回りを行う教示点におけるマニピュレータ３の先端の姿勢に対する位置算出用形状の向きを決めるために用いられる。その基準方向は、位置算出用形状に設定されたローカル座標系（例えば、３次元の直交座標系等）であってもよく、あるいは、その他のものであってもよい。本実施の形態では、位置算出用形状に、基準点と、ローカル座標系である基準方向との両方が設定されている場合について主に説明する。この形状情報を用いて、内回り開始位置が算出されることになる。また、形状情報のデータ形式は問わない。形状情報において、位置算出用形状を示す情報は、位置算出用形状の頂点を示す情報であってもよく、位置算出用形状の面を示す情報であってもよく、または、その他の方法によって位置算出用形状を示す情報であってもよい。また、形状情報において、基準点は座標で示されてもよく、または、基準点の位置はあらかじめ決められていてもよい。また、形状情報において、基準方向はあらかじめ決められていてもよく、または、基準方向はベクトル等で示されてもよい。

ここで、その形状情報の設定方法について説明する。まず、搬送ロボットがカセットの各スロットに搬送対象を設置したり、各スロットから搬送対象を取得したりする動作について説明する。図３は、搬送ロボットのハンド３ａの動きを示す図である。搬送対象をスロット５２に設置する場合には、ハンド３ａは、他のスロットの位置ＲＡ１から設置スタート位置ＲＴに移動し、その後、トップ位置ＥＴ、ミドル位置ＥＭ、ボトム位置ＥＢ、取得スタート位置ＲＢの順に移動して、他のスロットの位置ＲＡ２に移動する。搬送対象をスロット５２から取得する場合には、ハンド３ａは、逆の順に移動する。その軌道において、設置スタート位置ＲＴから取得スタート位置ＲＢまでは固定であるが、設置スタート位置ＲＴまたは取得スタート位置ＲＢまでは多様な軌跡が存在する。例えば、図４で示されるように、目標スロット５２の上下のスロットからアプローチする場合や、別のカセットから横方向に、または、斜め方向にアプローチする場合などがある。そして、そのような各方向からのアプローチに対して適切な内回り開始位置となるための形状情報を設定する必要がある。図４で示されるように、搬送対象が設置されたり、取得されたりするスロット５１〜５３は、入口の縦幅が、横幅と比較して小さいという特徴がある。したがって、搬送対象やハンド３ａがスロット５１〜５３と干渉しないように、鉛直方向の内回り開始距離は短くなるようにする必要がある。一方、水平方向についてはそのような制限がないため、タクトタイムを短縮するために、水平方向の内回り開始距離は長くなることが好適である。したがって、鉛直方向は長さが短く、水平方向は鉛直方向と比較して長い直方体である位置算出用形状を設定する。具体的には、図５Ａ，図５Ｂで示されるように、鉛直方向については、設置スタート位置ＲＴの高さと、上側のスロット５３の底面の高さとの距離ｚ１１と、設置スタート位置ＲＴの高さと、目標のスロット５２の上面の高さとの距離ｚ１２とを用いて設定する。また、水平方向のうち、スロット５２の奥行き方向については、設置スタート位置ＲＴと、カセット４１の入口との距離ｘ１１と、マニピュレータ３の旋回中心から奥行き方向に直角に延ばした直線と、設置スタート位置ＲＴとの距離ｘ１２とを用いて設定する。また、水平方向のうち、奥行き方向に直角な横方向については、設置スタート位置と、カセット４１の内壁面までの距離ｙ１１，ｙ１２を用いて設定する。なお、ｘ１１，ｘ１２，ｙ１１，ｙ１２，ｚ１１，ｚ１２は、すべて正の実数であるとする。図６は、ｘ１１等を用いて設定された、位置算出用形状５５の一例を示す図である。図６において、位置算出用形状５５は、ローカル座標系のｘｙ平面を水平方向とした場合における、鉛直方向及び水平方向の長さが異なる直方体である。図６で示される位置算出用形状５５は、ローカル座標系である３次元直交座標系におけるｘ＝ｘ１の面と、ｘ＝−ｘ２の面と、ｙ＝ｙ１の面と、ｙ＝−ｙ２の面と、ｚ＝ｚ１の面と、ｚ＝−ｚ２の面とで囲われる直方体である。また、図６では、基準点が、そのローカル座標系における原点、すなわち（ｘ、ｙ、ｚ）＝（０，０，０）にあらかじめ設定されており、基準方向がそのローカル座標系そのものであるとする。したがって、この場合には、形状情報は、例えば、各面を示す情報、すなわち、ｘ１，−ｘ２，ｙ１，−ｙ２，ｚ１，−ｚ２であってもよい。なお、ｘ１，ｘ２，ｙ１，ｙ２，ｚ１，ｚ２は、それぞれｘ１１，ｘ１２，ｙ１１，ｙ１２，ｚ１１，ｚ１２に、１より小さく、１に近い数（例えば、０．９等）を掛けた数であるとする。また、形状情報は、各頂点を示す情報、すなわち、（ｘ１，ｙ１，ｚ１）、（ｘ１，ｙ１，−ｚ２）、（ｘ１，−ｙ２，ｚ１）、（ｘ１，−ｙ２，−ｚ２）、（−ｘ２，ｙ１，ｚ１）、（−ｘ２，ｙ１，−ｚ２）、（−ｘ２，−ｙ２，ｚ１）、（−ｘ２，−ｙ２，−ｚ２）であってもよい。また、形状情報における基準位置は、ｘ１１等を測定したときの教示点の位置、すなわち設置スタート位置ＲＴである。また、基準方向は、その教示点におけるマニピュレータ３の先端の姿勢に応じて設定される。なお、本実施の形態では、マニピュレータ３の先端（例えば、ハンド３ａ）についても、ローカル座標系が設定されているものとする。そして、そのローカル座標系は、図５Ｂで示されるように、ｘｙ平面がハンド３ａの載置面方向となる３次元直交座標系であるとする。そのローカル座標系は、ハンド３ａの姿勢を基準とする相対座標系であるため、ハンド３ａの姿勢に応じてｘ軸等の方向が変わることになる。また、図５Ａ，図５Ｂを用いて説明した形状情報の設定方法は一例であり、これに限定されないことは言うまでもない。例えば、ｘ１２は、上述した以外の方法によって決められてもよい。

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

算出部２３は、教示情報によって示される時間的に隣接する２個の教示点の終点側の教示点において内回りを行う場合には、その２個の教示点に関して、その２個の教示点間の線分と、終点側の教示点を基準点とする位置算出用形状との交点である内回り開始位置を算出する。その位置算出用形状は、形状情報記憶部２２で記憶されている形状情報によって示されるものである。形状情報に基準方向も含まれており、マニピュレータ３の先端の姿勢も用いて内回り開始位置を算出する場合には、算出部２３は、終点側の教示点を基準点とし、その終点側の教示点におけるマニピュレータ３の先端の姿勢と基準方向とを合わせた位置算出用形状と、その２個の教示点を結ぶ線分との交点である内回り開始位置を算出してもよい。前述のように、位置算出用形状に基準方向であるローカル座標系が設定されており、マニピュレータ３の先端にもローカル座標系が設定されている場合には、算出部２３は、位置算出用形状の基準点が終点側の教示点に位置し、位置算出用形状に設定されているローカル座標系の各軸の向きが、その終点側の教示点におけるマニピュレータ３の先端の姿勢に応じたローカル座標系の各軸の向きと合うように位置算出用形状を配置してもよい。そして、算出部２３は、その配置した位置算出用形状と、２個の教示点を結ぶ線分との交点である内回り開始位置を算出してもよい。なお、２個の教示点のうち、終点側の教示点とは、教示点で示される軌道において時間的に後側の教示点である。また、教示情報にマニピュレータ３の先端以外の教示点が含まれる場合であっても、この内回り開始位置の算出において用いる教示点は、通常、マニピュレータ３の先端の教示点である。また、算出される内回り開始位置の座標は、通常、グローバル座標系（絶対座標系）の座標である。その算出された内回り開始位置は、図示しない記録媒体で記憶されてもよい。

ここで、図７Ａ，図７Ｂを用いて、算出部２３が内回り開始位置を算出する方法について説明する。図７Ａ，図７Ｂにおいて、教示された設置スタート位置ＲＴにおけるハンド３ａの向きは、設置スタート位置ＲＴを始点としてトップ位置ＥＴを終点とするベクトルの向きであるとする。すると、算出部２３は、形状情報記憶部２２から、ｘ１，ｘ２等の形状情報を読み出し、設置スタート位置ＲＴが基準点となり、ハンド３ａの向きが基準方向となるように位置算出用形状５５を配置する。具体的には、算出部２３は、位置算出用形状５５を示すグローバル座標系における座標値等を算出する。そして、算出部２３は、他のスロットの位置ＲＡ１と、設置スタート位置ＲＴとを結ぶ線分と、設置された位置算出用形状５５との交点を算出する。その算出された交点が、内回り開始位置ＳＰとなる。このように、位置算出用形状５５を用いて内回り開始位置ＳＰを算出するため、鉛直方向の移動については、小さい内回りとすることによって干渉を回避することができ（図７Ａ）、水平方向の移動については、大きい内回りとすることによってタクトタイムを短縮することができるようになる（図７Ｂ）。なお、算出部２３が、位置算出用形状５５のグローバル座標系における座標値等を算出する際に、他のスロットの位置ＲＡ１、及び設置スタート位置ＲＴを結ぶ線分と交わる面についてのみ、グローバル座標系への変換を行ってもよい。

軌道制御部１２は、教示情報記憶部２１で記憶されている教示情報と、算出部２３が算出した内回り開始位置とを用いて、内回り開始位置から内回り軌道が開始されるようにサーボコントローラ２を制御する。内回りを行う場合には、軌道制御部１２は、内回り開始位置から速度の重ね合わせを行うことによって内回り軌道が開始されるようにしてもよく、または、内回り開始位置から内回り軌道を生成し、その内回り軌道をマニピュレータ３の各軸が通過するようにしてもよい。本実施の形態では、前者の場合について主に説明する。具体的には、軌道制御部１２は、図７Ａ，図７Ｂにおいて、内回り開始位置ＳＰから設置スタート位置ＲＴまでの距離を計算する。そして、ハンド３ａが内回り開始位置ＳＰの位置となる時刻を算出する。例えば、他のスロットの位置ＲＡ１から、設置スタート位置ＲＴまでの軌道において、図８で示される速度の時間変化で移動することになっていたとする。速度の立ち上がり位置が他のスロットの位置ＲＡ１に対応し、時間「Ｔ２」の位置が設置スタート位置ＲＴに対応する。このような場合には、軌道制御部１２は、図８の斜線の面積が内回り開始位置ＳＰから設置スタート位置ＲＴまでの距離と等しくなる時間「Ｔ１」を求める。そして、その時間「Ｔ１」が内回りを開始する時点となるため、その時点から、設置スタート位置ＲＴからトップ位置ＥＴまでの速度との重ね合わせを開始し、内回り軌道が始まることになる。その速度の重ね合わせで用いられる速度の時間変化（例えば、図８で示されるもの）は、あらかじめ教示情報に含まれていてもよく、または、軌道制御部１２が教示情報を用いて生成してもよい。なお、軌道制御部１２は、教示情報によって示される教示点間を補間した軌道をマニピュレータ３の各軸が通過するように、サーボコントローラ２を制御してもよい。また、教示情報にマニピュレータ３の先端のみの教示点が含まれている場合には、軌道制御部１２は、その教示点を用いて各軸の位置を算出してもよい。軌道制御部１２による処理は、従来の内回りを行うロボットにおけるサーボコントローラの制御と同じであり、その詳細な説明を省略する。

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

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

次に、制御装置１の動作について図９のフローチャートを用いて説明する。
（ステップＳ１０１）軌道制御部１２は、教示情報記憶部２１から次の移動先の教示点の情報を読み出す。なお、移動の開始時においては、軌道制御部１２は、移動元の教示点の情報と移動先の教示点の情報との両方を読み出してもよい。

（ステップＳ１０２）軌道制御部１２は、ステップＳ１０１で読み出した教示点、すなわち、軌道における終点側の教示点において内回りを行うかどうか判断する。そして、内回りを行う場合には、ステップＳ１０３に進み、そうでない場合には、ステップＳ１０５に進む。

（ステップＳ１０３）算出部２３は、ステップＳ１０１で読み出された教示点について、内回り開始位置を算出する。具体的には、算出部２３は、形状情報記憶部２２から形状情報を読み出し、形状情報の示す位置算出用形状の基準点及び基準方向が、ステップＳ１０１で読み出された教示点の位置及びその教示点におけるマニピュレータ３の先端の姿勢となるように位置算出用形状を設置する。そして、算出部２３は、ステップＳ１０１で読み出された教示点、及びそれよりも１個前の教示点を結ぶ線分と、その位置算出用形状との交点である内回り開始位置を算出する。

（ステップＳ１０４）軌道制御部１２は、ステップＳ１０３で算出された内回り開始位置を用いて、内回りを開始する時刻を算出する。

（ステップＳ１０５）軌道制御部１２は、ステップＳ１０１で読み出した教示点と、その教示点よりも１個前の教示点との間の各位置を、補間によって生成する。なお、その位置が内回り開始位置を超えた場合には、速度の重ね合わせを行うことによって、内回り軌道を通るように各位置を生成する。この位置の生成は、例えば、あらかじめ決められた時間間隔（例えば、５ｍｓ等）ごとに行われてもよい。

（ステップＳ１０６）軌道制御部１２は、次の教示点の情報を読み出すかどうか判断する。そして、読み出す場合には、ステップＳ１０１に戻り、そうでない場合には、ステップＳ１０５に戻る。なお、軌道制御部１２は、例えば、内回りを行う場合であって、算出する位置が内回り開始位置を超えた場合には、次の軌道の情報が必要になるため、次の教示点の情報を読み出すと判断してもよい。また、軌道制御部１２は、例えば、内回りを行わない場合に、位置の算出が終点側の教示点まで終了した場合に、次の教示点の情報を読み出すと判断してもよい。なお、ステップＳ１０１に戻ると判断するまでは、軌道制御部１２は、ステップＳ１０５の処理を繰り返すことによって、教示点間の補間や、内回り軌道に応じた位置の算出を繰り返すことになる。

なお、図９のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。また、図９のフローチャートにおいて、ステップＳ１０３の内回り開始位置の算出を少なくとも行うのであれば、マニピュレータ３の先端が内回り軌道を通るように制御する方法は問わない。例えば、教示情報で示される内回りを行う各教示点について、あらかじめ内回り開始位置をバッチ処理で算出し、マニピュレータ３を制御する際に、その内回り開始位置を用いて制御を行ってもよい。

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

この教示情報において、Ｐｏｓｉｔｉｏｎ（Ｎ）は、マニピュレータ３の各軸の位置を示す情報である。ここで、Ｎは１以上の整数とする。なお、そのＰｏｓｉｔｉｏｎ（Ｎ）に、マニピュレータ３の先端であるハンド３ａの位置も含まれているものとする。また、Ｐｏｓｅ（Ｎ）は、マニピュレータ３の先端であるハンド３ａの角度を示す情報である。また、Ｆｌａｇ（Ｎ）は、Ｎ番目の教示点において内回りを行うかどうかを示すフラグである。Ｆｌａｇ（Ｎ）が「１」である場合には内回りを行い、Ｆｌａｇ（Ｎ）が「０」である場合には内回りを行わないことになる。すなわち、Ｆｌａｇ（Ｎ）が「０」である場合には、それに対応するＰｏｓｉｔｉｏｎ（Ｎ）の示す位置をマニピュレータ３が通過することになる。また、Ｐｏｓｉｔｉｏｎ（１０）は、図７Ｂにおける他のスロットの位置ＲＡ１に対応し、Ｐｏｓｉｔｉｏｎ（１１）は、図７Ｂにおける設置スタート位置ＲＴに対応し、Ｐｏｓｉｔｉｏｎ（１２）は、図７Ｂにおけるトップ位置ＥＴに対応するものとする。また、Ｆｌａｇ（１０）＝Ｆｌａｇ（１２）＝０であり、Ｆｌａｇ（１１）＝１であるとする。

また、この具体例において、形状情報記憶部２２に、図６の位置算出用形状５５を示す形状情報「ｘ１，ｘ２，ｙ１，ｙ２，ｚ１，ｚ２」が記憶されているものとする。なお、その位置算出用形状５５においては、基準方向は、ローカル座標系の各軸の方向、すなわち、（ｘ、ｙ、ｚ）＝（１，０，０）、（０，１，０）、（０，０，１）に設定されているものとする。

軌道制御部１２は、Ｐｏｓｉｔｉｏｎ（１）、Ｐｏｓｅ（１）、Ｆｌａｇ（１）から順次、教示点の情報を読み出して、適宜、内回り開始位置等を算出し、軌道を算出する（ステップＳ１０１〜Ｓ１０６）。そのようにして、Ｐｏｓｉｔｉｏｎ（１１）、Ｐｏｓｅ（１１）、Ｆｌａｇ（１１）が読み出されたとする（ステップＳ１０１）。軌道制御部１２は、新たな読み出しに応じて、Ｐｏｓｉｔｉｏｎ（１０）からＰｏｓｉｔｉｏｎ（１１）までの速度を算出する。その速度は、例えば、図８で示されるものである。また、軌道制御部１２は、Ｆｌａｇ（１１）が「１」であるかどうか判断する（ステップＳ１０２）。この場合には、Ｆｌａｇ（１１）＝１であるため、軌道制御部１２は、内回りを行うと判断し、算出装置１１に、内回りを行う教示点を識別する情報であるＮ＝１１と、内回り開始位置の算出を行う指示とを渡す。すると、算出部２３は、教示情報記憶部２１で記憶されている教示情報から、Ｎ＝１１であるＰｏｓｉｔｉｏｎ（１１）とＰｏｓｅ（１１）とを読み出し、また、形状情報記憶部２２から形状情報「ｘ１，ｘ２，ｙ１，ｙ２，ｚ１，ｚ２」を読み出す。そして、算出部２３は、読み出したＰｏｓｉｔｉｏｎ（１１）で示される位置が、位置算出用形状５５のローカル座標系の原点（基準点）となり、Ｐｏｓｅ（１１）で示されるハンド３ａの姿勢に応じたローカル座標系の各軸の方向が、位置算出用形状５５のローカル座標系の方向と一致するように位置算出用形状５５をグローバル座標系に配置する。また、算出部２３は、Ｐｏｓｉｔｉｏｎ（１０）を読み出し、図７Ｂで示されるように、Ｐｏｓｉｔｉｏｎ（１０）で示されるハンド３ａの位置（ＲＡ１）と、Ｐｏｓｉｔｉｏｎ（１１）で示されるハンド３ａの位置（ＲＴ）とを結ぶ線分と、上述のようにして配置した位置算出用形状５５との交点である内回り開始位置ＳＰの座標を算出し、軌道制御部１２に渡す（ステップＳ１０３）。なお、その算出された内回り開始位置ＳＰの座標は、グローバル座標系における座標である。

軌道制御部１２は、算出装置１１から内回り開始位置ＳＰを受け取ると、Ｐｏｓｉｔｉｏｎ（１１）で示されるハンド３ａの位置から、内回り開始位置ＳＰまでの距離である内回り開始距離を算出する。そして、軌道制御部１２は、あらかじめ算出していたＰｏｓｉｔｉｏｎ（１０）からＰｏｓｉｔｉｏｎ（１１）までの速度変化を用いて、Ｐｏｓｉｔｉｏｎ（１１）までの距離が、その算出した内回り開始距離と等しくなる時刻である内回り開始時刻を算出する（ステップＳ１０４）。その後、軌道制御部１２は、あらかじめ決められた補間間隔に応じて、Ｐｏｓｉｔｉｏｎ（１０）からＰｏｓｉｔｉｏｎ（１１）に向かう各位置を算出し、その各位置にマニピュレータ３が移動するように、サーボコントローラ２に指示を行う（ステップＳ１０５，Ｓ１０６）。なお、ステップＳ１０４で算出した内回り開始時刻に対応する位置まで算出すると、軌道制御部１２は、次の教示点を読み出すと判断し、トップ位置ＥＴに対応するＰｏｓｉｔｉｏｎ（１２）、Ｐｏｓｅ（１２）、Ｆｌａｇ（１２）を読み出す（ステップＳ１０１）。そして、軌道制御部１２は、Ｐｏｓｉｔｉｏｎ（１１）からＰｏｓｉｔｉｏｎ（１２）までの速度を算出する。なお、この場合には、Ｆｌａｇ（１２）＝０であり、内回りを行わないため、軌道制御部１２は、内回り開始時刻から速度の重ね合わせを行い、Ｐｏｓｉｔｉｏｎ（１２）までの軌道の各位置の算出を行う（ステップＳ１０５，Ｓ１０６）。このようにして、教示情報に応じて順次、マニピュレータ３の位置が算出され、マニピュレータ３の制御が行われることになる。また、内回りを行う場合には、図７Ａ，図７Ｂで示されるように、位置算出用形状５５を用いた内回り開始位置ＳＰの算出と、その算出された内回り開始位置ＳＰからの速度の重ね合わせとが行われ、マニピュレータ３の先端であるハンド３ａが、内回り軌道を移動するように制御される。

以上のように、本実施の形態による制御装置１によれば、形状情報を適切に設定することにより、例えば、スロットに搬送対象を設置したり、スロットから搬送対象を取得したりする際に、搬送対象やハンドとスロットとが干渉することを防止することができると共に、内回りを不要に制限することも回避でき、移動時間の長期化を防止することができる。すなわち、制御装置１は、多様な方向からのアプローチに対応する適切な内回り開始位置の算出を行うことができる。また、マニピュレータ３の先端の姿勢と基準方向とを合わせた位置算出用形状を用いて内回り開始位置を算出することによって、そのマニピュレータ３の先端の姿勢も考慮した適切な内回り開始位置の算出が可能となる。すなわち、マニピュレータ３の先端の姿勢を考慮しない位置算出用形状を用いる場合には、その向きに関係なく適切な干渉回避を実現できるようにするため、位置算出用形状を、例えば、水平方向の底辺が正方形である角柱や、水平方向の底辺が円である円柱等としなければならないこともあり、その結果として、ある方向の内回りが小さくなることもありうる。一方、マニピュレータ３の先端の姿勢に応じて配置された位置算出用形状を用いることによって、内回りを不要に制限する必要がなくなり、適切な内回りの制御を行うことができるようになりうる。

なお、本実施の形態では、位置算出用形状が、鉛直方向及び水平方向の長さが異なる直方体である場合について主に説明したが、そうでなくてもよい。位置算出用形状は、例えば、鉛直方向である第１の方向の長さと、水平方向である第２の方向の長さとを独立して設定可能な３次元形状であってもよい。なお、その位置算出用形状は、第１及び第２の方向の長さを独立して設定可能なものであれば、３以上の方向の長さを独立して設定可能なものであってもよい。２以上の方向の長さを独立して設定可能な位置算出用形状としては、具体的には、円柱や多角柱、楕円柱、円錐、角錐、楕円体等がある。位置算出用形状が円柱や円錐である場合には、高さと、底面の直径とを独立して設定可能である。また、位置算出用形状が多角柱や角錐である場合には、高さと、底面の形状とを独立して設定可能である。例えば、位置算出用形状の円柱や多角柱、楕円中、円錐、角錐の底面が水平方向であるとすると、その位置算出用形状は、鉛直方向と水平方向との２方向の長さを独立して設定可能であることになる。また、位置算出用形状が楕円体である場合には、ｘ^２／ａ^２＋ｙ^２／ｂ^２＋ｚ^２／ｃ^２＝１の方程式で示される形状であってもよい。その方程式において、ａ，ｂ，ｃは、それぞれが独立に設定可能な係数であってもよく、または、ａ＝ｂであり、ａ，ｃが独立して設定可能な係数であってもよい。ａ＝ｂである場合には、鉛直方向と、水平方向との２方向の長さを独立して設定可能なことになる。ただし、ｘｙ平面が水平方向であり、ｚ軸が鉛直方向であるとしている。

また、形状情報記憶部２２で記憶される形状情報が、位置算出用形状の形状を示す方法は問わない。例えば、前述のように、頂点の位置や面の位置によって位置算出用形状を示してもよく、または、方程式によって位置算出用形状を示してもよい。

また、本実施の形態では、１個の位置算出用形状のみを用いる場合について説明したが、そうでなくてもよい。算出部２３は、複数の位置算出用形状を用いて内回り開始位置を算出してもよい。その場合には、教示情報において、内回りを行う教示点に、内回り開始位置の算出で用いる形状情報を識別する形状情報識別子が対応付けられていてもよい。そして、算出部２３は、ある教示点について内回り開始位置を算出する場合に、その教示点に対応する形状情報識別子で識別される形状情報を読み出し、その読み出した形状情報の示す位置算出用形状を用いて内回り開始位置の算出を行ってもよい。具体的には、教示点である設置スタート位置ＲＴを基準点とする位置算出用形状を示す形状情報と、教示点である取得スタート位置ＲＢを基準点とする位置算出用形状を示す形状情報とが異なる場合には、それらの教示点に応じた形状情報を用いて内回り開始位置の算出が行われてもよい。また、カセットごとにスロット間の高さが異なる場合には、カセットごとに、そのスロット間の高さに応じた形状情報を用いて内回り開始位置の算出が行われてもよい。

また、本実施の形態では、位置算出用形状に基準方向も設定されており、その基準方向を用いて内回り開始位置を算出する場合について主に説明したが、そうでなくてもよい。位置算出用形状に基準方向が設定されておらず、その基準方向を用いないで内回り開始位置が算出されてもよい。その場合には、例えば、マニピュレータ３の先端の姿勢に関わらず、同じ形状となる位置算出用形状、例えば、水平方向が底面である円柱等であってもよく、または、そうでなくてもよい。

また、本実施の形態では、制御装置１が算出装置１１を含んでいる場合について説明したが、そうでなくてもよい。算出装置１１と、マニピュレータ３を制御する制御装置とは、異なる装置であってもよい。その場合には、例えば、算出装置１１は、教示情報の示す教示点のうち、内回りを行う各教示点に対応する内回り開始位置を算出し、その内回り開始位置を教示情報に追加したり、マニピュレータ３を制御する制御装置に渡したりしてもよい。したがって、算出装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合には、算出装置１１は、算出した内回り開始位置を蓄積したり、制御装置に渡したりする図示しない出力部を備えていてもよい。また、算出装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合に、算出装置１１の有する教示情報記憶部２１は、制御装置で記憶されている教示情報が一時的に記憶される記憶部であってもよく、あるいは、そうでなくてもよい。

また、本実施の形態では、マニピュレータ３の先端の姿勢を基準方向とする位置算出用形状を配置するため、マニピュレータ３の先端の姿勢に応じたローカル座標系の各軸と、位置算出用形状のローカル座標系の各軸とを一致させる場合について主に説明したが、そうでなくてもよい。例えば、教示情報において示されるマニピュレータ３の先端の姿勢が方位角のみである場合には、マニピュレータ３の先端の姿勢に応じた向きと、位置算出用形状に設定されている基準方向である一の向き（例えば、ｘ軸）とが一致するように位置算出用形状を配置してもよい。その場合には、位置算出用形状を配置する際に、基準方向を中心とする回転に関する自由度が存在するため、位置算出用形状において、あらかじめ水平方向に設定する面（例えば、ｘｙ平面等）が決められていてもよい。

また、本実施の形態では、産業用ロボットが搬送ロボットであり、マニピュレータ３の先端がハンド３ａである場合について主に説明したが、そうでなくてもよいことは言うまでもない。マニピュレータ３は、搬送ロボット以外の塗装ロボットや溶接ロボット等のマニピュレータであってもよい。その場合であっても、例えば、スロットと同様の形状をした凹部に手先効果器を挿入する動作を行う場合などにおいて、本実施の形態と同様に内回り開始位置を算出することによって、干渉を防止しながら、移動時間を短縮することができる効果が得られる。

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

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

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

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

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

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

以上より、本発明による算出装置等によれば、移動方向に応じて内回り開始距離を変更できるという効果が得られ、マニピュレータの内回り開始位置を算出する装置等として有用である。

１制御装置
２サーボコントローラ
３マニピュレータ
３ａハンド
１１算出装置
１２軌道制御部
２１教示情報記憶部
２２形状情報記憶部
２３算出部

Claims

モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの２以上の教示点を示す教示情報が記憶される教示情報記憶部と、
基準点の設定された３次元形状であり、第１の方向の長さ、及び当該第１の方向と異なる方向である第２の方向の長さを独立して設定可能な３次元形状である位置算出用形状を示す形状情報が記憶される形状情報記憶部と、
前記教示情報によって示される隣接する２個の教示点に関し、当該２個の教示点間の線分と、終点側の教示点を基準点とする、前記形状情報によって示される位置算出用形状との交点である内回り開始位置を算出する算出部と、を備えた算出装置。
前記教示情報は、教示点における前記マニピュレータの先端の姿勢をも示す情報であり、
前記位置算出用形状には、基準となる方向である基準方向も設定されており、
前記算出部は、前記終点側の教示点を基準点とし、当該終点側の教示点における前記マニピュレータの先端の姿勢と基準方向とを合わせた位置算出用形状と、前記２個の教示点間の線分との交点である内回り開始位置を算出する、請求項１記載の算出装置。
前記第１の方向は鉛直方向であり、前記第２の方向は水平方向である、請求項１または請求項２記載の算出装置。
前記位置算出用形状は、鉛直方向及び水平方向の長さが異なる直方体である、請求項３記載の算出装置。
請求項１から請求項４のいずれか記載の算出装置と、
前記教示情報と、前記算出部が算出した内回り開始位置とを用いて、当該内回り開始位置から内回り軌道が開始されるように、前記モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えた制御装置。
モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの２以上の教示点を示す教示情報が記憶される教示情報記憶部で記憶されている教示情報と、基準点の設定された３次元形状であり、第１の方向の長さ、及び当該第１の方向と異なる方向である第２の方向の長さを独立して設定可能な３次元形状である位置算出用形状を示す形状情報が記憶される形状情報記憶部で記憶されている形状情報とを用いて、前記教示情報によって示される隣接する２個の教示点に関し、当該２個の教示点間の線分と、終点側の教示点を基準点とする、前記形状情報によって示される位置算出用形状との交点である内回り開始位置を算出する算出ステップを備えた算出方法。