JP2010151766A

JP2010151766A - ロボットのツール位置検出方法、ロボットと対象物との相対位置検出方法、及びそれらの装置

Info

Publication number: JP2010151766A
Application number: JP2008333175A
Authority: JP
Inventors: Tetsuya Kubota; 哲也久保田; Jun Fujimori; 潤藤森; Ryota Ienaka; 良太家中; Yuhei Horiuchi; 悠平堀内
Original assignee: Kawasaki Heavy Industries Ltd
Current assignee: Kawasaki Heavy Industries Ltd
Priority date: 2008-12-26
Filing date: 2008-12-26
Publication date: 2010-07-08
Anticipated expiration: 2028-12-26
Also published as: JP5319271B2

Abstract

【課題】ツール位置やロボットと対象物との相対位置を簡単かつ高精度に検出する。
【解決手段】ロボット２のアーム７先端のツール取付部７ｅに球面部８ａを有するツール８を取り付け、ツール取付部７ｅから球面部８ａの中心までのツールベクトルの成分に未知数を設定し、球面部８ａを平板１６に対してツール８の姿勢を変えて少なくとも未知数の数と同じ回数当接させ、その当接時のロボット２のアーム７の関節角度に基づいて当接時のツール取付部７ｅの位置をそれぞれ求め、特定の座標系において、平板１６のＺ位置と球面部８ａの曲率半径Rcとの和から得られる球面部８ａの中心のＺ位置が、ツール取付部７ｅのＺ位置とツールベクトルのＺ成分との和から得られる球面部８ａの中心のＺ位置に等しいことを意味する連立方程式を少なくとも未知数の数と同じ回数連立させ、その連立方程式を解くことでツールベクトルの成分を求める。
【選択図】図１

Description

本発明は、ロボットのツール位置検出方法、ロボットと対象物との相対位置検出方法及びそれらの装置に関するものである。

従来、複数の関節を有するアームの先端にツール（例えば、溶接トーチやエンドミル等）を取り付け、生産現場等で使用される産業用ロボットが知られている。この産業用ロボットでは、作業者が予めティーチングペンダントを用いて作業動作をロボットに教示しておき、本作業においてロボットが当該作業動作を再生することで、作業の自動化が図られるようになっている（例えば、特許文献１等参照）。

ロボットがツールでワークに対して作業をする際には、ツールの先端位置及び姿勢が正確に把握されている必要があるが、ツールに寸法誤差がある場合やツールが変形している場合などが考えられるため、予め作業者がツール先端位置及び姿勢を教示するようにしている。具体的には、先端が尖ったツールの場合には、針状の治具の先端に対してツールの先端を姿勢を変えて正確に点接触させ、それらの接触時の教示点からツール先端位置及び姿勢を求めるようにしている。また、その他には、三次元計測器を用いてツールの寸法等を正確に計測するという方法もある。

さらに、ロボットがツールでワークに対して作業をする際には、ロボットとワークとの相対位置関係も正確に把握されている必要がある。そこで、予め作業者がロボットに取り付けられたツールを、ワーク上における複数の特徴点（マーキング部、尖部、角部など）に正確に接触させて、それらの接触時の教示点からロボットとワークとの相対位置関係を求めるようにしている。また、その他には、ロボットに視覚センサを取り付け、その視覚センサによりワーク上の特徴点を計測し、ロボットとワークとの相対位置関係を求めるという方法もある。
特公平６−８７３０号公報

しかしながら、作業者がツール先端位置及び姿勢を教示する際には、ツールの先端を治具の先端に正確に位置決めする必要があり、非常に時間が掛かる。また、非熟練作業者がその作業を行うと、ツールを治具に押し付けすぎてツールが撓んだり、ツールと治具との間に隙間が生じたりして、位置決めが正確に行われない場合があり、教示精度にバラツキが生じる。さらに、三次元計測器を用いる場合には、ツールを一度外して計測作業を行わねばならず面倒であると共に、その装置が大型であると作業現場に持っていくことが困難となる等の問題もある。

また、作業者がワーク上の特徴点を教示する際も同様に、非常に時間が掛かると共に、非熟練作業者が行うと教示精度にバラツキが生じる。さらに、視覚センサを用いる場合には、計測精度に光などの影響を受けやすく、作業環境の悪い場所では使用できないと共に、センサが高価であるという問題もある。

そこで本発明は、ツール位置やロボットと対象物との相対位置を簡単かつ高精度に検出できるようにすることを目的としている。

本発明は前記事情に鑑みてなされたものであり、本発明に係るロボットのツール位置検出方法は、ロボットのアーム先端のツール取付部に、球面部又は尖部からなる先端部を有するツールを取り付け、前記ツール取付部から前記先端部の中心までのツールベクトルの成分に未知数を設定し、前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、特定の座標系において、前記平面のＺ位置と前記先端部の曲率半径との和から得られる前記先端部の中心のＺ位置が、前記ツール取付部のＺ位置と前記ツールベクトルのＺ成分との和から得られる前記先端部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、前記連立方程式を解くことで前記ツールベクトルの成分を求めることを特徴とする。

これによれば、一方向であるＺ位置のみに着目するだけでも、ツールを姿勢を変えて平面に当接させて連立方程式を解くことでツールベクトルの成分を求めることができる。そして、Ｚ位置のみに着目することで、ツールの先端部を平面上のどの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。なお、ツールの先端部が尖部である場合には、前記曲率半径はゼロであるとして扱うとよい。

略法線方向に変位可能な平板と、前記平板の変位を検出可能な変位センサとを用い、前記平面は、前記平板の当接面からなり、前記平面のＺ位置を前記変位センサの出力に基づいて求めてもよい。

これによれば、ツールの先端部を平板の当接面に当接させた際、平板が変位可能であるため、その当接圧が吸収される。よって、ツールが撓まずに正確な検出が行えるとともに、治具（本発明では平板等）の変形等を防止することができる。また、前記当接は、球面部又は尖部からなる先端部と平面との間で行われるので、平板及び変位センサを１自由度の安価なものとすることができる。

光学エリアセンサを用い、前記平面は、前記光学エリアセンサの感知面からなり、前記ツールの前記先端部が前記感知面に接触した時点で、前記ロボットのアームの関節角度を検出してもよい。

これによれば、光学的に形成された平面（感知面）にツールの先端部が当接したことを検知すればよいため、ツールの先端部を機械的に平面に接触させずに済み、ツールやセンサに負荷が掛かるのを防止することができる。

前記平面に前記先端部が当接していないときにおける前記平面のＺ位置と、前記先端部の曲率半径とをさらに未知数とし、前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも全未知数の数と同じ回数当接させ、前記連立方程式を少なくとも全未知数の数と同じ回数連立させてもよい。

これによれば、ツールの姿勢を変えて平面に先端部を当接させる回数を増やして、連立方程式の数を増やすだけで、平面のＺ位置及び先端部の曲率半径を予め知る必要がなくなり、教示作業をより簡素化することができる。

前記ツール取付部に寸法形状が既知の基準ツールを取り付け、前記基準ツールを前記平面内で移動させて前記基準ツールを前記平面の複数箇所に当接させることで、前記平面上のＸ，Ｙ位置とＺ位置との相関関係を予め求め、前記基準ツールに代えて前記ツールを前記ツール取付部に取り付けて前記ツールを前記平面に当接させ、その当接箇所の平面内におけるＸ，Ｙ位置を、前記検出されたツール取付部の位置および前記ツールの概略寸法形状から求め、前記相関関係により前記Ｘ，Ｙ位置に対応するＺ位置を求め、その求めたＺ位置を前記連立方程式における前記平面のＺ位置に代入してもよい。

これによれば、平面上のＸ，Ｙ位置とＺ位置との相関関係を予め求め、ツールの先端部が当接した平面上のＸ，Ｙ位置から前記相関関係を用いてＺ位置を補正するため、平面の法線方向が前記特定の座標系におけるＺ方向に一致していない場合であっても、正しくツールベクトルの成分を求めることができる。

また本発明のロボットと対象物との相対位置検出方法は、ロボットのアーム先端のツール取付部に平面を有する部材を取り付け、球面部又は尖部からなる当接部を有するターゲットを対象物に取り付け、前記当接部の位置及び方向を示すターゲットベクトルの成分に未知数を設定し、前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、特定の座標系において、前記ターゲットベクトルのＺ成分から得られる前記当接部の中心のＺ位置が、前記ツール取付部のＺ位置と前記平面のＺ位置と前記当接部の曲率半径との和から得られる前記当接部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、前記連立方程式を解くことで前記ターゲットベクトルの成分を求めることを特徴とする。

これによれば、一方向であるＺ位置のみに着目するだけでも、部材の姿勢を変えて平面をターゲットの当接部に当接させて連立方程式を解くことでターゲットベクトルの成分を求めることができる。そして、Ｚ位置のみに着目することで、ターゲットの当接部を平面上のどの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。なお、ターゲットの当接部が尖部である場合には、前記曲率半径はゼロであるとして扱うとよい。

略法線方向に変位可能な平板と、前記平板の変位を検出する変位センサとを前記ツール取付部に取り付け、前記平面は、前記平板の当接面からなり、前記平面のＺ位置を前記変位センサの出力に基づいて求めてもよい。

これによれば、平板をターゲットの当接部に当接させた際、平板が変位可能であるため、その当接圧が吸収される。よって、ターゲットが撓まずに正確な検出が行えるとともに、治具（本発明では平板等）の破壊を防止することができる。また、前記当接は、球面部又は尖部からなる先端部と平面との間で行われるので、平板及び変位センサを１自由度の安価なものとすることができる。

光学エリアセンサを前記ツール取付部に取り付け、前記平面は、前記光学エリアセンサの感知面からなり、前記感知面が前記ターゲットに接触した時点で、前記ロボットのアームの関節角度を検出してもよい。

これによれば、光学的に形成された平面（感知面）にターゲットの当接部が当接したことを検知すればよいため、ターゲットの当接部を機械的に平面に接触させずに済み、センサやターゲットに負荷が掛かるのを防止することができる。

前記平面が前記ターゲットに当接していないときにおける前記平面のＺ位置と、前記ターゲットの曲率半径とをさらに未知数とし、前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも全未知数の数と同じ回数当接させ、前記連立方程式を少なくとも全未知数の数と同じ回数連立させてもよい。

これによれば、部材の姿勢を変えて平面にターゲットの当接部を当接させる回数を増やして、連立方程式の数を増やすだけで、平面のＺ位置及び先端部の曲率半径を予め知る必要がなくなり、教示作業をより簡素化することができる。

前記対象物は、ワークであってもよい。

これによれば、ロボットとワークとの相対位置関係を求めることができ、ロボットによるワークの取り扱いを正確に行うことができる。

また本発明のロボットのツール位置検出装置は、アーム先端のツール取付部に球面部又は尖部からなる先端部を有するツールが取り付けられるロボットと、前記ロボットを制御する制御手段と、を備え、前記制御手段は、前記ツール取付部から前記先端部の中心までのツールベクトルの成分に未知数を設定し、前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも前記未知数の数と同じ回数当接させるよう前記ロボットを駆動し、前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、特定の座標系において、前記平面のＺ位置と前記先端部の曲率半径との和から得られる前記先端部の中心のＺ位置が、前記ツール取付部のＺ位置と前記ツールベクトルのＺ成分との和から得られる前記先端部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、前記連立方程式を解くことで前記ツールベクトルの成分を求めることを特徴とする。

これによれば、前述したツール位置検出方法と同様に、一方向であるＺ位置のみに着目するだけでも、ツールを姿勢を変えて平面に当接させて連立方程式を解くことでツールベクトルの成分を求めることができる。そして、Ｚ位置のみに着目することで、ツールの先端部を平面上のどの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。

また本発明のロボットと対象物との相対位置検出装置は、アーム先端のツール取付部に平面を有する部材が取り付けられるロボットと、前記ロボットを制御する制御手段と、を備え、前記制御手段は、対象物に取り付けられた球面部又は尖部からなる当接部を有するターゲットの前記当接部の位置及び方向を示すターゲットベクトルの成分に未知数を設定し、前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、特定の座標系において、前記ターゲットベクトルのＺ成分から得られる前記当接部の中心のＺ位置が、前記ツール取付部のＺ位置と前記平面のＺ位置と前記当接部の曲率半径との和から得られる前記当接部の中心のＺ位置に等しいことを意味する連立方程式を前記検出の回数分連立させ、前記連立方程式を解くことで前記ターゲットベクトルの成分を求めることを特徴とする。

これによれば、前述した相対位置検出方法と同様に、一方向であるＺ位置のみに着目するだけでも、部材の姿勢を変えて平面をターゲットの当接部に当接させて連立方程式を解くことでターゲットベクトルの成分を求めることができる。そして、Ｚ位置のみに着目することで、ターゲットの当接部を平面上のどの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。

以上の説明から明らかなように、本発明によれば、ツール位置やロボットと対象物との相対位置を簡単かつ高精度に検出することができる。

以下、本発明に係る実施形態を図面を参照して説明する。

（第１実施形態）
図１は本発明の第１実施形態に係るロボットのツール位置検出装置を示す模式図である。図１に示すように、ロボットのツール位置検出装置１は、産業用のロボット２と、ロボット２を制御する制御手段３とを備えている。制御手段３は、ロボット２に接続されたコントローラ４と、コントローラ４に接続されたコンピュータであるパソコン５とを有している。ロボット２は、地面等の設置面Ｇに設置される基台６と、基台６から突出するアーム７とを有し、基台６が設置された設置面Ｇを基準にした座標系をベース座標系と設定している。アーム７にはコントローラ４からの指令により角度変更するように駆動される複数の関節７ａ〜７ｄが設けられていると共に、その先端にはフランジ状のツール取付部７ｅが設けられている。

ロボット２には、関節７ａ〜７ｄの角度を検出可能なエンコーダ（図示せず）などが組み込まれており、関節７ａ〜７ｄの角度と、アーム７を構成するリンクの寸法とにより、ツール取付部７ｅのベース座標系における位置及び姿勢が分かるようになっている。ツール取付部７ｅには、球面部又は尖部からなる先端部を有するツール８が取り付け固定されている。具体的には、ツール８には、先端に球面部を有する溶接トーチや、先端に尖部を有するエンドミルなどが使用される。本実施形態では、球面部８ａからなる先端部を有するツール８を用いて説明する。なお、ツール取付部７ｅを基準とした座標系をツール座標系として設定している。

ロボット２の設置面Ｇには、タッチセンサ１０が設置されている。タッチセンサ１０は、ハウジング１１と、ハウジング１１内に収容されたバネ受板１２と、バネ受板１２を上方に付勢するバネ１３と、バネ受板１２からハウジング１１を貫通して上方に突出したロッド１４と、ロッド１４の上下方向の変位量を検出可能な変位センサ１５とを備えている。タッチセンサ１０の具体例としては、例えば、キーエンス社製の接触式デジタルセンサ（型番：GT2-H12L）などを使用することができる。タッチセンサ１０のロッド１４の先端には、上面を当接面とする平板１６が固定されている。変位センサ１５の出力は、パソコン５に入力されるように構成されている。

なお、本実施形態では、タッチセンサ１０の変位検出方向および平板１６の当接面１６ａ（上面）の法線方向が、ベース座標系のＺ方向と一致するように設けられているが、必ずしも一致させる必要はない。例えば、タッチセンサの変位検出方向がベース座標系のＺ方向に一致していない場合には、それを一致させるための行列を予め計測しておき、該行列を用いてタッチセンサ１０の座標系をベース座標系に変換するようにパソコン５を設定しておくとよい。

次に、ツール位置検出装置１によるツール位置検出手順について説明する。図２は図１に示す装置１によるツール位置検出方法を説明する図面である。図１及び２に示すように、まず、コントローラ４によりロボット２を操作して、ツール８の球面部８ａをタッチセンサ１０の平板１６の当接面１６ａに当接させる。その当接時には、タッチセンサ１０のロッド１４がバネ１３の弾性力により上下に変位可能であることで平板１６が下方に移動するため、当接時にツール８に掛かる力が緩和される。さらに、当接は、球面部８ａと平板１６との間で行われるので、平板１６及びタッチセンサ１０を１自由度の安価なものとすることができる。

また、図３（ａ）に示すように、平板１６と球面部８ａとの当接力は、常に平板１６の法線方向であるバネ１３の伸縮方向にのみ作用するため、ツール８の変形が防止されて正確な計測が行える。つまり、図３（ｂ）の比較例のように、球同士（球面部８ａと球体Ｂ）の当接の場合等には、球面部８ａの中心に向かった力が発生し、当接力がバネ１３の伸縮方向に作用するとは限らないため、ツールが変形してしまう場合が考えられるが、本実施形態によればそれが防止される。

次いで、図２に戻り、ツール８の球面部８ａを平板１６の当接面１６ａに当接させた時点において、ツール取付部７ｄの位置及び姿勢をロボット２の関節７ａ〜７ｄの角度に基づいて計測すると共に、変位センサ１５の出力も計測する。そして、そのツール取付部７ｄの位置及び姿勢と変位センサ１５の出力とをパソコン５に記録する。次いで、ロボット２によりツール８の姿勢を変えて、ツール取付部７ｄの位置及び姿勢と変位センサ１５の出力との計測・記録を複数回（後述する連立方程式(3)における未知数の数以上の回数）繰り返す。その際、ツール８の球面部８ａが平板１６の当接面１６ａに当接しさえすれば、各回ごとに当接面１６ａ内における当接位置がずれていてもよい。また、球面部８ａを繰り返し平板１６に当接させる際の各回のツール８の姿勢は、同一軸線周りの回転にならないよう選ぶようにする（例えば、ツール８が、鉛直方向下向き、Ｘ軸周りに±４５度、Ｙ軸周りに±４５度の５姿勢など）。

次いで、繰り返し回数分得たツール取付部７ｄの位置及び姿勢と変位センサ１５の出力のデータを用いて後述の連立方程式(3)を解くことで、ツール８の球面部８ａの中心の位置、即ち、ツール取付部７ｄから球面部８ａの中心までのツールベクトルtXtcの成分を求める。図２から分かるように、各ベクトルをベース座標系におけるＺ方向の線分上に投影すると、ツール８の姿勢に関わらず、常に以下の数式１が成り立つ。

ここで、数式１における記号や変数は、以下の通り定義される。
“・”：ベクトルの内積
“＊”：行列とベクトルの積
Nbz ：ベース座標系のＺ方向に平行な単位ベクトル、即ち、(0,0,1)の転置ベクトル
bXbt：ツール取付部までのベクトル（計測データごとに既知）
Rbt ：ベース座標系とツール座標系との間の座標変換行列（計測データごとに既知）
tXtc：ツールベクトル（各成分は未知数）
L0：タッチセンサの自然長（出力０のときの長さ：未知または既知のどちらでも可）
δL：タッチセンサの出力（縮む方向を正、計測データごとに既知）
Rc：球面部の曲率半径（未知または既知のどちらでも可）
この数式１は、ベース座標系において、平板１６の当接面１６ａのＺ位置と球面部８ａの曲率半径Rcとの和から得られる球面部の中心のＺ位置が、ツール取付部７ｅのＺ位置とツールベクトルtXtcのＺ成分との和から得られる球面部８ａの中心のＺ位置に等しいことを意味している。

そして、内積は表現するベクトルを変えても同じ値であるので、後述する連立方程式(3)を後述する数式４にまとめ易くするため、数式１をツール座標系に変換した数式２に書き換える。

ここで、Rtb ：Rbtの逆行列（計測データごとに既知）である。

図４は図１に示す装置１においてツール８を平板１６に対して姿勢を変えて当接させることを説明する図面である。図４に示すように、ロボット２を動作させてツール８の姿勢を複数回変えて計測すると、数式２は計測回数Ｎと同じ数だけ連立させた以下の連立方程式(3)を得ることができる（Nは未知数の数）。

そして、この連立方程式(3)は一般的に以下のように整理できる。

Ｘ，Ａ，Ｂは、タッチセンサ１０の自然長L0（即ち、平板１６の初期Ｚ位置）と球面部８ａの曲率半径Rcとを未知数として設定する場合には、以下の数式５〜７で表される。

一方、タッチセンサ１０の自然長L0と球面部８ａの曲率半径Rcとを共に既知として設定する場合には、Ｘ，Ａ，Ｂは以下の数式８〜１０で表される。

そして、行列Ａが正則の場合（Ｎ＝未知数の数）は、未知数のベクトルＸは、以下の数式１１で表される。

一方、行列Ａが非正則の場合（Ｎ＞未知数の数）は、未知数のベクトルＸは、以下の数式１２で表される。

なお、Ａ^＋は、以下の数式１３に示すようにＡの擬似逆行列であり、これにより最小２乗法で求めるとの同じように精度の高い解を得ることができる。

そして、数式１１または数式１２により、ツールベクトルtXtcの各成分が求められる。

以上によれば、一方向であるＺ位置のみに着目するだけでも、ツール８を姿勢を変えて平板１６に当接させて連立方程式を解くことでツールベクトルtXtcの各成分を求めることができる。そして、Ｚ位置のみに着目することで、ツール８の球面部８ａを平板１６の当接面１６ａ上の何れの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。また、タッチセンサ１０の自然長L0（即ち、平板１６の初期Ｚ位置）と球面部８ａの曲率半径Rcとを未知数として設定すれば、当接回数を増やして連立方程式の数を増やすだけで、L0及びRcを予め知る必要がなくなり、教示作業をより簡素化することができる。

なお、本実施形態では、ツール８の先端部を球面部８ａとしたが、ツールの先端部が尖部であってもよい（例えば、ツールをエンドミルとした場合等）。その場合には、前記した曲率半径Rcをゼロであるとして扱えば、その他のツール位置検出手順は前記同様である。また、本実施形態では、連立方程式(3)をツール座標系で解いたが、ベース座標系で解いてもよい。

（第２実施形態）
図５（ａ）（ｂ）は本発明の第２実施形態のツールの平板への当接状態を説明する図面である。図５（ａ）（ｂ）に示すように、本実施形態は、平板１６の法線方向がベース座標系のＺ方向に対して傾いた状態でタッチセンサ１０のロッド１４に固定された場合を想定している。この場合、ツール８の先端の球面部８ａを高さが一定となるように動かした場合でも、平板１６は上下に変位し、変位センサ１５の出力値がLb分だけ変化してしまう。（センサ値として得られるδLの値が右側の（ｂ）図ではLbだけ増加して検出される。）そこで、この影響を補正するために、基礎式である数式２を以下の数式１４にように修正する。

そして、ツール８の球面部８ａの中心のＸ，Ｙ位置を入力とし、変位センサ１５の出力値の補正量Lbを出力とする補正テーブルまたは補正関数を、以下のように予め求めておく。

まず、ロボット２のツール取付部７ｅに寸法形状が既知の基準ツール（図示せず）を取り付ける。次いで、ロボット２により基準ツールを平板１６に当接させ、その当接時における基準ツールの先端部の位置（bXbc, bYbc, bZbc）と、変位センサ１５の出力値δLとを記録する（図５（ａ）参照）。なお、基準ツールの先端部の位置は、基準ツールの概略寸法形状が既知であるため、以下の数式１５のようにロボット２のツール取付部７ｅの位置及び姿勢から求めることができる。

なお、tXtc0はツール取付部７ｅから基準ツールの先端部の中心までの基準ツールベクトルであり、基準ツールの寸法形状及びツール取付部７ｅの姿勢から求められる。

そして、基準ツールの先端部のＺ位置を変えないようにＸＹ平面内（ベース座標系）で基準ツールを移動させ、基準ツールを平板１６の当接面１６ａの複数箇所に当接させ、その当接時における基準ツールの先端部の位置（bXbc, bYbc, bZbc）と、変位センサ１５の出力値δLとを記録する（図５（ｂ）参照）。実際には、基準ツールをＸＹ平面に平行に移動させずに、一度、基準ツールを平板から離してから平行移動させ、同じ高さまで戻すことでツールの変形を防ぐ。この操作を繰り返すことにより、数式４の未知数の数以上の回数の計測を行う。このようにして得られた計測結果を元に、平板１６の傾きを補正するための、平板１６上におけるＸ，Ｙ位置とＺ位置との間の相関関係を求める。

具体的には、ツール先端位置とそれに対応したセンサ値は、計測した数だけ得られるので、ツール先端部のＸ，Ｙ位置（bXbc, bYbc）を入力として、変位センサ１５の出力値の補正量Lbを出力する線形補間テーブルを求める。または、テーブルの代わりに、数式１６に示すような平面の方程式を求めてもよい。

なお、Ａ、Ｂ，Ｚ０は係数であり、この３つの値は、計測した回数だけ得られている基準ツールの先端位置Ｘ，Ｙとセンサ出力値δＬで求まるベクトル（Ｘ、Ｙ，Ｚ）＝（bXbc,bYbc,δL＋bZbc）を数式１６に代入して得られる連立方程式を解くことで求めることができる。

次いで、ロボット２のツール取付部７ｅに取り付けられた基準ツールを計測対象のツール８に交換し、第１実施形態と同様に、ツール８の球面部８ａを平板１６に対して少なくとも未知数の数と同じ回数当接させ、第１実施形態の数式２を数式８に置き換えた（即ち、数式２中のδLをδL−Lbに置き換えた）うえで数式４を求める。その際のLbの値については、前記線形補間テーブルまたは数式１６を用いて、当接時のＸ，Ｙ位置に対応するLbを求める。そして、この連立方程式を解くことで、ツールベクトルの仮の値を算出する。それ以降は、この得られたツールベクトルの値を用いて同様の計測を繰り返せば、さらにツールベクトルの算出精度を向上させることができる。なお、他の構成・手順等は前述した第１実施形態と同様であるため詳細な説明を省略する。

（第３実施形態）
図６は本発明の第３実施形態の光学エリアセンサを示す斜視図である。図６に示すように、本実施形態では、平板１６を取り付けたタッチセンサ１０の代わりに、非接触の光学エリアセンサ２０を用いている。詳しくは、光学エリアセンサ２０は、複数の光線を平行に照射する複数の発光部を有する発光装置２１と、それら光線を受光する複数の受光部を有する受光装置２２とを備えた光電スイッチであり、複数の光線を含む仮想平面が感知面２３となっている。そして、光線を受光しない受光部が発生することで、ツール８の球面部８ａが感知面２３に接触したことを検知できるようになっている。

この光学エリアセンサ２０は、その感知面２３がベース座標系のＸＹ平面と平行になるように任意の高さに設置する。次いで、ロボット２のツール取付部７ｅに計測対象のツール８を取り付けて、光学エリアセンサ２０の上方からゆっくりとアプローチさせる。そして、ツール８の球面部８ａが光線を遮ったことを受光装置２２で検知した時点で、ロボット２の関節角度を検出することによりツール取付部７ｅの位置及び姿勢を記録する。この計測は、姿勢を変えて少なくとも３回（又はL0とRcが未知数の場合には少なくとも４回）以上行う。そして、そのツール取付部７ｅの位置及び姿勢情報を数式２に代入して連立方程式(4)を算出する。この際、センサ出力値δLの値はゼロとして計算する。それ以降は、第１実施形態に記載手順に従って連立方程式を解くことでツールベクトルが求まる。なお、他の構成・手順等は前述した第１実施形態と同様であるため詳細な説明を省略する。

（第４実施形態）
図７は本発明の第４実施形態のロボット２とワークＷとの相対位置検出装置３０を示す模式図である。なお、第１実施形態と共通する構成については同一符号を付している。図７に示すように、相対位置検出装置３０は、産業用のロボット２と、ロボット２を制御する制御手段１０３とを備えている。制御手段１０３は、ロボット２に接続されたコントローラ４と、コントローラ４に接続されたコンピュータであるパソコン１０５とを有している。ロボット２のツール取付部７ｅには、タッチセンサ１０が取り付け固定されている。タッチセンサ１０のロッド１４の先端には、平板１６が固定されている。タッチセンサ１０の変位センサ１５の出力は、パソコン１０５に入力されるように構成されている。

なお、本実施形態では、タッチセンサ１０の変位検出方向および平板１６の当接面１６ａの法線方向が、ツール座標系のＺ方向と一致するように設けられているが、必ずしも一致させる必要はない。例えば、タッチセンサ１０の変位検出方向がツール座標系のＺ方向に一致していない場合には、それを一致させるための行列を予め計測しておき、該行列を用いてタッチセンサ１０の座標系をツール座標系に変換するようにパソコン１０５を設定しておくとよい。

また、ロボット２の作業範囲内にはワークＷが設けられている。ワークＷには、球面部又は尖部からなる当接部を有するターゲットＴが３個以上取り付けられている。具体的には、ターゲットＴは、球面部Ｔａと、その球面部Ｔａから下方に突出する軸部Ｔｂとを有する金属部材からなり、ワークＷに形成された３個以上の基準穴Ｈに軸部Ｔｂをそれぞれ差し込むことで、各ターゲットＴを位置決めしている。そして、ワークＷを基準としたワーク座標系に対する球面部Ｔａの位置を三次元計測器などで予め計測しておく。

次に、相対位置検出装置３０によるロボット２とワークＷとの相対位置の検出手順について説明する。図８は図７に示す装置３０による相対位置検出方法を説明する図面である。図８に示すように、まず、コントローラ４によりロボット２を操作して平板１６をターゲットＴの球面部Ｔａに当接させる。その当接時には、タッチセンサ１０のロッド１４がバネ１３の弾性力により変位可能であることで平板１６が法線方向に移動するため、当接による負荷が緩和される。さらに、当接は、球面部Ｔａと平板１６との間で行われるので、平板１６及びタッチセンサ１０を１自由度の安価なものとすることができる。

平板１６の当接面１６ａにターゲットＴの球面部Ｔａに当接させた時点において、ツール取付部７ｄの位置及び姿勢をロボット２の関節７ａ〜７ｄの角度に基づいて計測すると共に、変位センサ１５の出力も計測する。そして、そのツール取付部７ｄの位置及び姿勢と変位センサ１５の出力とをパソコン１０５に記録する。次いで、ロボット２によりツール８の姿勢を変えて、ツール取付部７ｄの位置及び姿勢と変位センサ１５の出力との計測・記録を複数回（後述する連立方程式(18)における未知数の数以上の回数）繰り返す。その際、平板１６の当接面１６ａがターゲットＴの球面部Ｔａに当接しさえすれば、各回ごとに当接面１６ａ内における当接位置がずれていてもよい。また、平板１６を繰り返し球面部Ｔａに当接させる際の各回の平板１６の姿勢は、同一軸線周りの回転にならないよう選ぶようにする。

次いで、繰り返し回数分得たツール取付部７ｄの位置及び姿勢と変位センサ１５の出力のデータを用いて後述の連立方程式(18)を解くことで、ターゲットＴの球面部Ｔａの中心の位置、即ち、ベース座標系における球面部Ｔａの中心の位置及び方向を示すターゲットベクトルbXbcの成分を求める。図８から分かるように、各ベクトルをツール座標系におけるＺ方向の線分上に投影すると、平板１６の姿勢に関わらず、常に以下の数式１７が成り立つ。

ここで、数式１７における記号や変数は、以下の通り定義される。
“・”：ベクトルの内積
Ntz ：ツール座標系のＺ方向に平行な単位ベクトル
bXbc：ターゲットベクトル（各成分は未知数）
bXbt：ツール取付部までのベクトル（計測データごとに既知）
L0：タッチセンサの自然長（出力０での長さ：未知または既知のどちらでも可）
δL：タッチセンサの出力（縮む方向を正、計測データごとに既知）
Rc：球面部の曲率半径（未知または既知のどちらでも可）
この数式１７は、ツール座標系において、ターゲットベクトルbXbtのＺ成分から得られる球面部Ｔａの中心のＺ位置が、ツール取付部７ａのＺ位置と平板１６の当接面１６ａのＺ位置と球面部Ｔａの曲率半径Rcとの和から得られる球面部Ｔａの中心のＺ位置に等しいことを意味している。

図９は図７に示す装置３０において平板１６をターゲットＴに対して姿勢を変えて当接させることを説明する図面である。図９に示すように、ロボット２を動作させて平板１６の姿勢を複数回変えて計測すると、数式１７は計測回数Ｎと同じ数だけ連立させた以下の連立方程式(18)を得ることができる（Nは未知数の数）。

そして、この連立方程式(18)は一般的に以下のように整理できる。

Ｘ，Ａ，Ｂは、タッチセンサ１０の自然長L0（即ち、平板１６の初期Ｚ位置）と球面部Ｔａの曲率半径Rcとを未知数として設定する場合には、以下の数式２０〜２２で表される。

一方、タッチセンサ１０の自然長L0と球面部８ａの曲率半径Rcとを共に既知として設定する場合には、Ｘ，Ａ，Ｂは以下の数式２３〜２５で表される。

そして、数式１９を解くことでターゲットベクトルbXbcの各成分が求められる。

以上によれば、一方向であるＺ位置のみに着目するだけでも、タッチセンサ１０の姿勢を変えて平板１６をターゲットＴの球面部Ｔａに当接させて連立方程式を解くことでターゲットベクトルの成分を求めることができる。そして、Ｚ位置のみに着目することで、ターゲットＴの球面部Ｔａを平板１６上のどの位置に当接させてもよく（前回の当接位置に対してＸ，Ｙ位置がずれてもよい）、教示作業をラフに行うことが可能となる。よって、教示作業が簡単となり作業時間の短縮化が図れるとともに、検出精度を向上させることができる。また、タッチセンサ１０の自然長L0（即ち、平板１６の初期Ｚ位置）と球面部Ｔａの曲率半径Rcとを未知数として設定すれば、当接回数を増やして連立方程式の数を増やすだけで、L0及びRcを予め知る必要がなくなり、教示作業をより簡素化することができる。

なお、本実施形態では、ターゲットＴの当接部を球面部Ｔａとしたが、尖部としてもよい。その場合には、前記した曲率半径Rcをゼロであるとして扱えば、その他の手順は前記同様である。また、本実施形態ではタッチセンサ１０及び平板１６を用いたが、その代わりに第３実施形態のような光学エリアセンサ２０をツール取付部７ｅに取り付けて用いてもよい。

（第５実施形態）
図１０（ａ）〜（ｄ）は本発明の第５実施形態の相対位置検出方法を説明する図面である。図１０（ａ）〜（ｄ）に示すように、本実施形態では、ワークが設置されるテーブル４１を回転可能な駆動軸４２で支持したポジショナ４０を計測対象物としている。そして、テーブル４１にターゲットＴを取り付け、テーブル４１を回転させてターゲットＴを少なくとも３つの位置に移動させ、その３つの位置におけるターゲットベクトルａ１，ａ２，ａ３の成分を求める。このようにすれば、３つの位置におけるターゲットベクトルａ１，ａ２，ａ３の成分からポジショナ４０の運動方向（駆動軸４２の回転中心及び軸心ベクトルなど）を求めることができる。なお、他の構成は前述した第４実施形態と同様であるため説明を省略する。

（第６実施形態）
図１１（ａ）〜（ｃ）は本発明の第６実施形態の相対位置検出方法を説明する図面である。図１１（ａ）〜（ｃ）に示すように、本実施形態では、ロボット２は、床面に設置された走行レール５０上を走行可能に設けられている。そして、計測対象物は、走行レール５０が設置された床面としている。ロボット２を走行レール５０上で少なくとも２つの位置に移動させ、その２つの位置におけるターゲットベクトルｂ１，ｂ２の成分を求める。このようにすれば、２つの位置におけるターゲットベクトルｂ１，ｂ２の成分からロボット２の走行軸の位置及びベクトルを求めることができる。なお、他の構成は前述した第４実施形態と同様であるため説明を省略する。

以上のように、本発明に係るロボットのツール位置検出方法、ロボットと対象物との相対位置検出方法、及びそれらの装置は、ツール位置やロボットと対象物との相対位置を簡単かつ高精度に検出可能である優れた効果を有しており、生産現場で使用される産業用ロボット等の分野に広く適用すると有益である。

本発明の第１実施形態に係るロボットのツール位置検出装置を示す模式図である。図１に示す装置によるツール位置検出方法を説明する図面である。（ａ）は本発明におけるツールの平板への当接状態を示し、（ｂ）は比較例におけるツールのタッチセンサへの当接状態を示す図面である。図１に示す装置においてツールを平板に対して姿勢を変えて当接させることを説明する図面である。（ａ）（ｂ）は本発明の第２実施形態のツールの平板への当接状態を説明する図面である。本発明の第３実施形態の光学エリアセンサを示す斜視図である。本発明の第４実施形態のロボットとワークとの相対位置検出装置を示す模式図である。図７に示す装置による相対位置検出方法を説明する図面である。図７に示す装置において平板をターゲットに対して姿勢を変えて当接させることを説明する図面である。（ａ）〜（ｄ）は本発明の第５実施形態の相対位置検出方法を説明する図面である。（ａ）〜（ｃ）は本発明の第６実施形態の相対位置検出方法を説明する図面である。

符号の説明

１ツール位置検出装置
２ロボット
３，１０３制御手段
７アーム
７ａ〜７ｄ関節
７ｅツール取付部
８ツール
８ａ球面部（先端部）
１０タッチセンサ
１３バネ
１６平板
１６ａ当接面
２０光学エリアセンサ
２３感知面
３０相対位置検出装置
Ｔターゲット
Ｔａ球面部（当接部）
Ｗワーク（対象物）

Claims

ロボットのアーム先端のツール取付部に、球面部又は尖部からなる先端部を有するツールを取り付け、
前記ツール取付部から前記先端部の中心までのツールベクトルの成分に未知数を設定し、
前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、
前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、
特定の座標系において、前記平面のＺ位置と前記先端部の曲率半径との和から得られる前記先端部の中心のＺ位置が、前記ツール取付部のＺ位置と前記ツールベクトルのＺ成分との和から得られる前記先端部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、
前記連立方程式を解くことで前記ツールベクトルの成分を求めることを特徴とするロボットのツール位置検出方法。
略法線方向に変位可能な平板と、前記平板の変位を検出可能な変位センサとを用い、
前記平面は、前記平板の当接面からなり、
前記平面のＺ位置を前記変位センサの出力に基づいて求めることを特徴とする請求項１に記載のロボットのツール位置検出方法。
光学エリアセンサを用い、
前記平面は、前記光学エリアセンサの感知面からなり、
前記ツールの前記先端部が前記感知面に接触した時点で、前記ロボットのアームの関節角度を検出することを特徴とする請求項１に記載のロボットのツール位置検出方法。
前記平面に前記先端部が当接していないときにおける前記平面のＺ位置と、前記先端部の曲率半径とをさらに未知数とし、
前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも全未知数の数と同じ回数当接させ、前記連立方程式を少なくとも全未知数の数と同じ回数連立させることを特徴とする請求項１乃至３のいずれかに記載のロボットのツール位置検出方法。
前記ツール取付部に寸法形状が既知の基準ツールを取り付け、前記基準ツールを前記平面内で移動させて前記基準ツールを前記平面の複数箇所に当接させることで、前記平面上のＸ，Ｙ位置とＺ位置との相関関係を予め求め、
前記基準ツールに代えて前記ツールを前記ツール取付部に取り付けて前記ツールを前記平面に当接させ、その当接箇所の平面内におけるＸ，Ｙ位置を、前記検出されたツール取付部の位置および前記ツールの概略寸法形状から求め、
前記相関関係により前記Ｘ，Ｙ位置に対応するＺ位置を求め、その求めたＺ位置を前記連立方程式における前記平面のＺ位置に代入することを特徴とする請求項１乃至４のいずれかに記載のロボットのツール位置検出方法。
ロボットのアーム先端のツール取付部に平面を有する部材を取り付け、
球面部又は尖部からなる当接部を有するターゲットを対象物に取り付け、
前記当接部の位置及び方向を示すターゲットベクトルの成分に未知数を設定し、
前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、
前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、
特定の座標系において、前記ターゲットベクトルのＺ成分から得られる前記当接部の中心のＺ位置が、前記ツール取付部のＺ位置と前記平面のＺ位置と前記当接部の曲率半径との和から得られる前記当接部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、
前記連立方程式を解くことで前記ターゲットベクトルの成分を求めることを特徴とするロボットと対象物との相対位置検出方法。
略法線方向に変位可能な平板と、前記平板の変位を検出する変位センサとを前記ツール取付部に取り付け、
前記平面は、前記平板の当接面からなり、
前記平面のＺ位置を前記変位センサの出力に基づいて求めることを特徴とする請求項６に記載のロボットと対象物との相対位置検出方法。
光学エリアセンサを前記ツール取付部に取り付け、
前記平面は、前記光学エリアセンサの感知面からなり、
前記感知面が前記ターゲットに接触した時点で、前記ロボットのアームの関節角度を検出することを特徴とする請求項６に記載のロボットと対象物との相対位置検出方法。
前記平面が前記ターゲットに当接していないときにおける前記平面のＺ位置と、前記ターゲットの曲率半径とをさらに未知数とし、
前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも全未知数の数と同じ回数当接させ、前記連立方程式を少なくとも全未知数の数と同じ回数連立させることを特徴とする請求項６乃至８のいずれかに記載のロボットと対象物との相対位置検出方法。
前記対象物は、ワークであることを特徴とする請求項６乃至９に記載のロボットと対象物との相対位置検出方法。
アーム先端のツール取付部に球面部又は尖部からなる先端部を有するツールが取り付けられるロボットと、
前記ロボットを制御する制御手段と、を備え、
前記制御手段は、
前記ツール取付部から前記先端部の中心までのツールベクトルの成分に未知数を設定し、
前記先端部を平面に対して前記ツールの姿勢を変えて少なくとも前記未知数の数と同じ回数当接させるよう前記ロボットを駆動し、
前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、
特定の座標系において、前記平面のＺ位置と前記先端部の曲率半径との和から得られる前記先端部の中心のＺ位置が、前記ツール取付部のＺ位置と前記ツールベクトルのＺ成分との和から得られる前記先端部の中心のＺ位置に等しいことを意味する連立方程式を少なくとも前記未知数の数と同じ回数連立させ、
前記連立方程式を解くことで前記ツールベクトルの成分を求めることを特徴とするロボットのツール位置検出装置。
アーム先端のツール取付部に平面を有する部材が取り付けられるロボットと、
前記ロボットを制御する制御手段と、を備え、
前記制御手段は、
対象物に取り付けられた球面部又は尖部からなる当接部を有するターゲットの前記当接部の位置及び方向を示すターゲットベクトルの成分に未知数を設定し、
前記当接部に対して前記平面を前記部材の姿勢を変えて少なくとも前記未知数の数と同じ回数当接させ、
前記当接時の前記ロボットのアームの関節角度に基づいて前記当接時の前記ツール取付部の位置をそれぞれ求め、
特定の座標系において、前記ターゲットベクトルのＺ成分から得られる前記当接部の中心のＺ位置が、前記ツール取付部のＺ位置と前記平面のＺ位置と前記当接部の曲率半径との和から得られる前記当接部の中心のＺ位置に等しいことを意味する連立方程式を前記検出の回数分連立させ、
前記連立方程式を解くことで前記ターゲットベクトルの成分を求めることを特徴とするロボットと対象物との相対位置検出装置。