(57)【特許請求の範囲】
【請求項１】
手先に視覚センサを搭載したマニピュレータを、前記視覚センサの位置姿勢が誘導前の位置姿勢から対象物の位置を示す計測点が前記視覚センサにより得られる視野の中央となる誘導後の位置姿勢に、誘導するマニピュレータ装置であって、
前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面にいずれも平行な２つの方向をそれぞれＸｒ軸、Ｙｒ軸とする直交３軸からなる前記マニピュレータの座標系とし、
前記視覚センサの上方向をＹｓ軸、前記視覚センサの視覚方向をＺｓ軸、前記Ｙｓ軸及び前記Ｚｓ軸と直交する方向をＸｓ、とする直交３軸からなる前記視覚センサの座標系とし、
前記の誘導前の位置姿勢において視覚センサにより前記対象物を含む画像を取得し、
前記画像に基づいて前記視覚センサの座標系における前記計測点を定め、
前記視覚センサの座標系の原点と前記計測点との距離として予め設定された値Ｌを用いて、前記計測点を中心とした半径Ｌの球面を表す式１を設定し、
前記計測点を含み前記マニピュレータの座標系におけるＸｒＹｒ平面と平行な平面を表す式２を設定し、
前記視覚センサの座標系における前記計測点の位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面を表す式３を設定し、
前記の式１〜３をいずれも満たす位置を誘導後の前記視覚センサの位置である誘導位置として求め、
前記誘導位置と前記計測点とを通るベクトルを正規化して、誘導後の前記視覚センサの座標系におけるＺｓ軸を表す単位ベクトルＺｓ’を求め、
誘導後の前記視覚センサの座標系におけるＸｓＺｓ平面と前記マニピュレータの座標系のＸｒＹｒ平面とが平行となるとしたときの誘導後の前記視覚センサの座標系におけるＹｓ軸を表す単位ベクトルＹｓ’を求め、
前記単位ベクトルＺｓ’と前記単位ベクトルＹｓ’とから誘導後の前記視覚センサの座標系におけるＸｓ軸を表す単位ベクトルＸｓ’を求め、
前記誘導位置、前記単位ベクトルＸｓ’、単位ベクトルＹｓ’及び前記単位ベクトルＺｓ’に基づいて前記視覚センサを誘導する
ことを特徴とするマニピュレータ装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、マニピュレータ手先に搭載された視覚センサを対象物に誘導するマニピュレータ装置に関する。
【０００２】
【従来の技術】
ハンドアイシステムと呼ばれる、視覚センサを搭載したマニピュレータを用いて視覚センサを対象物に誘導し、対象物を検出する方法がある。この際、対象物が視覚センサの視野に入るよう、あるいは、対象物が視覚センサ視野の中央に位置するよう、視覚センサを誘導する必要がある。
まず、第１の従来の技術を、特開平５−８０８４２号の「移動ロボットの制御方法」を例に説明する。ワーク近くに２次元状のマークを配置し、マニピュレータには画像入力手段が取り付けられている。画像入力手段は、視覚情報を取得する機能を持つため、視覚センサに相当する。図３に第１の従来の技術に係る視覚センサの誘導方法ブロック図を示す。ティーチング時には、画像入力手段の視野内にマークが入るよう、マニピュレータを操作し、画像入力手段によって、予め、マーク画像（ティーチングマーク画像３０１）を取得する。プレイバック時にも、画像入力手段によってマーク画像３０２を取得する。
第１の従来の技術の誘導位置・誘導姿勢算出手段３０５は、比較照合手段３０３と座標変換手段３０４とから構成される。比較照合手段３０３では、ティーチングマーク画像３０１とマーク画像３０２とを入力し、これらを比較照合する。これは、ティーチングマーク画像３０１とマーク画像３０２とのマーク位置にずれがあるためである。比較照合手段３０３の比較照合の結果、ティーチングマーク画像３０１とマーク画像３０２とのマーク位置の差が求められる。座標変換手段３０４はこのマーク位置の差を入力し、これをマニピュレータ座標系に変換し、ティーチングマーク画像３０１が取得できる位置・姿勢に画像入力手段を誘導するような、誘導位置１０４と誘導姿勢１０５とを出力する。
【０００３】
次に、第２の従来の技術を、「脇迫ら：“遠隔操作型配電作業ロボットの距離センサシステム”，日本ロボット学会誌，１５巻，３号，４６８〜４７３ページ，１９９７」を例に説明する。距離センサがパン・チルト軸を持つ架台に置かれており、操作者が、パン・チルト軸を遠隔操作することで対象物までの距離を計測する。距離センサは、視覚情報を取得する機能を持つため、視覚センサに、また、架台は、距離センサの位置と姿勢とを変更する機能を持つため、マニピュレータに、それぞれ相当する。
図４に第２の従来の技術に係る視覚センサの誘導方法ブロック図を示す。なお、図４では、第２の従来の技術の誘導位置・誘導姿勢算出手段４０４の説明をするために、操作者による操作と、ティーチングプレイバックによる操作との２つの方式を同時に示している。操作者は、距離センサのカメラの映像を見ながら、対象物がカメラの中央に来るように、距離センサの架台を遠隔操作する。操作者による操作指令４０２に基づき、距離センサの誘導位置１０４と誘導姿勢１０５とが決定される。また、距離センサと対象物との位置関係がおおよそ既知である場合には、予めティーチングしたデータに基づきプレイバックする、ティーチングプレイバック方式で実行される。プレイバック時には、プレイバック指令４０１により、ティーチングデータ４０３が読み出され、誘導位置１０４と誘導姿勢１０５とが決定される。
【０００４】
以下では、第２の従来の技術を装柱に対する配電作業に適用した例を詳しく説明する。
従来の装柱に対する配電作業で行われる視覚センサ誘導の概略説明図を図５に示す。５０１は装柱で、電柱５０２、アーム５０３、第１の碍子５０４、第２の碍子５０５、第３の碍子５０６から構成される。通常、電柱５０２は地面に対しほぼ垂直に設置されており、アーム５０３は電柱５０２に対し垂直に取り付けられている。第１の碍子５０４、第２の得子５０５、第３の碍子５０６は、アーム５０３に固定されている。５０７はマニピュレータで、先端に搭載してある視覚センサ５１１の位置と姿勢とを変更する。マニピュレータ５０７は、コントローラ５０８を介し、操作者５１０がティーチボックス５０９を操作することで駆動される。５１２は誘導前の視覚センサ位置で、得られる映像が誘導前の映像５１４である。５１３は誘導後の視覚センサ位置で、得られる映像が誘導後の映像５１５である。誘導後の視覚センサ位置５１３の時の視覚センサ５１１と、それを配置するマニピュレータ５０７と、誘導後の映像５１５と、それへの映像信号線とを点線で表示している。
【０００５】
ここでは、視覚センサにより装柱５０１を遠方から捉え、視覚センサを対象物に近付けることを行う（対象物は第１の碍子５０４とする）。より具体的には、誘導前の映像５１４が得られる誘導前の視覚センサ位置５１２の時の視覚センサ５１１を、第１の碍子５０４の映像（誘導後の映像５１５）が得られる誘導後の視覚センサ位置５１３に誘導する。これは、注目する対象物（この場合第１の碍子５０４）の位置・姿勢情報をより詳細に検出する際に必要となる。この時、視覚センサ５１１が持つ光学歪みを考慮すると、第１の碍子５０４が、誘導後の映像５１５内で、ほぼ中央に位置するように、視覚センサ５１１を配置する方が望ましい。また、この場合、第１の碍子５０４の特徴を良好に捉えるためには、視覚センサ５１１を第１の碍子５０４のほぼ真横に配置する方が望ましい。
第２の従来の技術の操作者５１０による操作では、この誘導位置１０４と誘導姿勢１０５との決定を、操作者５１０が誘導前の映像５１４を見て、誘導後の映像５１５が得られるように、ティーチボックス５０９を操作し、マニピュレータ５０７を駆動して実行する。
また、第２の従来の技術のティーチングプレイバックによる操作では、コントローラ５０８に保存されたティーチングデータ４０３が、操作者５１０がティーチボックス５０９を操作することにより出されるプレイバック指令４０１により読み出され、マニピュレータ５０７がプレイバックを実行し、視覚センサ５１１の誘導位置１０４と誘導姿勢１０５とが決定される。
【０００６】
【発明が解決しようとする課題】
第１の従来の技術で述べた特開平５−８０８４２号の「移動ロボットの制御方法」では、予めワーク近くにマークを配置する必要があり、あるいは、ティーチングマーク画像３０１を予めティーチングする必要があり、そのための労力が発生し、手間が掛かるという問題があった。また、配置されるマークは２次元状であるが、平面上でない対象物にマークを配置する場合、視覚センサ５１１が見える場所にマークを配置する必要があるという問題があった。また、第１の碍子５０４を誘導後の映像５１５内ほぼ中央で、かつ、第１の碍子５０４のほぼ真横を捉えるように視覚センサ５１１を誘導するためには、視覚センサ５１１の位置と姿勢を３次元的に変更する必要があるが、マークが２次元状であるために、２次元的な位置と姿勢の変更しか行えないという問題があった。
【０００７】
第２の従来の技術で述べた操作者５１０による操作では、操作者５１０が誘導前の映像５１４や誘導後の映像５１５を見ながら、誘導前の視覚センサ位置５１２の視覚センサ５１１を誘導後の視覚センサ位置５１３に誘導するため、操作者（人）の介在が必要で自動化できないという問題があった。また、第１の碍子５０４を誘導後の映像５１５内ほぼ中央で、かつ、第１の碍子５０４のほぼ真横を捉えるように視覚センサ５１１を誘導する必要があるため、その誘導に微調整が必要で、マニピュレータ５０７の操作に熟練を要する、あるいは手間が掛かるという問題があった。第２の従来の技術で述べたティーチングプレイバックによる操作では、装柱５０１とマニピュレータ５０７との位置関係を予め知っておく必要があり、そのための労力が発生し、手間が掛かるという問題があった。また、ティーチングの際に、装柱５０１とマニピュレータ５０７との位置関係にずれがある場合には、プレイバック時に、誘導後の映像５１５から第１の碍子５０４が外れる、あるいは、誘導後の映像５１５の中央に第１の碍子５０４が位置しない、あるいは、視覚センサ５１１が第１の碍子５０４の真横に配置されない、という問題があった。本発明は、このような従来の技術における課題を解決するものであり、マニピュレータ手先に搭載された視覚センサを対象物に誘導する際、操作者（人）の介在、微調整を必要とせず自動的に視覚センサを誘導することができ、対象物を確実に視野内ほぼ中央に捉えることができることにより光学歪みが改善され良好な対象物の検出が実行でき、視覚センサの位置と姿勢を３次元的に変更することができ、余分な労力を必要とせず、手間の掛からない、簡便なマニピュレータ装置を提供することを目的とする。
【０００８】
【課題を解決するための手段】
上記問題を解決するため、本発明のマニピュレータ装置は、手先に視覚センサを搭載したマニピュレータを、前記視覚センサの位置姿勢が誘導前の位置姿勢から対象物の位置を示す計測点が前記視覚センサにより得られる視野の中央となる誘導後の位置姿勢に、誘導するマニピュレータ装置であって、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面にいずれも平行な２つの方向をそれぞれＸｒ軸、Ｙｒ軸とする直交３軸からなる前記マニピュレータの座標系とし、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視覚方向をＺｓ軸、前記Ｙｓ軸及び前記Ｚｓ軸と直交する方向をＸｓ、とする直交３軸からなる前記視覚センサの座標系とし、前記の誘導前の位置姿勢において視覚センサにより前記対象物を含む画像を取得し、前記画像に基づいて前記視覚センサの座標系における前記計測点を定め、前記視覚センサの座標系の原点と前記計測点との距離として予め設定された値Ｌを用いて、前記計測点を中心とした半径Ｌの球面を表す式１を設定し、前記計測点を含み前記マニピュレータの座標系におけるＸｒＹｒ平面と平行な平面を表す式２を設定し、前記視覚センサの座標系における前記計測点の位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面を表す式３を設定し、前記の式１〜３をいずれも満たす位置を誘導後の前記視覚センサの位置である誘導位置として求め、前記誘導位置と前記計測点とを通るベクトルを正規化して、誘導後の前記視覚センサの座標系におけるＺｓ軸を表す単位ベクトルＺｓ’を求め、誘導後の前記視覚センサの座標系におけるＸｓＺｓ平面と前記マニピュレータの座標系のＸｒＹｒ平面とが平行となるとしたときの誘導後の前記視覚センサの座標系におけるＹｓ軸を表す単位ベクトルＹｓ’を求め、前記単位ベクトルＺｓ’と前記単位ベクトルＹｓ’とから誘導後の前記視覚センサの座標系におけるＸｓ軸を表す単位ベクトルＸｓ’を求め、前記誘導位置、前記単位ベクトルＸｓ’、単位ベクトルＹｓ’及び前記単位ベクトルＺｓ’に基づいて前記視覚センサを誘導するものである。
【０００９】
【発明の実施の形態】
図５を用いて説明した従来の技術の場合と同様、本実施例でも、誘導前の映像５１４が得られる誘導前の視覚センサ位置５１２の視覚センサ５１１を、誘導後の映像５１５が得られる誘導後の視覚センサ位置５１３に誘導することを行う。
以下、図１及び図２を用いて本発明の実施例を説明する。
図２に本発明の実施例に係る視覚センサの誘導方法モデル図を示す。図２では、装柱５０１の構成要素である、第１の碍子５０４とアーム５０３の一部のみを示している。２０１及び２０３は視覚センサ５１１の内部に設置された座標系で、２０１が、誘導前の視覚センサ位置５１２の視覚センサの座標系（誘導前の視覚センサ座標系）、２０３が、誘導後の視覚センサ位置５１３の視覚センサの座標系（誘導後の視覚センサ座標系）である。図５に示した視覚センサ５１１の紙面手前方向をＸｓ軸、上方向をＹｓ軸、左方向（視覚センサの視線方向）をＺｓ軸とする。これによれば、視覚センサ５１１の底面はＸｓＺｓ平面と平行となる。誘導後の視覚センサ座標系２０３の各座標軸には、誘導前の視覚センサ座標系２０１のそれらと区別するために、「’」を付している。２０２はマニピュレータのベース部に設置された座標系（マニピュレータ座標系）で、図５で示したマニピュレータ５０７の紙面右方向をＸｒ軸、、奥方向をＹｒ軸、上方向をＺｒ軸とする。これによれば、マニピュレータ５０７の底面はＸｒＹｒ平面と平行となる。
【００１０】
本実施例は、この誘導後の視覚センサ座標系２０３の位置と姿勢とを決定するものである。より具体的には、マニピュレータ座標系２０２に関する誘導後の視覚センサ座標系２０３の位置と姿勢とを求めるものである。マニピュレータ５０７と視覚センサ５１１とは校正されているため、マニピュレータ５０７の移動に伴って、マニピュレータ座標系２０２に関する視覚センサ座標系（２０１や２０３）の位置と姿勢が分かる。マニピュレータ座標系２０２に関する誘導前の視覚センサ座標系２０１の位置と姿勢をＴ、誘導後の視覚センサ座標系２０３の位置と姿勢をＴ’とする。Ｔ及びＴ’は位置と姿勢を表す４行４列の同次変換行列である。従って、本実施例は、Ｔ’を求めることが目的となる。なお、２０４から２０７の説明については、図１の説明の中で記載する。
【００１１】
視覚センサ座標系に関する概略位置２０７は、誘導前の視覚センサ位置５１２で視覚センサ５１１を用いて第１の碍子５０４を見た時の、誘導前の視覚センサ座標系２０１に関する第１の碍子５０４の概略位置で、図２では、ベクトル表記している。この視覚センサ座標系に関する概略位置２０７をマニピュレータ座標系２０２に関する位置で表し、
【数１】
0004568978.tif 000002
とする。ここで、ｔは転置である。マニピュレータ座標系に関する概略位置２０８は、マニピュレータ５０７と視覚センサ５１１とが校正されているため（Ｔが分かるため）、視覚センサ座標系に関する概略位置２０７から容易に求めることができる。以降では、マニピュレータ座標系２０２に関する座標値を扱う。なお、視覚センサ座標系に関する概略位置２０７は、視覚センサ５１１として、レーザレンジファインダ等を用いて求めることができる。
【００１２】
図１に本発明の実施例に係る視覚センサの誘導方法ブロック図を示す。図２における２０４は、視覚センサ座標系に関する概略位置２０７を示す計測点を表している。対象物距離１０１は、誘導後の視覚センサ位置５１３の視覚センサ５１１と、第１の碍子５０４との距離であり、予め設定する。すなわち、図２においては、誘導後の視覚センサ座標系２０３の原点（２０５）と計測点２０４との距離であり、これをＬとする。
本実施例における誘導位置・誘導姿勢算出手段１０６は、誘導位置算出手段１０２と誘導姿勢算出手段１０３とから構成される。誘導位置算出手段１０２では、マニピュレータに関する概略位置２０８と対象物距離１０１とを入力し、誘導位置１０４、すなわち、誘導後の視覚センサ座標系２０３の位置を出力する。計測点２０４を中心とし、半径をＬとした球面２０６を設定する。球面２０６の方程式は
【数２】
0004568978.tif 000003
となる。計測点２０４を通り、ＸｒＹｒ平面に平行な平面は
【数３】
0004568978.tif 000004
となる。球面２０６と視覚センサ座標系に関する概略位置２０７のベクトルとの交点を
【数４】
0004568978.tif 000005
とすると、視覚センサ座標系に関する概略位置２０７のベクトルを含みＺｒに平行な平面は
【数５】
0004568978.tif 000006
となる。式（２）、（３）、（５）を連立し解くと、
【数６】
0004568978.tif 000007
となる。
【００１３】
２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、第１の碍子５０４と同じ高さで、距離Ｌの位置に誘導後の視覚センサ座標系２０３の位置を決定することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。誘導後の視覚センサ座標系２０３のＸｓ’Ｚｓ’平面とマニピュレータ座標系２０２のＸｒＹｒ平面とが平行であるとすると、Ｙｓ’軸の単位ベクトルは
【数７】
0004568978.tif 000008
となる。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化し
【数８】
0004568978.tif 000009
とすると、これがＺｓ’軸の単位ベクトルとなる。Ｘｓ’軸は式（７）と（８）との外積演算
【数９】
0004568978.tif 000010
により求めると、これがＸｓ’軸の単位ベクトルとなる。
【００１４】
式（７）、（８）、（９）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【数１０】
0004568978.tif 000011
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（６）、（１０）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ’は
【数１１】
0004568978.tif 000012
となる。
以上により、誘導後の視覚センサ座標系２０３を、第１の碍子５０４と同じ高さで、距離Ｌの位置に、視覚センサ５１１の視線が第１の碍子５０４に向かう姿勢で、決定することができる。
【００１５】
【発明の効果】
以上、説明したように、本発明は、対象物の概略位置と、予め設定した誘導位置と対象物との距離とから誘導位置を求めるようにしたので、対象物と同じ高さで、すなわち対象物の真横で、予め設定した誘導位置と対象物との距離を保った位置に視覚センサの位置を自動的にかつ３次元的に決定することができる。
また、本発明は、対象物の概略位置と、誘導位置とから誘導姿勢を求めるようにしたので、対象物に視覚センサの視線方向が向かうように、すなわち対象物が視覚センサで得られる視野の中央となるように、視覚センサの姿勢を自動的にかつ３次元的に決定することができる。
また、その結果、光学歪みが改善され、良好な対象物の検出が実行できる。
更に、本発明は、誘導位置及び誘導姿勢を自動的に決定するようにしたので、手間を掛けずに、また、余分な労力を必要とせずに、簡便に視覚センサを誘導することができる。
【図面の簡単な説明】
【図１】  本発明の実施例に係る視覚センサの誘導方法ブロック図である。
【図２】  本発明の実施例に係る視覚センサの誘導方法モデル図である。
【図３】  第１の従来の技術に係る視覚センサの誘導方法ブロック図である。
【図４】  第２の従来の技術に係る視覚センサの誘導方法ブロック図である。
【図５】  従来の装柱に対する配電作業で行われる視覚センサ誘導の概略説明図である。
【符号の説明】
１０１    対象物距離
１０２    誘導位置算出手段
１０３    誘導姿勢算出手段
１０４    誘導位置
１０５    誘導姿勢
１０６    誘導位置・誘導姿勢算出手段
２０１    誘導前の視覚センサ座標系
２０２    マニピュレータ座標系
２０３    誘導後の視覚センサ座標系
２０４    計測点
２０５    誘導後の視覚センサ座標系の原点
２０６    球面
２０７    視覚センサ座標系に関する概略位置
２０８    マニピュレータ座標系に関する概略位置
３０１    ティーチングマーク画像
３０２    マーク画像
３０３    比較照合手段
３０４    座標変換手段
３０５    第１の従来の技術の誘導位置・誘導姿勢算出手段
４０１    プレイバック指令
４０２    操作指令
４０３    ティーチングデータ
４０４    第２の従来の技術の誘導位置・誘導姿勢算出手段
５０１    装柱
５０２    電柱
５０３    アーム
５０４    第１の碍子
５０５    第２の碍子
５０６    第３の碍子
５０７    マニピュレータ
５０８    コントローラ
５０９    ティーチボックス
５１０    操作者
５１１    視覚センサ
５１２    誘導前の視覚センサ位置
５１３    誘導後の視覚センサ位置
５１４    誘導前の映像
５１５    誘導後の映像
【図１】
0004568978.tif 000013
【図２】
0004568978.tif 000014
【図３】
0004568978.tif 000015
【図４】
0004568978.tif 000016
【図５】
0004568978.tif 000017