(57)【特許請求の範囲】
【請求項１】
マニピュレータ手先に視覚センサを搭載し、前記マニピュレータを駆動することで対象物に前記視覚センサを誘導する方法において、
前記対象物に向かう方向をＸｒ軸、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面に平行な方向をＹｒ軸、とする直交３軸からなる前記マニピュレータの座標系と、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視線方向をＺｓ軸、前記視覚センサの底面に平行な方向をＸｓ軸、とする直交３軸からなる前記視覚センサの座標系と、を設定し、
前記視覚センサで前記対象物を計測して、その計測点を中心とした所定の半径の球面を表す式を設定し、
誘導後の前記視覚センサの位置の近傍に障害物があるとき、前記計測点から前記障害物を回避するよう上下方向にオフセットした高さにあって、前記マニピュレータの座標系の底面を表すＸｒＹｒ面に平行な面を表す式を設定し、
誘導前の前記視覚センサの座標系の原点から前記計測点までの概略位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面の式を設定し、
これら３つの式から得られる２つの解の内、前記視覚センサ側の解が表す位置を誘導後の位置とし、
前記計測点と誘導後の前記視覚センサの座標系の原点とを通るベクトルを正規化したものを、誘導後の前記視覚センサの座標系のＺｓ’軸の単位ベクトルとし、
前記視覚センサの座標系のＹｓ’軸が前記マニピュレータの座標系のＸｒＺｒ平面と平行であるとしたときの前記Ｙｓ’軸の単位ベクトルと、前記Ｚｓ’軸の単位ベクトルとの内積が０であることを利用し正規化したものを誘導後の前記視覚センサ座標系のＹｓ’軸の単位ベクトルとし、
前記Ｚｓ’軸の単位ベクトルと前記Ｙｓ’軸の単位ベクトルとの外積演算から誘導後の前記視覚センサ座標系のＸｓ’軸の単位ベクトルを求め、
これら３つの単位ベクトルを要素とした座標系を誘導後の姿勢とし、
前記誘導後の位置と前記誘導後の姿勢へ前記視覚センサを誘導することを特徴とする視覚センサの誘導方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、マニピュレータ手先に搭載された視覚センサを対象物に誘導する視覚センサの誘導方法に関する。
【０００２】
【従来の技術】
本願出願人は先に、特願２０００−２７８７７６において、対象物の概略位置と、誘導位置と前記対象物との距離を与えて、マニピュレータを誘導位置に誘導する方法を提案している。これは、次のような方法である。
【０００３】
図５は配電作業用マニピュレータを用いて視覚センサを誘導する方法の概略説明図である。
図において、５０１は装柱である。装柱５０１は、電柱５０２、アーム５０３、第１の碍子５０４、第２の碍子５０５、第３の碍子５０６から構成される。通常、電柱５０２は地面に対しほぼ垂直に設置されており、アーム５０３は電柱５０２に対し直角に取り付けられている。第１の碍子５０４、第２の得子５０５、第３の碍子５０６は、アーム５０３に固定されている。５０７はマニピュレータであり、その先端に視覚センサ５１１を取り付けている。５１２は誘導前の視覚センサ位置であり、そこで得られる映像が誘導前の映像５１４である。５１３は誘導後の視覚センサ位置であり、そこで得られる映像が誘導後の映像５１５である。
【０００４】
ここで、視覚センサ５１１により装柱５０１を遠方から捉え、視覚センサ５１１を第１の碍子５０４に誘導する方法を説明する。より具体的には、視覚センサ５１１を視覚センサ位置５１２から視覚センサ位置５１３に誘導する方法を説明する。
【０００５】
図３は従来技術による視覚センサの誘導方法を示すモデル図であり、図４は従来技術による視覚センサの誘導方法を示すブロック図である。図３は、図５に示した装柱５０１の部分拡大図であり、第１の碍子５０４とアーム５０３の一部のみを示している。２０１及び２０３は視覚センサ５１１の内部に設置された座標系で、２０１が、誘導前の視覚センサ位置５１２の視覚センサの座標系（誘導前の視覚センサ座標系）、２０３が、誘導後の視覚センサ位置５１３の視覚センサの座標系（誘導後の視覚センサ座標系）である。図５に示した視覚センサ５１１の紙面手前方向をＸｓ軸、上方向をＹｓ軸、左方向（視覚センサの視線方向）をＺｓ軸とする。これによれば、視覚センサ５１１の底面はＸｓＺｓ平面と平行となる。誘導後の視覚センサ座標系２０３の各座標軸には、誘導前の視覚センサ座標系２０１のそれらと区別するために、「'」を付している。２０２はマニピュレータのベース部に設置された座標系（マニピュレータ座標系）で、図５に示したマニピュレータ５０７の紙面右方向をＸｒ軸、奥方向をＹｒ軸、上方向をＺｒ軸とする。これによれば、マニピュレータ５０７の底面はＸｒＹｒ平面と平行となる。
【０００６】
２０７は、誘導前の視覚センサ位置５１２から第１の碍子５０４を見た時の、第１の碍子５０４の概略位置（つまり誘導前の視覚センサ座標系２０１の原点に対する第１の碍子５０４の概略位置）を表すベクトルである。２０８はマニピュレータ座標系２０２の原点に対する第１の碍子５０４の概略位置を表すベクトルであり、次の様に、表される。
【０００７】
【数１】
0003972356.tif 000002
【０００８】
ここで、ｔは転置である。マニピュレータ座標系に関する概略位置２０８は、マニピュレータ５０７と視覚センサ５１１とが校正されているため（Ｔが分かるため）、視覚センサ座標系に関する概略位置２０７から容易に求めることができる。  以後の説明では、マニピュレータ座標系２０２を基準に説明する。なお、視覚センサ座標系に関する概略位置２０７は、方向と距離を測定できるセンサ、例えばレーザレンジファインダを用いて求めることができる。
【０００９】
図４において、１０１は対象物距離であり、視覚センサ５１１が誘導後の視覚センサ位置５１３に来たときの視覚センサ５１１と、第１の碍子５０４との距離であり、すなわち、図３に示す誘導後の視覚センサ座標系２０３の原点（２０５）と計測点２０４との距離である。ここで、対象物距離１０１の大きさをＬとする。
１０６は誘導位置・誘導姿勢算出手段であり、誘導位置算出手段１０２と誘導姿勢算出手段１０３とから構成される。
誘導位置算出手段１０２はマニピュレータ座標系に関する概略位置２０８と対象物距離１０１とを入力し、誘動位置１０４、すなわち、誘導後の視覚センサ座標系２０３の位置を出力する手段である。また誘導姿勢算出手段１０２はマニピュレータ座標系に関する概略位置２０８と誘導位置１０４とを入力し、誘動姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する手段である。
【００１０】
まず最初に、計測点２０４を中心とし、半径をＬとした球面２０６を設定する。球面２０６の方程式は
【００１１】
【数２】
0003972356.tif 000003
【００１２】
となる。計測点２０４を通り、ＸｒＹｒ平面に平行な平面は
【００１３】
【数３】
0003972356.tif 000004
【００１４】
となる。球面２０６と視覚センサ座標系に関する概略位置２０７のベクトルとの交点を
【００１５】
【数４】
0003972356.tif 000005
【００１６】
とすると、視覚センサ座標系に関する概略位置２０７のベクトルを含みＺｒに平行な平面は
【００１７】
【数５】
0003972356.tif 000006
【００１８】
となる。式（２）、（３）、（５）を連立して解くと、
【００１９】
【数６】
0003972356.tif 000007
【００２０】
となる。
【００２１】
２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、第１の碍子５０４と同じ高さで、距離Ｌの位置に誘導後の視覚センサ座標系２０３の位置を決定することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。誘導後の視覚センサ座標系２０３のＸｓ'Ｚｓ'平面とマニピュレータ座標系２０２のＸｒＹｒ平面とが平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００２２】
【数７】
0003972356.tif 000008
【００２３】
となる。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化し
【００２４】
【数８】
0003972356.tif 000009
【００２５】
とすると、これがＺｓ'軸の単位ベクトルとなる。Ｘｓ'軸は式（７）と（８）との外積演算
【００２６】
【数９】
0003972356.tif 000010
【００２７】
により求めると、これがＸｓ'軸の単位ベクトルとなる。
【００２８】
式（７）、（８）、（９）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００２９】
【数１０】
0003972356.tif 000011
【００３０】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（６）、（１０）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００３１】
【数１１】
0003972356.tif 000012
【００３２】
となる。
以上により、誘導後の視覚センサ座標系２０３を、第１の碍子５０４と同じ高さで、距離Ｌの位置に、視覚センサ５１１の視線が第１の碍子５０４に向かう姿勢で、決定することができる。
【００３３】
【発明が解決しようとする課題】
ところが、この従来の方法では、誘導前の視覚センサ座標系２０１の位置が決まり、視覚センサ座標系に関する概略位置２０７が計測されると、誘導後の視覚センサ座標系２０３の位置と姿勢が一意に決まってしまうため、誘導後の視覚センサ座標系２０３の位置の近傍に障害物があっても、それを避けることができず、視覚センサ５１１を破損してしまうという問題があった。
そこで、本発明は対象物の近傍にある障害物を避けて、視覚センサを対象物に誘導する方法を提供することを目的とする。
【００３４】
【課題を解決するための手段】
上記問題を解決するため、本発明の請求項１の発明は、マニピュレータ手先に視覚センサを搭載し、前記マニピュレータを駆動することで対象物に前記視覚センサを誘導する方法において、前記対象物に向かう方向をＸｒ軸、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面に平行な方向をＹｒ軸、とする直交３軸からなる前記マニピュレータの座標系と、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視線方向をＺｓ軸、前記視覚センサの底面に平行な方向をＸｓ軸、とする直交３軸からなる前記視覚センサの座標系と、を設定し、前記視覚センサで前記対象物を計測して、その計測点を中心とした所定の半径の球面を表す式を設定し、誘導後の前記視覚センサの位置の近傍に障害物があるとき、前記計測点から前記障害物を回避するよう上下方向にオフセットした高さにあって、前記マニピュレータの座標系の底面を表すＸｒＹｒ面に平行な面を表す式を設定し、誘導前の前記視覚センサの座標系の原点から前記計測点までの概略位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面の式を設定し、これら３つの式から得られる２つの解の内、前記視覚センサ側の解が表す位置を誘導後の位置とし、前記計測点と誘導後の前記視覚センサの座標系の原点とを通るベクトルを正規化したものを、誘導後の前記視覚センサの座標系のＺｓ’軸の単位ベクトルとし、前記視覚センサの座標系のＹｓ’軸が前記マニピュレータの座標系のＸｒＺｒ平面と平行であるとしたときの前記Ｙｓ’軸の単位ベクトルと、前記Ｚｓ’軸の単位ベクトルとの内積が０であることを利用し正規化したものを誘導後の前記視覚センサ座標系のＹｓ’軸の単位ベクトルとし、前記Ｚｓ’軸の単位ベクトルと前記Ｙｓ’軸の単位ベクトルとの外積演算から誘導後の前記視覚センサ座標系のＸｓ’軸の単位ベクトルを求め、これら３つの単位ベクトルを要素とした座標系を誘導後の姿勢とし、前記誘導後の位置と前記誘導後の姿勢へ前記視覚センサを誘導する視覚センサの誘導方法とするものである。
【００３５】
【発明の実施の形態】
以下、図に基づいて、本発明の実施例を説明する。
【００３６】
図１は本発明の実施例を示す視覚センサの誘導方法のモデル図であり、図２は同じくブロック図である。
図１は、図５に示した装柱５０１の部分拡大図であり、従来技術と共通する構成要素には同一の符号を付したので説明を省略する。
７０１は従来技術における誘導後の視覚センサ座標系２０３の近傍に存在する障害物である。従来技術の方法で視覚センサ５１１を誘導すると、障害物７０１に衝突するので、これを回避する必要がある。そこで高さオフセット７０２または角度オフセット７０３を与えて、誘導位置の高さあるいは角度を変更する。
図２に示したブロック図は、基本的な構成は図４に示した従来のブロック図と共通するが、対象物距離１０１と概略位置２０８に加えて、オフセット６０１を入力することが異なる。ここで、オフセット６０１は障害物７０１を回避するための修正量であり、前述の高さオフセット７０２または角度オフセット７０３のいずれかを入力するものである。
【００３７】
まず、高さオフセット７０２をオフセット６０１として、誘導位置算出手段１０２に入力して、高さオフセット７０２だけ上方または下方に誘導する方法を説明する。
計測点２０４より高さｈだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる。
【００３８】
【数１２】
0003972356.tif 000013
【００３９】
式（２）、（５）、（１２）を連立して解くと、
【００４０】
【数１３】
0003972356.tif 000014
【００４１】
となる。
また、角度オフセット７０３をオフセット６０１として、誘導位置算出手段１０２に入力して、角度オフセット７０３だけ上方または下方に誘導するにはつぎのようにする。
計測点２０４より角度θだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる
【００４２】
【数１４】
0003972356.tif 000015
【００４３】
式（２）、（５）、（１４）を連立して解くと、
【００４４】
【数１５】
0003972356.tif 000016
【００４５】
となる。
【００４６】
式（１３）、（１５）において、２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、障害物７０１と衝突を回避して、視覚センサ５１１を誘導することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化したものが式（８）であり、これが誘導後の視覚センサ座標系２０３のＺｓ'軸の単位ベクトルとなる。Ｙｓ'軸がＸｒＺｒ平面と平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００４７】
【数１６】
0003972356.tif 000017
【００４８】
となる。これと式（８）との内積が０であることを利用し、正規化したものを、
【００４９】
【数１７】
0003972356.tif 000018
【００５０】
とする。これがＹｓ'軸の単位ベクトルとなる。なお、α、γの正負は所望の方向を取るように決定すれば良い。Ｘｓ'軸は式（８）と（１７）との外積演算により、
【００５１】
【数１８】
0003972356.tif 000019
【００５２】
となる。これがＸｓ'軸の単位ベクトルとなる。
式（８）、（１７）、（１８）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００５３】
【数１９】
0003972356.tif 000020
【００５４】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（１３）あるいは（１５）と、（１９）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００５５】
【数２０】
0003972356.tif 000021
【００５６】
となる。
以上により、誘導後の視覚センサ５１１の位置を、高さ、あるいは角度によってオフセットすることができる。
なお、本発明によるオフセットは上下方向に限られるものではなく、左右方向も選択できることは言うまでもない。
【００５７】
【発明の効果】
以上、説明したように、本発明は、対象物の概略位置に対して、誘導位置をオフセットすることができるので、対象物の近傍に障害物があっても簡単にそれを回避して視覚センサを対象物に誘導することができる。
【図面の簡単な説明】
【図１】  本発明の実施例を示す視覚センサの誘導方法モデル図である。
【図２】  本発明の実施例を示す視覚センサの誘導方法ブロック図である。
【図３】  従来の技術に係る視覚センサの誘導方法モデル図である。
【図４】  従来の技術に係る視覚センサの誘導方法ブロック図であ。
【図５】  装柱に対する配電作業で行われる視覚センサ誘導の概略説明図である。
【符号の説明】
１０１    対象物距離
１０２    誘導位置算出手段
１０３    誘導姿勢算出手段
１０４    誘導位置
１０５    誘導姿勢
１０６    誘導位置・誘導姿勢算出手段
２０１    誘導前の視覚センサ座標系
２０２    マニピュレータ座標系
２０３    誘導後の視覚センサ座標系
２０４    計測点
２０５    誘導後の視覚センサ座標系の原点
２０６    球面
２０７    視覚センサ座標系に関する概略位置
２０８    マニピュレータ座標系に関する概略位置
５０１    装柱
５０２    電柱
５０３    アーム
５０４    第１の碍子
５０５    第２の碍子
５０６    第３の碍子
５０７    マニピュレータ
５０８    コントローラ
５０９    ティーチボックス
５１０    操作者
５１１    視覚センサ
５１２    誘導前の視覚センサ位置
５１３    誘導後の視覚センサ位置
５１４    誘導前の映像
５１５    誘導後の映像
６０１    オフセット
７０１    障害物
７０２    高さ位置
７０３    仰角あるいは俯角
【図１】
0003972356.tif 000022
【図２】
0003972356.tif 000023
【図３】
0003972356.tif 000024
【図４】
0003972356.tif 000025
【図５】
0003972356.tif 000026