(57)【特許請求の範囲】
【請求項１】
左右方向をＸ軸、前後方向をＹ軸、上下方向をＺ軸としたとき、ＸＹ平面に平行で基準点を持つ四角形状のエリアをＸＹ方向に連ねたマトリクス平面を、Ｚ方向に所定間隔おきに積み上げることにより３次元マトリクスを構築し、前記３次元マトリクスの動作空間に指定された作業ロボットの目的位置を補正する方法であって、
前記所定間隔を隔てて配置された上下一対の前記エリアを含む直方体空間をブロックと称し、前記目的位置を前記３次元マトリクスの動作空間に指定したときの前記目的位置を含むブロックを特定ブロックと称するとした場合、
（ａ）前記特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する工程と、
（ｂ）前記特定ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットのエンドエフェクタの制御点の測定ズレ量、前記第１ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記エンドエフェクタの制御点の測定ズレ量、及び、前記第２ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記エンドエフェクタの制御点の測定ズレ量に基づいて前記目的位置を補正する工程と、
を含み、
前記エンドエフェクタの制御点は、前記基準点に前記エンドエフェクタの先端が一致するように前記作業ロボットを制御したときの前記エンドエフェクタの先端の位置である、
作業ロボットの目的位置補正方法。
【請求項２】
前記工程（ａ）では、前記特定ブロックの前後左右の各面には前ブロック、後ブロック、左ブロック及び右ブロックが接しており、前記第１及び第２ブロックを、前記目的位置に基づいて、前記前ブロック、前記後ブロック、前記左ブロック及び前記右ブロックのうちの２つのブロックから選択する、
請求項１に記載の作業ロボットの目的位置補正方法。
【請求項３】
前記工程（ａ）では、前記特定ブロックを４等分して左前方ゾーン、右前方ゾーン、左後方ゾーン及び右後方ゾーンを作成し、該４つのゾーンの中から前記目的位置を含むゾーンを選択し、選択されたゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックに設定する、
請求項２に記載の作業ロボットの目的位置補正方法。
【請求項４】
前記工程（ｂ）では、まず、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して前記目的位置のＺ座標を含むＸＹ平面に対応する傾斜面（水平面の場合も含む）を求め、前記傾斜面に基づいて前記目的位置のＺ方向の補正量を求める、
請求項１〜３のいずれか１項に記載の作業ロボットの目的位置補正方法。
【請求項５】
前記工程（ｂ）で、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置を前記上エリアに投影した上方投影位置と、前記下エリアの前記基準点から前記測定ズレ量だけずれた位置を前記下エリアに投影した下方投影位置とを結んだ直線が、前記目的位置のＺ座標を含むＸＹ平面と交わる交点を求め、前記特定ブロックの前記交点と前記第１ブロックの前記交点とを結んだ直線及び前記特定ブロックの前記交点と前記第２ブロックの前記交点とを結んだ直線に基づいて前記目的位置のＸ方向及びＹ方向の補正量を求める、
請求項４に記載の作業ロボットの目的位置補正方法。
【請求項６】
前記工程（ｂ）で、Ｚ方向のズレ量を考慮して前記目的位置のＺ方向の補正量を求める際の前記傾斜面を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上方投影位置と前記下方投影位置とを結んだ直線と、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置と前記下エリアの前記基準点から前記測定ズレ量だけずれた位置とを結んだ直線とを用いて、前記目的位置のＺ座標でのズレ量を求め、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々の前記目的位置のＺ座標でのズレ量に基づいて前記傾斜面を求める、
請求項４又は５に記載の作業ロボットの目的位置補正方法。
【発明の詳細な説明】
【技術分野】
【０００１】  本明細書は、作業ロボットの目的位置補正方法について開示する。
【背景技術】
【０００２】  従来、作業ロボットとしては、目標点に対してＤＨパラメータを適用して座標変換を行なうことでロボットの動作を制御するものが提案されている（例えば、特許文献１参照）。ＤＨパラメータの設定は以下のようにして行なわれる。即ち、ロボットの制御装置は、ロボットの動作空間を複数の領域に分割し、分割した領域ごとに測定点を設定する。次に、制御装置は、測定点へロボットを移動させて３次元の位置データを取得する。そして、制御装置は、取得した位置データと測定点との誤差からＤＨパラメータを導出する。制御装置は、目標点に基づいてロボットの動作を制御する際、複数の領域ごとに導出したＤＨパラメータのうち目標点が属する領域のＤＨパラメータを選択し、目標点に対して選択したＤＨパラメータを適用して座標変換を行なう。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−１４８８５０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】  しかし、上述した作業ロボットでは、座標変換に用いるＤＨパラメータを最適化しても、十分な作業精度を確保できない場合があった。そのため、作業ロボットの作業精度をより向上させることが望まれている。【０００５】  本開示は、３次元マトリクスの動作空間に指定された目的位置へ作業ロボットを精度よく位置決めすることを主目的とする。
【課題を解決するための手段】
【０００６】  本開示の作業ロボットの目的位置補正方法は、  左右方向をＸ軸、前後方向をＹ軸、上下方向をＺ軸としたとき、ＸＹ平面に平行で基準点を持つ四角形状のエリアをＸＹ方向に連ねたマトリクス平面を、Ｚ方向に所定間隔おきに積み上げることにより３次元マトリクスを構築し、前記３次元マトリクスの動作空間に指定された作業ロボットの目的位置を補正する方法であって、  前記所定間隔を隔てて配置された上下一対の前記エリアを含む直方体空間をブロックと称し、前記目的位置を前記３次元マトリクスの動作空間に指定したときの前記目的位置を含むブロックを特定ブロックと称するとした場合、（ａ）前記特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する工程と、（ｂ）前記特定ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量、前記第１ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量、及び、前記第２ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量に基づいて前記目的位置を補正する工程と、  を含むものである。【０００７】  この目的位置補正方法では、目的位置を含む特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する。そして、特定ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量、第１ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量、及び、第２ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量に基づいて、目的位置を補正する。このように、特定ブロックの基準点からの測定ズレ量だけでなく、特定ブロックに接している２つのブロックの基準点からの測定ズレ量を利用して目的位置を補正する。したがって、３次元マトリクスの動作空間に指定された目的位置へ作業ロボットを精度よく位置決めすることができる。
【図面の簡単な説明】
【０００８】
【図１】ロボットシステム１０の構成の概略を示す構成図。
【図２】３次元マトリクス２５の説明図。
【図３】ロボット２０と制御装置７０との電気的な接続関係を示すブロック図。
【図４】補正前処理ルーチンのフローチャート。
【図５】基準点Ｒと測定点Ｍとの関係を示す説明図。
【図６】目的位置補正ルーチンのフローチャート。
【図７Ａ】目的位置Ｔが左前方ゾーンａ１にあるときの特定ブロックＢｓの上面図。
【図７Ｂ】目的位置Ｔが左前方ゾーンａ１にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。
【図８Ａ】目的位置Ｔが右前方ゾーンａ２にあるときの特定ブロックＢｓの上面図。
【図８Ｂ】目的位置Ｔが右前方ゾーンａ２にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。
【図９Ａ】目的位置Ｔが左後方ゾーンａ３にあるときの特定ブロックＢｓの上面図。
【図９Ｂ】目的位置Ｔが左後方ゾーンａ３にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。
【図１０Ａ】目的位置Ｔが右後方ゾーンａ４にあるときの特定ブロックＢｓの上面図。
【図１０Ｂ】目的位置Ｔが右後方ゾーンａ４にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。
【図１１】特定ブロックＢｓの仮想投影点ＨＰを求めるときの説明図。
【図１２】補正量Δｘt，Δｙtの算出方法の説明図。
【図１３】特定ブロックＢｓの仮想測定点ＨＭの求め方の説明図。
【図１４】図１３のＡ視図。
【図１５】仮想測定点ＨＭｓ，ＨＭ１，ＨＭ２を含む傾斜面の説明図。
【発明を実施するための形態】
【０００９】  次に、本開示を実施するための形態について図面を参照しながら説明する。
【００１０】  図１は、ロボットシステム１０の構成の概略を示す構成図である。図２は、３次元マトリクス２５の説明図である。図３は、ロボット２０と制御装置７０との電気的な接続関係を示すブロック図である。なお、図１，２中、左右方向はＸ軸方向であり、前後方向はＹ軸方向であり、上下方向はＺ軸方向である。
【００１１】  ロボットシステム１０は、ロボット２０と、ロボット２０を制御する制御装置７０と、を備える。ロボットシステム１０は、ワークをピックアップし、ピックアップしたワークを対象物にプレースするシステムとして構成されている。本実施形態では、ロボットシステム１０は、ロボット２０を用いてワークに対して作業を行なうシステムであれば、如何なるシステムにも適用できる。例えば、ロボットシステム１０は、ロボット２０により部品を採取して基板上に実装する部品実装システムに適用できる。
【００１２】  ロボット２０は、５軸の垂直多関節アーム（以下、アームという）２２と、ロボット２０の手先である図示しないエンドエフェクタ２３と、を備える。アーム２２は、６つのリンク（第１〜第６リンク３１〜３６）と、各リンクを回転または旋回可能に連結する５つの関節（第１〜第５関節４１〜４５）と、を備える。各関節（第１〜第５関節４１〜４５）は、対応する関節を駆動するモータ（第１〜第５モータ５１〜５５、図３参照）、対応するモータの回転角度を検出するエンコーダ（第１〜第５エンコーダ６１〜６５、図３参照）と、を備える。本実施形態では、モータはサーボモータであり、エンコーダはロータリエンコーダである。エンドエフェクタ２３は、アーム２２の先端リンク（第６リンク３６）に取り付けられ、部品（ワーク）の保持とその解除とが可能となっている。エンドエフェクタ２３は、例えば、メカチャックや吸着ノズル、電磁石などを用いることができる。
【００１３】  ここで、ロボット２０の動作空間について図２を参照しながら説明する。３次元マトリクス２５は、ロボット２０の動作空間である。ロボット２０のエンドエフェクタ２３の先端は、３次元マトリクス２５内に目的位置が設定されると、制御装置７０によってその目的位置に移動されるが、この点は後で詳述する。３次元マトリクス２５は、マトリクス平面ＰをＺ方向に所定間隔おきに積み上げることにより構築されている。３次元マトリクス２５の下からｎ番めのマトリクス平面Ｐをマトリクス平面Ｐn（ｎは自然数）と称する。また、マトリクス平面Ｐn の高さすなわちｚ座標をｚnと表す。マトリクス平面Ｐは、ＸＹ平面に平行で中心に基準点Ｒを持つ矩形のエリアＡがＸＹ方向に複数連なったものである。基準点Ｒはワールド座標（絶対座標）上に設定されている。すべての基準点Ｒの座標は、予めＨＤＤ７３に保存されている。本実施形態では、所定間隔を隔てて配置された上下一対のエリアＡを含む直方体空間をブロックＢと称する。
【００１４】  制御装置７０は、図３に示すように、ＣＰＵ７１を中心としたマイクロプロセッサとして構成され、ＣＰＵ７１の他に、ＲＯＭ７２やＨＤＤ７３、ＲＡＭ７４、駆動回路７５などを備える。駆動回路７５は、第１〜第５モータ５１〜５５を駆動するための回路である。制御装置７０には、第１〜第５エンコーダ６１〜６５や入力装置７６などから信号が入力される。制御装置７０からは、出力装置７７や第１〜第５モータ５１〜５５へ信号が出力される。なお、入力装置７６は、オペレータが入力操作を行なう入力デバイスである。また、出力装置７７は、各種情報を表示するための表示デバイスである。
【００１５】  次に、ロボットシステム１０の動作について説明する。図６は目的位置補正ルーチンのフローチャートである。この目的位置補正ルーチンを実行する前に、ロボットシステム１０は補正前処理ルーチンを実行するため、まずその補正前処理ルーチンについて説明し、その後に目的位置補正ルーチンについて説明する。図４は補正前処理ルーチンのフローチャートである。
【００１６】  制御装置７０のＣＰＵ７１は、補正前処理ルーチンを開始すると、１つの基準点Ｒの座標を読み出し（Ｓ１００）、その座標にロボット２０のエンドエフェクタ２３の先端が一致するようにロボット２０を制御する（Ｓ１１０）。次に、ＣＰＵ７１は、そのときの実際のエンドエフェクタ２３の先端のワールド座標上の停止位置を認識し（Ｓ１２０）、その停止位置を測定点Ｍとして今回の基準点Ｒに対応づけてＨＤＤ７３に記憶する（Ｓ１３０）。そして、ＣＰＵ７１は、補正前処理ルーチンが終了したか否か、すなわちすべての基準点Ｒに測定点Ｍを対応づけて記憶したか否かを判定する（Ｓ１４０）。判定結果が否定であれば、ＣＰＵ７１はＳ１００に戻って次の基準点Ｒについて同様の処理を実行する。一方、判定結果が肯定であれば、ＣＰＵ７１はこの補正前処理ルーチンを終了する。
【００１７】  ここで、基準点Ｒと測定点Ｍとの関係の一例について図５を参照して説明する。図５では、高さｚ1のエリアＡと高さｚ2のエリアＡとが上下一対になったブロックＢの基準点Ｒと測定点Ｍとの関係を示す説明図である。高さｚ1のエリアＡの基準点Ｒは（ｘr，ｙr，ｚ1）であるのに対し測定点Ｍは（ｘr1，ｙr1，ｚr1）であり、高さｚ2のエリアＡの基準点Ｒは（ｘr，ｙr，ｚ2）であるのに対し測定点Ｍは（ｘr2，ｙr2，ｚr2）である。
【００１８】  次に、目的位置補正ルーチンについて図６を参照して説明する。目的位置補正ルーチンでは、目的位置Ｔにエンドエフェクタ２３の先端を精度よく位置決めするために、ロボット２０に入力された目的位置Ｔを補正するルーチンである。
【００１９】  ＣＰＵ７１は、目的位置補正ルーチンを開始すると、まず、今回の目的位置Ｔの座標（ｘt，ｙt，ｚt）を読み出す（Ｓ２００）。目的位置Ｔの座標（ｘt，ｙt，ｚt）は、予めオペレータによって入力装置７６を介してロボット２０に入力されたものであり、ＨＤＤ７３に記憶されている。次に、ＣＰＵ７１は、特定ブロックＢｓを設定する（Ｓ２１０）。具体的には、ワールド座標において目的位置Ｔを含むブロックＢを特定ブロックＢｓに設定する。
【００２０】  次に、ＣＰＵ７１は、第１及び第２ブロックＢ１，Ｂ２を設定する（Ｓ２２０）。ここでは、ＣＰＵ７１は、特定ブロックＢｓに接しているブロックＢの中から第１及び第２ブロックＢ１，Ｂ２を設定する。具体的には、図７Ａに示すように、特定ブロックＢｓは、左前方ゾーンａ１、右前方ゾーンａ２、左後方ゾーンａ３及び右後方ゾーンａ４に４等分されている。ＣＰＵ７１は、目的位置Ｔがどのゾーンに含まれているかを認識し、認識したゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックＢ１，Ｂ２に設定する。例えば、図７Ａでは、目的位置Ｔが左前方ゾーンａ１に含まれている。この場合、図７Ｂのように、特定ブロックＢｓの前後左右の各面に接している４つのブロックのうち、左前方ゾーンａ１の２つの外面（図７Ａの太線参照）と接する左及び前ブロックが第１及び第２ブロックＢ１，Ｂ２に設定される。また、図８Ａでは、目的位置Ｔが右前方ゾーンａ２に含まれている。この場合、図８Ｂのように、右前方ゾーンａ２の２つの外面（図８Ａの太線参照）と接する右及び前ブロックが第１及び第２ブロックＢ１，Ｂ２に設定される。また、図９Ａでは、目的位置Ｔが左後方ゾーンａ３に含まれている。この場合、図９Ｂのように、左後方ゾーンａ３の２つの外面（図９Ａの太線参照）と接する左及び後ブロックが、第１及び第２ブロックＢ１，Ｂ２に設定される。また、図１０Ａでは、目的位置Ｔが右後方ゾーンａ４に含まれている。この場合、図１０Ｂのように、右後方ゾーンａ４の２つの外面（図１０Ａの太線参照）と接する右及び後ブロックが、第１及び第２ブロックＢ１，Ｂ２に設定される。
【００２１】  次に、ＣＰＵ７１は、目的位置ＴのＸ，Ｙ方向の補正量Δｘt，Δｙtを算出する（Ｓ２３０）。ここでは、ＣＰＵ７１は、目的位置ＴのＺ方向のズレ量を考慮することなく目的位置ＴのＸ，Ｙ方向の補正量Δｘt，Δｙtを算出する。
【００２２】  具体的には、ＣＰＵ７１は、まず、特定ブロックＢｓ、第１及び第２ブロックＢ１，Ｂ２の各々について目的位置高さｚtのＸＹ平面における仮想投影点ＨＰを求める。一例として、特定ブロックＢｓの仮想投影点ＨＰの求め方を図１１を参照しながら説明する。特定ブロックＢｓは、上下一対のエリアＡを含む。下方のエリアＡの高さがｚ1、上方のエリアＡの高さがｚ2だとする。下方のエリアＡの基準点Ｒ（ｘs，ｙs，ｚ1）の測定点Ｍ（ｘs1，ｙs1，ｚs1）は、その基準点Ｒから測定ズレ量だけずれた位置にある。この測定点Ｍ（ｘs1，ｙs1，ｚs1）を下方のエリアＡに投影した点（ｘs1，ｙs1，ｚ1）を下方投影点ＬＰと称する。下方投影点ＬＰは、Ｚ方向のズレ量を考慮しない測定点ということができる。一方、上方のエリアＡの基準点Ｒ（ｘs，ｙs，ｚ2）の測定点Ｍ（ｘs2，ｙs2，ｚs2）は、その基準点Ｒから測定ズレ量だけずれた位置にある。この測定点Ｍ（ｘs2，ｙs2，ｚs2）を上方のエリアＡに投影した点（ｘs2，ｙs2，ｚ2）を上方投影点ＵＰと称する。上方投影点ＵＰは、Ｚ方向のズレ量を考慮しない測定点ということができる。そして、下方投影点ＬＰと上方投影点ＵＰとを結んだ直線が目的位置高さｚtのＸＹ平面（図１１で網かけで示した平面）と交わる交点を数学的に求める。この交点は、特定ブロックＢｓにおける高さｚtのＸＹ平面上の仮想投影点ＨＰ（ｘst，ｙst，ｚt）である。仮想投影点ＨＰ（ｘst，ｙst，ｚt）は、高さｚtのＸＹ平面の仮想基準点ＨＲ（ｘs，ｙs，ｚt）の図示しない仮想測定点ＨＭ（ｘst，ｙst，ｚst）を高さｚtのＸＹ平面に投影した点ということができる。第１及び第２ブロックＢ１，Ｂ２の仮想投影点ＨＰも同様にして求める。
【００２３】  続いて、特定ブロックＢｓの仮想投影点ＨＰ（ＨＰｓという）と第１ブロックＢ１の仮想投影点ＨＰ（ＨＰ１という）とを結んだ直線と、特定ブロックＢｓの仮想投影点ＨＰｓと第２ブロックＢ２の仮想投影点ＨＰ（ＨＰ２という）とを結んだ直線とに基づいて、目的位置ＴのＸ，Ｙ方向の補正量Δｘt，Δｙtを算出する。なお、以下では、特定ブロックＢｓ、第１ブロックＢ１及び第２ブロックＢ２の仮想基準点ＨＲをそれぞれＨＲｓ，ＨＲ１，ＨＲ２と称する。仮想基準点ＨＲｓ，ＨＲ１，ＨＲ２や仮想投影点ＨＰｓ，ＨＰ１，ＨＰ２の座標は既に述べたとおり確定している。
【００２４】  補正量Δｘt，Δｙtの算出方法を図１２を参照しながら説明する。補正量Δｘtは以下のようにして求める。まず、２つの仮想投影点ＨＰｓ、ＨＰ１を結んだ直線の方程式（ｘとｙの方程式）と目的位置Ｔのｙ座標であるｙtとから、ｙ座標がｙtのときのｘ座標を求める。そして、そのｘ座標と特定ブロックＢｓの基準点Ｒｓのｘ座標（＝ｘs）との差分Δｘt’を求める。次に、以下の２点を結んだ直線の方程式を求める。１点は、横軸が仮想基準点ＨＲｓのｘ座標（＝ｘs）で、縦軸が仮想投影点ＨＰｓのｘ座標と仮想基準点ＨＲｓのｘ座標との差分である点である。もう１点は、横軸が仮想基準点ＨＲ２のｘ座標で、縦軸が仮想投影点ＨＰ２のｘ座標と仮想基準点ＨＲ２のｘ座標との差分である点である。この方程式から、横軸が目的位置Ｔのｘ座標（＝ｘt）のときの縦軸の値（ｘ方向の差分）を求める。求めたｘ方向の差分を仮想基準点ＨＲｓにおけるｘ方向の差分で除した値に差分Δｘt’を乗じることにより、目的位置におけるｘ方向の補正量Δｘtを得る。
【００２５】  補正量Δｙtも同様にして求める。まず、２つの仮想投影点ＨＰｓ、ＨＰ２を結んだ直線の方程式（ｘとｙの方程式）と目的位置Ｔのｘ座標であるｘtとから、ｘ座標がｘtのときのｙ座標を求め、そのｙ座標と特定ブロックＢｓの基準点Ｒｓのｙ座標（＝ｙs）との差分Δｙt’を求める。次に、以下の２点を結んだ直線の方程式を求める。１点は、横軸が仮想基準点ＨＲｓのｙ座標（＝ｙs）で、縦軸が仮想投影点ＨＰｓのｙ座標と仮想基準点ＨＲｓのｙ座標との差分である点である。もう１点は、横軸が仮想基準点ＨＲ２のｙ座標で、縦軸が仮想投影点ＨＰ２のｙ座標と仮想基準点ＨＲ２のｙ座標との差分である点である。この方程式から、横軸が目的位置Ｔのｙ座標（＝ｙt）のときの縦軸の値（ｙ方向の差分）を求める。求めたｙ方向の差分を仮想基準点ＨＲｓにおけるｙ方向の差分で除した値に差分Δｙt’を乗じることにより、目的位置におけるｙ方向の補正量Δｙtを得る。なお、図１２には、暫定補正後の目標位置ＣＴを点線の「＋」で示した。暫定補正後の目標位置ＣＴは、Ｚ方向のずれを考慮せずに暫定的に求めた補正後の目標位置であり、（ｘt−Δｘt，ｙt−Δｙt，ｚt）である。
【００２６】  図６に戻り、Ｓ２３０の後、ＣＰＵ７１は、目的位置ＴのＺ方向の補正量Δｚtを算出する（Ｓ２４０）。ここでは、ＣＰＵ７１は、目的位置高さｚtのＸＹ平面に対応する傾斜面を求め、その傾斜面に基づいて補正後の目的位置のＺ方向の補正量Δｚtを算出する。
【００２７】  具体的には、ＣＰＵ７１は、まず、特定ブロックＢｓ、第１及び第２ブロックＢ１，Ｂ２の各々について目的位置高さｚtのＸＹ平面における仮想基準点ＨＲに対応する仮想測定点ＨＭを求める。仮想測定点ＨＭは、仮に仮想基準点ＨＲの座標にエンドエフェクタ２３の先端が一致するようにロボット２０を制御した場合のエンドエフェクタ２３の先端の停止位置（予測値）である。
【００２８】  一例として、特定ブロックＢｓの仮想測定点ＨＭの求め方を図１３を参照しながら説明する。なお、図１３に示した点や位置は図１１に示した点や位置と同じである。ここでは、下方のエリアＡの測定点Ｍ（ｘs1，ｙs1，ｚs1）と上方のエリアＡの測定点Ｍ（ｘs2，ｙs2，ｚs2）とを結ぶ。この状態を白抜き矢印の方向（Ａ視）から見たときの様子を図１４に示す。横軸をＺ座標、縦軸をＺ方向のずれ量とするグラフにおいて、Ｚ座標がｚ1でずれ量が（ｚr1−ｚ1）である点と、Ｚ座標がｚ2でずれ量が（ｚr2−ｚ2）である点とを結んだ直線の方程式を求める。この方程式からＺ座標がｚtのときのずれ量すなわち（ｚst−ｚt）を求める。これにより、仮想測定点ＨＭを求めることができる。第１及び第２ブロックＢ１，Ｂ２の仮想測定点ＨＭも同様にして求める。
【００２９】  続いて、図１５に示すように、特定ブロックＢｓの仮想測定点ＨＭ（ＨＭｓという）と第１ブロックＢ１の仮想測定点ＨＭ（ＨＭ１という）と第２ブロックＢ２の仮想測定点ＨＭ（ＨＭ２という）とを含む傾斜面（水平面の場合もあり得る）の方程式を数学的に求める。そして、前出の暫定補正後の目的位置ＣＴ（ｘt−Δｘt，ｙt−Δｙt，ｚt）のｘ及びｙの値を傾斜面の方程式に代入し、そのときのｚ座標の値を求め、Ｚ方向の補正量Δｚt  を求める。
【００３０】  図６に戻り、Ｓ２４０の後、ＣＰＵ７１は、最終的な補正後の目的位置ＡＴ（ｘt−Δｘt，ｙt−Δｙt，ｚ−Δｚt ）を算出し（Ｓ２５０）、本ルーチンを終了する。その結果、ロボットシステム１０に目的位置Ｔが入力された場合、ＣＰＵ７１は補正後の目的位置ＡＴにエンドエフェクタ２３の先端が一致するようロボット２０を制御する。これにより、エンドエフェクタ２３の先端はワールド座標上の目的位置Ｔに精度よく一致する。
【００３１】  以上説明した本実施形態では、特定ブロックＢｓの基準点Ｒからの測定ズレ量（測定点Ｍと基準点Ｒとの差分）だけでなく、特定ブロックＢｓに接している第１及び第２ブロックＢ１，Ｂ２の基準点Ｒからの測定ズレ量を利用して目的位置Ｔを補正する。したがって、３次元マトリクス２５の動作空間に指定された目的位置Ｔへエンドエフェクタ２３の先端を精度よく位置決めすることができる。
【００３２】  また、特定ブロックＢｓの周囲の４つのブロックの中から目的位置Ｔに近い２つのブロックが第１及び第２ブロックに設定される。そのため、目的位置Ｔをより適切に補正することができる。
【００３３】  更に、本実施形態では、Ｚ方向のズレ量を考慮することなく目的位置ＴのＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して目的位置高さｚtのＸＹ平面に対応する傾斜面を求め、その傾斜面に基づいて目的位置のＺ方向の補正量を求めている。そのため、ＸＹＺ方向の補正量を一度に求める場合に比べて、各方向の補正量を容易に求めることができる。
【００３４】  更にまた、本実施形態では、特定ブロックＢｓの仮想投影点ＨＰｓと第１ブロックＢ１の仮想投影点ＨＰ１とを結んだ直線及び特定ブロックＢｓの仮想投影点ＨＰと第２ブロックＢ２の仮想投影点ＨＰ２とを結んだ直線に基づいて目的位置ＴのＸ方向及びＹ方向の補正量を求めている。そのため、目的位置ＴのＸ方向及びＹ方向の補正量を比較的簡単に求めることができる。
【００３５】  そしてまた、本実施形態では、特定ブロックＢｓ、第１ブロックＢ１及び第２ブロックＢ２の各々について、上方投影位置ＵＰと下方投影位置ＬＰとを結んだ直線と、上方の測定点Ｍと下方の測定点Ｍとを結んだ直線とを用いて、目的位置高さｚtにおけるズレ量を求めている。そして、特定ブロックＢｓ、第１ブロックＢ１及び第２ブロックＢ２の各々の目的位置高さｚtにおけるズレ量に基づいて傾斜面を求めている。そのため、目的位置ＴのＺ方向の補正量を比較的簡単に求めることができる。
【００３６】  なお、本開示は、上述した実施形態に何ら限定されることはなく、本開示の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。
【００３７】  例えば、上述した実施形態では、ロボット２０として垂直多関節ロボットを例示したが、特にこれに限定されない。例えば、ロボット２０として直交ロボット、水平多関節ロボット、パラレルリンクロボットなどを採用してもよい。但し、本開示の目的位置補正方法は、ロボット２０として垂直多関節ロボットを用いる場合に採用するのが適している。
【００３８】  上述した実施形態では、特定ブロックＢｓの前後左右の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定したが、特にこれに限定されない。例えば、特定ブロックＢｓの上下前後の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定してもよい。あるいは、特定ブロックＢｓの上下左右の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定してもよい。これらの場合も、特定ブロックＢｓの周囲の４つのブロックの中から目的位置Ｔに近い２つのブロックを第１及び第２ブロックに設定するのが好ましい。
【００３９】  上述した実施形態では、特定ブロックＢｓに接する４つのブロックの中から目的位置Ｔに近い２つのブロックを第１及び第２ブロックに設定したが、特定ブロックＢｓに接するブロックが２つ又は３つの場合には、この方法によらずに第１及び第２ブロックを設定してもよい。例えば、特定ブロックＢｓが３次元マトリクス２５の角に位置している場合のように特定ブロックＢｓに接するブロックが２つしかない場合には、その２つのブロックを第１及び第２ブロックに設定すればよい。
【００４０】  本開示の作業ロボットの目的位置補正方法は、以下のように構成してもよい。
【００４１】  本開示の作業ロボットの目的位置補正方法において、前記工程（ａ）では、前記特定ブロックの前後左右の各面には前ブロック、後ブロック、左ブロック及び右ブロックが接しており、前記第１及び第２ブロックを、前記目的位置に基づいて、前記前ブロック、前記後ブロック、前記左ブロック及び前記右ブロックのうちの２つのブロックから選択してもよい。この場合、前記工程（ａ）では、前記特定ブロックを４等分して左前方ゾーン、右前方ゾーン、左後方ゾーン及び右後方ゾーンを作成し、該４つのゾーンの中から前記目的位置を含むゾーンを選択し、選択されたゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックに設定してもよい。こうすれば、特定ブロックの周囲の４つのブロックの中から目的位置に近い２つのブロックが第１及び第２ブロックに設定される。そのため、目的位置をより適切に補正することができる。
【００４２】  本開示の作業ロボットの目的位置補正方法において、前記工程（ｂ）では、まず、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して前記目的位置のＺ座標を含むＸＹ平面に対応する傾斜面（水平面の場合も含む）を求め、前記傾斜面に基づいて前記目的位置のＺ方向の補正量を求めてもよい。こうすれば、ＸＹＺ方向の補正量を一度に求める場合に比べて、各方向の補正量を容易に求めることができる。
【００４３】  前記工程（ｂ）で、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置を前記上エリアに投影した上方投影位置と、前記下エリアの前記基準点から前記測定ズレ量だけずれた位置を前記下エリアに投影した下方投影位置とを結んだ直線が、前記目的位置のＺ座標を含むＸＹ平面と交わる交点を求め、前記特定ブロックの前記交点と前記第１ブロックの前記交点とを結んだ直線及び前記特定ブロックの前記交点と前記第２ブロックの前記交点とを結んだ直線に基づいて前記目的位置のＸ方向及びＹ方向の補正量を求めてもよい。こうすれば、目的位置のＸ方向及びＹ方向の補正量を比較的簡単に求めることができる。
【００４４】  また、前記工程（ｂ）で、Ｚ方向のズレ量を考慮して前記目的位置のＺ方向の補正量を求める際の前記傾斜面を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記前記上方投影位置と前記下方投影位置とを結んだ直線と、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置と前記下エリアの前記基準点から前記測定ズレ量だけずれた位置とを結んだ直線とを用いて、前記目的位置のＺ座標でのズレ量を求め、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々の前記目的位置のＺ座標でのズレ量に基づいて前記傾斜面を求めてもよい。こうすれば、目的位置のＺ方向の補正量を比較的簡単に求めることができる。
【産業上の利用可能性】
【００４５】  本開示は、作業ロボットを利用した部品の組立作業を行う機械産業などに利用可能である。
【符号の説明】
【００４６】１０  ロボットシステム、２０  ロボット、２２  アーム、２３  エンドエフェクタ、２５  ３次元マトリクス、３１〜３６  第１〜第６リンク、４１〜４５  第１〜第５関節、５１〜５５  第１〜第５モータ、６１〜６５  第１〜第５エンコーダ、７０  制御装置、７１  ＣＰＵ、７２  ＲＯＭ、７３  ＨＤＤ、７４  ＲＡＭ、７５  駆動回路、７６  入力装置、７７  出力装置、Ａ  エリア、ａ１  左前方ゾーン、ａ２  右前方ゾーン、ａ３  左後方ゾーン、ａ４  右後方ゾーン、ＡＴ  補正後の目標位置、Ｂ  ブロック、Ｂ１  第１ブロック、Ｂ２  第２ブロック、Ｂｓ  特定ブロック、ＣＴ  暫定補正後の目標位置、ＨＭ  仮想測定点、ＨＰ  仮想投影点、ＨＲ  仮想基準点、ＬＰ  下方投影点、Ｍ  測定点、Ｒ  基準点、Ｔ  目標位置、ＵＰ  上方投影点。
【図１】
0006705017.tif 000002
【図２】
0006705017.tif 000003
【図３】
0006705017.tif 000004
【図４】
0006705017.tif 000005
【図５】
0006705017.tif 000006
【図６】
0006705017.tif 000007
【図７Ａ】
0006705017.tif 000008
【図７Ｂ】
0006705017.tif 000009
【図８Ａ】
0006705017.tif 000010
【図８Ｂ】
0006705017.tif 000011
【図９Ａ】
0006705017.tif 000012
【図９Ｂ】
0006705017.tif 000013
【図１０Ａ】
0006705017.tif 000014
【図１０Ｂ】
0006705017.tif 000015
【図１１】
0006705017.tif 000016
【図１２】
0006705017.tif 000017
【図１３】
0006705017.tif 000018
【図１４】
0006705017.tif 000019
【図１５】
0006705017.tif 000020