JP6705017B2

JP6705017B2 - 作業ロボットの目的位置補正方法

Info

Publication number: JP6705017B2
Application number: JP2018556060A
Authority: JP
Inventors: 英希保坂
Original assignee: Individual
Current assignee: Individual
Priority date: 2016-12-13
Filing date: 2016-12-13
Publication date: 2020-06-03
Anticipated expiration: 2036-12-13
Also published as: EP3556519B1; EP3556519A4; US20200061812A1; EP3556519A1; US10940586B2; CN110072675A; WO2018109828A1; CN110072675B; JPWO2018109828A1

Description

本明細書は、作業ロボットの目的位置補正方法について開示する。

従来、作業ロボットとしては、目標点に対してＤＨパラメータを適用して座標変換を行なうことでロボットの動作を制御するものが提案されている（例えば、特許文献１参照）。ＤＨパラメータの設定は以下のようにして行なわれる。即ち、ロボットの制御装置は、ロボットの動作空間を複数の領域に分割し、分割した領域ごとに測定点を設定する。次に、制御装置は、測定点へロボットを移動させて３次元の位置データを取得する。そして、制御装置は、取得した位置データと測定点との誤差からＤＨパラメータを導出する。制御装置は、目標点に基づいてロボットの動作を制御する際、複数の領域ごとに導出したＤＨパラメータのうち目標点が属する領域のＤＨパラメータを選択し、目標点に対して選択したＤＨパラメータを適用して座標変換を行なう。

特開２００９−１４８８５０号公報

しかし、上述した作業ロボットでは、座標変換に用いるＤＨパラメータを最適化しても、十分な作業精度を確保できない場合があった。そのため、作業ロボットの作業精度をより向上させることが望まれている。

本開示は、３次元マトリクスの動作空間に指定された目的位置へ作業ロボットを精度よく位置決めすることを主目的とする。

本開示の作業ロボットの目的位置補正方法は、
左右方向をＸ軸、前後方向をＹ軸、上下方向をＺ軸としたとき、ＸＹ平面に平行で基準点を持つ四角形状のエリアをＸＹ方向に連ねたマトリクス平面を、Ｚ方向に所定間隔おきに積み上げることにより３次元マトリクスを構築し、前記３次元マトリクスの動作空間に指定された作業ロボットの目的位置を補正する方法であって、
前記所定間隔を隔てて配置された上下一対の前記エリアを含む直方体空間をブロックと称し、前記目的位置を前記３次元マトリクスの動作空間に指定したときの前記目的位置を含むブロックを特定ブロックと称するとした場合、
（ａ）前記特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する工程と、
（ｂ）前記特定ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量、前記第１ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量、及び、前記第２ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットの測定ズレ量に基づいて前記目的位置を補正する工程と、
を含むものである。

この目的位置補正方法では、目的位置を含む特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する。そして、特定ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量、第１ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量、及び、第２ブロックの上エリア及び下エリアの各々の基準点とその基準点からの作業ロボットの測定ズレ量に基づいて、目的位置を補正する。このように、特定ブロックの基準点からの測定ズレ量だけでなく、特定ブロックに接している２つのブロックの基準点からの測定ズレ量を利用して目的位置を補正する。したがって、３次元マトリクスの動作空間に指定された目的位置へ作業ロボットを精度よく位置決めすることができる。

ロボットシステム１０の構成の概略を示す構成図。３次元マトリクス２５の説明図。ロボット２０と制御装置７０との電気的な接続関係を示すブロック図。補正前処理ルーチンのフローチャート。基準点Ｒと測定点Ｍとの関係を示す説明図。目的位置補正ルーチンのフローチャート。目的位置Ｔが左前方ゾーンａ１にあるときの特定ブロックＢｓの上面図。目的位置Ｔが左前方ゾーンａ１にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。目的位置Ｔが右前方ゾーンａ２にあるときの特定ブロックＢｓの上面図。目的位置Ｔが右前方ゾーンａ２にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。目的位置Ｔが左後方ゾーンａ３にあるときの特定ブロックＢｓの上面図。目的位置Ｔが左後方ゾーンａ３にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。目的位置Ｔが右後方ゾーンａ４にあるときの特定ブロックＢｓの上面図。目的位置Ｔが右後方ゾーンａ４にあるときの第１及び第２ブロックＢ１，Ｂ２の説明図。特定ブロックＢｓの仮想投影点ＨＰを求めるときの説明図。補正量Δｘ_t，Δｙ_tの算出方法の説明図。特定ブロックＢｓの仮想測定点ＨＭの求め方の説明図。図１３のＡ視図。仮想測定点ＨＭｓ，ＨＭ１，ＨＭ２を含む傾斜面の説明図。

次に、本開示を実施するための形態について図面を参照しながら説明する。

図１は、ロボットシステム１０の構成の概略を示す構成図である。図２は、３次元マトリクス２５の説明図である。図３は、ロボット２０と制御装置７０との電気的な接続関係を示すブロック図である。なお、図１，２中、左右方向はＸ軸方向であり、前後方向はＹ軸方向であり、上下方向はＺ軸方向である。

ロボットシステム１０は、ロボット２０と、ロボット２０を制御する制御装置７０と、を備える。ロボットシステム１０は、ワークをピックアップし、ピックアップしたワークを対象物にプレースするシステムとして構成されている。本実施形態では、ロボットシステム１０は、ロボット２０を用いてワークに対して作業を行なうシステムであれば、如何なるシステムにも適用できる。例えば、ロボットシステム１０は、ロボット２０により部品を採取して基板上に実装する部品実装システムに適用できる。

ロボット２０は、５軸の垂直多関節アーム（以下、アームという）２２と、ロボット２０の手先である図示しないエンドエフェクタ２３と、を備える。アーム２２は、６つのリンク（第１〜第６リンク３１〜３６）と、各リンクを回転または旋回可能に連結する５つの関節（第１〜第５関節４１〜４５）と、を備える。各関節（第１〜第５関節４１〜４５）は、対応する関節を駆動するモータ（第１〜第５モータ５１〜５５、図３参照）、対応するモータの回転角度を検出するエンコーダ（第１〜第５エンコーダ６１〜６５、図３参照）と、を備える。本実施形態では、モータはサーボモータであり、エンコーダはロータリエンコーダである。エンドエフェクタ２３は、アーム２２の先端リンク（第６リンク３６）に取り付けられ、部品（ワーク）の保持とその解除とが可能となっている。エンドエフェクタ２３は、例えば、メカチャックや吸着ノズル、電磁石などを用いることができる。

ここで、ロボット２０の動作空間について図２を参照しながら説明する。３次元マトリクス２５は、ロボット２０の動作空間である。ロボット２０のエンドエフェクタ２３の先端は、３次元マトリクス２５内に目的位置が設定されると、制御装置７０によってその目的位置に移動されるが、この点は後で詳述する。３次元マトリクス２５は、マトリクス平面ＰをＺ方向に所定間隔おきに積み上げることにより構築されている。３次元マトリクス２５の下からｎ番めのマトリクス平面Ｐをマトリクス平面Ｐ_n（ｎは自然数）と称する。また、マトリクス平面Ｐ_n の高さすなわちｚ座標をｚ_nと表す。マトリクス平面Ｐは、ＸＹ平面に平行で中心に基準点Ｒを持つ矩形のエリアＡがＸＹ方向に複数連なったものである。基準点Ｒはワールド座標（絶対座標）上に設定されている。すべての基準点Ｒの座標は、予めＨＤＤ７３に保存されている。本実施形態では、所定間隔を隔てて配置された上下一対のエリアＡを含む直方体空間をブロックＢと称する。

制御装置７０は、図３に示すように、ＣＰＵ７１を中心としたマイクロプロセッサとして構成され、ＣＰＵ７１の他に、ＲＯＭ７２やＨＤＤ７３、ＲＡＭ７４、駆動回路７５などを備える。駆動回路７５は、第１〜第５モータ５１〜５５を駆動するための回路である。制御装置７０には、第１〜第５エンコーダ６１〜６５や入力装置７６などから信号が入力される。制御装置７０からは、出力装置７７や第１〜第５モータ５１〜５５へ信号が出力される。なお、入力装置７６は、オペレータが入力操作を行なう入力デバイスである。また、出力装置７７は、各種情報を表示するための表示デバイスである。

次に、ロボットシステム１０の動作について説明する。図６は目的位置補正ルーチンのフローチャートである。この目的位置補正ルーチンを実行する前に、ロボットシステム１０は補正前処理ルーチンを実行するため、まずその補正前処理ルーチンについて説明し、その後に目的位置補正ルーチンについて説明する。図４は補正前処理ルーチンのフローチャートである。

制御装置７０のＣＰＵ７１は、補正前処理ルーチンを開始すると、１つの基準点Ｒの座標を読み出し（Ｓ１００）、その座標にロボット２０のエンドエフェクタ２３の先端が一致するようにロボット２０を制御する（Ｓ１１０）。次に、ＣＰＵ７１は、そのときの実際のエンドエフェクタ２３の先端のワールド座標上の停止位置を認識し（Ｓ１２０）、その停止位置を測定点Ｍとして今回の基準点Ｒに対応づけてＨＤＤ７３に記憶する（Ｓ１３０）。そして、ＣＰＵ７１は、補正前処理ルーチンが終了したか否か、すなわちすべての基準点Ｒに測定点Ｍを対応づけて記憶したか否かを判定する（Ｓ１４０）。判定結果が否定であれば、ＣＰＵ７１はＳ１００に戻って次の基準点Ｒについて同様の処理を実行する。一方、判定結果が肯定であれば、ＣＰＵ７１はこの補正前処理ルーチンを終了する。

ここで、基準点Ｒと測定点Ｍとの関係の一例について図５を参照して説明する。図５では、高さｚ₁のエリアＡと高さｚ₂のエリアＡとが上下一対になったブロックＢの基準点Ｒと測定点Ｍとの関係を示す説明図である。高さｚ₁のエリアＡの基準点Ｒは（ｘ_r，ｙ_r，ｚ₁）であるのに対し測定点Ｍは（ｘ_r1，ｙ_r1，ｚ_r1）であり、高さｚ₂のエリアＡの基準点Ｒは（ｘ_r，ｙ_r，ｚ₂）であるのに対し測定点Ｍは（ｘ_r2，ｙ_r2，ｚ_r2）である。

次に、目的位置補正ルーチンについて図６を参照して説明する。目的位置補正ルーチンでは、目的位置Ｔにエンドエフェクタ２３の先端を精度よく位置決めするために、ロボット２０に入力された目的位置Ｔを補正するルーチンである。

ＣＰＵ７１は、目的位置補正ルーチンを開始すると、まず、今回の目的位置Ｔの座標（ｘ_t，ｙ_t，ｚ_t）を読み出す（Ｓ２００）。目的位置Ｔの座標（ｘ_t，ｙ_t，ｚ_t）は、予めオペレータによって入力装置７６を介してロボット２０に入力されたものであり、ＨＤＤ７３に記憶されている。次に、ＣＰＵ７１は、特定ブロックＢｓを設定する（Ｓ２１０）。具体的には、ワールド座標において目的位置Ｔを含むブロックＢを特定ブロックＢｓに設定する。

次に、ＣＰＵ７１は、第１及び第２ブロックＢ１，Ｂ２を設定する（Ｓ２２０）。ここでは、ＣＰＵ７１は、特定ブロックＢｓに接しているブロックＢの中から第１及び第２ブロックＢ１，Ｂ２を設定する。具体的には、図７Ａに示すように、特定ブロックＢｓは、左前方ゾーンａ１、右前方ゾーンａ２、左後方ゾーンａ３及び右後方ゾーンａ４に４等分されている。ＣＰＵ７１は、目的位置Ｔがどのゾーンに含まれているかを認識し、認識したゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックＢ１，Ｂ２に設定する。例えば、図７Ａでは、目的位置Ｔが左前方ゾーンａ１に含まれている。この場合、図７Ｂのように、特定ブロックＢｓの前後左右の各面に接している４つのブロックのうち、左前方ゾーンａ１の２つの外面（図７Ａの太線参照）と接する左及び前ブロックが第１及び第２ブロックＢ１，Ｂ２に設定される。また、図８Ａでは、目的位置Ｔが右前方ゾーンａ２に含まれている。この場合、図８Ｂのように、右前方ゾーンａ２の２つの外面（図８Ａの太線参照）と接する右及び前ブロックが第１及び第２ブロックＢ１，Ｂ２に設定される。また、図９Ａでは、目的位置Ｔが左後方ゾーンａ３に含まれている。この場合、図９Ｂのように、左後方ゾーンａ３の２つの外面（図９Ａの太線参照）と接する左及び後ブロックが、第１及び第２ブロックＢ１，Ｂ２に設定される。また、図１０Ａでは、目的位置Ｔが右後方ゾーンａ４に含まれている。この場合、図１０Ｂのように、右後方ゾーンａ４の２つの外面（図１０Ａの太線参照）と接する右及び後ブロックが、第１及び第２ブロックＢ１，Ｂ２に設定される。

次に、ＣＰＵ７１は、目的位置ＴのＸ，Ｙ方向の補正量Δｘ_t，Δｙ_tを算出する（Ｓ２３０）。ここでは、ＣＰＵ７１は、目的位置ＴのＺ方向のズレ量を考慮することなく目的位置ＴのＸ，Ｙ方向の補正量Δｘ_t，Δｙ_tを算出する。

具体的には、ＣＰＵ７１は、まず、特定ブロックＢｓ、第１及び第２ブロックＢ１，Ｂ２の各々について目的位置高さｚ_tのＸＹ平面における仮想投影点ＨＰを求める。一例として、特定ブロックＢｓの仮想投影点ＨＰの求め方を図１１を参照しながら説明する。特定ブロックＢｓは、上下一対のエリアＡを含む。下方のエリアＡの高さがｚ₁、上方のエリアＡの高さがｚ₂だとする。下方のエリアＡの基準点Ｒ（ｘ_s，ｙ_s，ｚ₁）の測定点Ｍ（ｘ_s1，ｙ_s1，ｚ_s1）は、その基準点Ｒから測定ズレ量だけずれた位置にある。この測定点Ｍ（ｘ_s1，ｙ_s1，ｚ_s1）を下方のエリアＡに投影した点（ｘ_s1，ｙ_s1，ｚ₁）を下方投影点ＬＰと称する。下方投影点ＬＰは、Ｚ方向のズレ量を考慮しない測定点ということができる。一方、上方のエリアＡの基準点Ｒ（ｘ_s，ｙ_s，ｚ₂）の測定点Ｍ（ｘ_s2，ｙ_s2，ｚ_s ₂）は、その基準点Ｒから測定ズレ量だけずれた位置にある。この測定点Ｍ（ｘ_s2，ｙ_s2，ｚ_s2）を上方のエリアＡに投影した点（ｘ_s2，ｙ_s2，ｚ₂）を上方投影点ＵＰと称する。上方投影点ＵＰは、Ｚ方向のズレ量を考慮しない測定点ということができる。そして、下方投影点ＬＰと上方投影点ＵＰとを結んだ直線が目的位置高さｚ_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）とを結ぶ。この状態を白抜き矢印の方向（Ａ視）から見たときの様子を図１４に示す。横軸をＺ座標、縦軸をＺ方向のずれ量とするグラフにおいて、Ｚ座標がｚ₁でずれ量が（ｚ_r1−ｚ₁）である点と、Ｚ座標がｚ₂でずれ量が（ｚ_r2−ｚ₂）である点とを結んだ直線の方程式を求める。この方程式からＺ座標がｚ_tのときのずれ量すなわち（ｚ_s _t−ｚ_t）を求める。これにより、仮想測定点ＨＭを求めることができる。第１及び第２ブロックＢ１，Ｂ２の仮想測定点ＨＭも同様にして求める。

続いて、図１５に示すように、特定ブロックＢｓの仮想測定点ＨＭ（ＨＭｓという）と第１ブロックＢ１の仮想測定点ＨＭ（ＨＭ１という）と第２ブロックＢ２の仮想測定点ＨＭ（ＨＭ２という）とを含む傾斜面（水平面の場合もあり得る）の方程式を数学的に求める。そして、前出の暫定補正後の目的位置ＣＴ（ｘ_t−Δｘ_t，ｙ_t−Δｙ_t，ｚ_t）のｘ及びｙの値を傾斜面の方程式に代入し、そのときのｚ座標の値を求め、Ｚ方向の補正量Δｚ _t を求める。

図６に戻り、Ｓ２４０の後、ＣＰＵ７１は、最終的な補正後の目的位置ＡＴ（ｘ_t−Δｘ_t，ｙ_t−Δｙ_t，ｚ−Δｚ_t ）を算出し（Ｓ２５０）、本ルーチンを終了する。その結果、ロボットシステム１０に目的位置Ｔが入力された場合、ＣＰＵ７１は補正後の目的位置ＡＴにエンドエフェクタ２３の先端が一致するようロボット２０を制御する。これにより、エンドエフェクタ２３の先端はワールド座標上の目的位置Ｔに精度よく一致する。

以上説明した本実施形態では、特定ブロックＢｓの基準点Ｒからの測定ズレ量（測定点Ｍと基準点Ｒとの差分）だけでなく、特定ブロックＢｓに接している第１及び第２ブロックＢ１，Ｂ２の基準点Ｒからの測定ズレ量を利用して目的位置Ｔを補正する。したがって、３次元マトリクス２５の動作空間に指定された目的位置Ｔへエンドエフェクタ２３の先端を精度よく位置決めすることができる。

また、特定ブロックＢｓの周囲の４つのブロックの中から目的位置Ｔに近い２つのブロックが第１及び第２ブロックに設定される。そのため、目的位置Ｔをより適切に補正することができる。

更に、本実施形態では、Ｚ方向のズレ量を考慮することなく目的位置ＴのＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して目的位置高さｚ_tのＸＹ平面に対応する傾斜面を求め、その傾斜面に基づいて目的位置のＺ方向の補正量を求めている。そのため、ＸＹＺ方向の補正量を一度に求める場合に比べて、各方向の補正量を容易に求めることができる。

更にまた、本実施形態では、特定ブロックＢｓの仮想投影点ＨＰｓと第１ブロックＢ１の仮想投影点ＨＰ１とを結んだ直線及び特定ブロックＢｓの仮想投影点ＨＰと第２ブロックＢ２の仮想投影点ＨＰ２とを結んだ直線に基づいて目的位置ＴのＸ方向及びＹ方向の補正量を求めている。そのため、目的位置ＴのＸ方向及びＹ方向の補正量を比較的簡単に求めることができる。

そしてまた、本実施形態では、特定ブロックＢｓ、第１ブロックＢ１及び第２ブロックＢ２の各々について、上方投影位置ＵＰと下方投影位置ＬＰとを結んだ直線と、上方の測定点Ｍと下方の測定点Ｍとを結んだ直線とを用いて、目的位置高さｚ_tにおけるズレ量を求めている。そして、特定ブロックＢｓ、第１ブロックＢ１及び第２ブロックＢ２の各々の目的位置高さｚ_tにおけるズレ量に基づいて傾斜面を求めている。そのため、目的位置ＴのＺ方向の補正量を比較的簡単に求めることができる。

なお、本開示は、上述した実施形態に何ら限定されることはなく、本開示の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。

例えば、上述した実施形態では、ロボット２０として垂直多関節ロボットを例示したが、特にこれに限定されない。例えば、ロボット２０として直交ロボット、水平多関節ロボット、パラレルリンクロボットなどを採用してもよい。但し、本開示の目的位置補正方法は、ロボット２０として垂直多関節ロボットを用いる場合に採用するのが適している。

上述した実施形態では、特定ブロックＢｓの前後左右の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定したが、特にこれに限定されない。例えば、特定ブロックＢｓの上下前後の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定してもよい。あるいは、特定ブロックＢｓの上下左右の各面に接する４つのブロックの中から２つのブロックを第１及び第２ブロックに設定してもよい。これらの場合も、特定ブロックＢｓの周囲の４つのブロックの中から目的位置Ｔに近い２つのブロックを第１及び第２ブロックに設定するのが好ましい。

上述した実施形態では、特定ブロックＢｓに接する４つのブロックの中から目的位置Ｔに近い２つのブロックを第１及び第２ブロックに設定したが、特定ブロックＢｓに接するブロックが２つ又は３つの場合には、この方法によらずに第１及び第２ブロックを設定してもよい。例えば、特定ブロックＢｓが３次元マトリクス２５の角に位置している場合のように特定ブロックＢｓに接するブロックが２つしかない場合には、その２つのブロックを第１及び第２ブロックに設定すればよい。

本開示の作業ロボットの目的位置補正方法は、以下のように構成してもよい。

本開示の作業ロボットの目的位置補正方法において、前記工程（ａ）では、前記特定ブロックの前後左右の各面には前ブロック、後ブロック、左ブロック及び右ブロックが接しており、前記第１及び第２ブロックを、前記目的位置に基づいて、前記前ブロック、前記後ブロック、前記左ブロック及び前記右ブロックのうちの２つのブロックから選択してもよい。この場合、前記工程（ａ）では、前記特定ブロックを４等分して左前方ゾーン、右前方ゾーン、左後方ゾーン及び右後方ゾーンを作成し、該４つのゾーンの中から前記目的位置を含むゾーンを選択し、選択されたゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックに設定してもよい。こうすれば、特定ブロックの周囲の４つのブロックの中から目的位置に近い２つのブロックが第１及び第２ブロックに設定される。そのため、目的位置をより適切に補正することができる。

本開示の作業ロボットの目的位置補正方法において、前記工程（ｂ）では、まず、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して前記目的位置のＺ座標を含むＸＹ平面に対応する傾斜面（水平面の場合も含む）を求め、前記傾斜面に基づいて前記目的位置のＺ方向の補正量を求めてもよい。こうすれば、ＸＹＺ方向の補正量を一度に求める場合に比べて、各方向の補正量を容易に求めることができる。

前記工程（ｂ）で、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置を前記上エリアに投影した上方投影位置と、前記下エリアの前記基準点から前記測定ズレ量だけずれた位置を前記下エリアに投影した下方投影位置とを結んだ直線が、前記目的位置のＺ座標を含むＸＹ平面と交わる交点を求め、前記特定ブロックの前記交点と前記第１ブロックの前記交点とを結んだ直線及び前記特定ブロックの前記交点と前記第２ブロックの前記交点とを結んだ直線に基づいて前記目的位置のＸ方向及びＹ方向の補正量を求めてもよい。こうすれば、目的位置のＸ方向及びＹ方向の補正量を比較的簡単に求めることができる。

また、前記工程（ｂ）で、Ｚ方向のズレ量を考慮して前記目的位置のＺ方向の補正量を求める際の前記傾斜面を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記前記上方投影位置と前記下方投影位置とを結んだ直線と、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置と前記下エリアの前記基準点から前記測定ズレ量だけずれた位置とを結んだ直線とを用いて、前記目的位置のＺ座標でのズレ量を求め、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々の前記目的位置のＺ座標でのズレ量に基づいて前記傾斜面を求めてもよい。こうすれば、目的位置のＺ方向の補正量を比較的簡単に求めることができる。

本開示は、作業ロボットを利用した部品の組立作業を行う機械産業などに利用可能である。

１０ロボットシステム、２０ロボット、２２アーム、２３エンドエフェクタ、２５３次元マトリクス、３１〜３６第１〜第６リンク、４１〜４５第１〜第５関節、５１〜５５第１〜第５モータ、６１〜６５第１〜第５エンコーダ、７０制御装置、７１ＣＰＵ、７２ＲＯＭ、７３ＨＤＤ、７４ＲＡＭ、７５駆動回路、７６入力装置、７７出力装置、Ａエリア、ａ１左前方ゾーン、ａ２右前方ゾーン、ａ３
左後方ゾーン、ａ４右後方ゾーン、ＡＴ補正後の目標位置、Ｂブロック、Ｂ１第１ブロック、Ｂ２第２ブロック、Ｂｓ特定ブロック、ＣＴ暫定補正後の目標位置、ＨＭ仮想測定点、ＨＰ仮想投影点、ＨＲ仮想基準点、ＬＰ下方投影点、Ｍ測定点、Ｒ基準点、Ｔ目標位置、ＵＰ上方投影点。

Claims

左右方向をＸ軸、前後方向をＹ軸、上下方向をＺ軸としたとき、ＸＹ平面に平行で基準点を持つ四角形状のエリアをＸＹ方向に連ねたマトリクス平面を、Ｚ方向に所定間隔おきに積み上げることにより３次元マトリクスを構築し、前記３次元マトリクスの動作空間に指定された作業ロボットの目的位置を補正する方法であって、
前記所定間隔を隔てて配置された上下一対の前記エリアを含む直方体空間をブロックと称し、前記目的位置を前記３次元マトリクスの動作空間に指定したときの前記目的位置を含むブロックを特定ブロックと称するとした場合、
（ａ）前記特定ブロックに接している２つのブロックを第１及び第２ブロックに設定する工程と、
（ｂ）前記特定ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記作業ロボットのエンドエフェクタの制御点の測定ズレ量、前記第１ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記エンドエフェクタの制御点の測定ズレ量、及び、前記第２ブロックの上エリア及び下エリアの各々の基準点と前記基準点からの前記エンドエフェクタの制御点の測定ズレ量に基づいて前記目的位置を補正する工程と、
を含み、
前記エンドエフェクタの制御点は、前記基準点に前記エンドエフェクタの先端が一致するように前記作業ロボットを制御したときの前記エンドエフェクタの先端の位置である、
作業ロボットの目的位置補正方法。
前記工程（ａ）では、前記特定ブロックの前後左右の各面には前ブロック、後ブロック、左ブロック及び右ブロックが接しており、前記第１及び第２ブロックを、前記目的位置に基づいて、前記前ブロック、前記後ブロック、前記左ブロック及び前記右ブロックのうちの２つのブロックから選択する、
請求項１に記載の作業ロボットの目的位置補正方法。
前記工程（ａ）では、前記特定ブロックを４等分して左前方ゾーン、右前方ゾーン、左後方ゾーン及び右後方ゾーンを作成し、該４つのゾーンの中から前記目的位置を含むゾーンを選択し、選択されたゾーンの２つの外面とそれぞれ接するブロックを第１及び第２ブロックに設定する、
請求項２に記載の作業ロボットの目的位置補正方法。
前記工程（ｂ）では、まず、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求め、その後、Ｚ方向のズレ量を考慮して前記目的位置のＺ座標を含むＸＹ平面に対応する傾斜面（水平面の場合も含む）を求め、前記傾斜面に基づいて前記目的位置のＺ方向の補正量を求める、
請求項１〜３のいずれか１項に記載の作業ロボットの目的位置補正方法。
前記工程（ｂ）で、Ｚ方向のズレ量を考慮することなく前記目的位置のＸ方向及びＹ方向の補正量を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置を前記上エリアに投影した上方投影位置と、前記下エリアの前記基準点から前記測定ズレ量だけずれた位置を前記下エリアに投影した下方投影位置とを結んだ直線が、前記目的位置のＺ座標を含むＸＹ平面と交わる交点を求め、前記特定ブロックの前記交点と前記第１ブロックの前記交点とを結んだ直線及び前記特定ブロックの前記交点と前記第２ブロックの前記交点とを結んだ直線に基づいて前記目的位置のＸ方向及びＹ方向の補正量を求める、
請求項４に記載の作業ロボットの目的位置補正方法。
前記工程（ｂ）で、Ｚ方向のズレ量を考慮して前記目的位置のＺ方向の補正量を求める際の前記傾斜面を求めるにあたり、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々について、前記上方投影位置と前記下方投影位置とを結んだ直線と、前記上エリアの前記基準点から前記測定ズレ量だけずれた位置と前記下エリアの前記基準点から前記測定ズレ量だけずれた位置とを結んだ直線とを用いて、前記目的位置のＺ座標でのズレ量を求め、前記特定ブロック、前記第１ブロック及び前記第２ブロックの各々の前記目的位置のＺ座標でのズレ量に基づいて前記傾斜面を求める、
請求項４又は５に記載の作業ロボットの目的位置補正方法。