JP3972356B2 - 視覚センサの誘導方法 - Google Patents
視覚センサの誘導方法 Download PDFInfo
- Publication number
- JP3972356B2 JP3972356B2 JP2001387997A JP2001387997A JP3972356B2 JP 3972356 B2 JP3972356 B2 JP 3972356B2 JP 2001387997 A JP2001387997 A JP 2001387997A JP 2001387997 A JP2001387997 A JP 2001387997A JP 3972356 B2 JP3972356 B2 JP 3972356B2
- Authority
- JP
- Japan
- Prior art keywords
- visual sensor
- guidance
- coordinate system
- axis
- manipulator
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、マニピュレータ手先に搭載された視覚センサを対象物に誘導する視覚センサの誘導方法に関する。
【０００２】
【従来の技術】
本願出願人は先に、特願２０００−２７８７７６において、対象物の概略位置と、誘導位置と前記対象物との距離を与えて、マニピュレータを誘導位置に誘導する方法を提案している。これは、次のような方法である。
【０００３】
図５は配電作業用マニピュレータを用いて視覚センサを誘導する方法の概略説明図である。
図において、５０１は装柱である。装柱５０１は、電柱５０２、アーム５０３、第１の碍子５０４、第２の碍子５０５、第３の碍子５０６から構成される。通常、電柱５０２は地面に対しほぼ垂直に設置されており、アーム５０３は電柱５０２に対し直角に取り付けられている。第１の碍子５０４、第２の得子５０５、第３の碍子５０６は、アーム５０３に固定されている。５０７はマニピュレータであり、その先端に視覚センサ５１１を取り付けている。５１２は誘導前の視覚センサ位置であり、そこで得られる映像が誘導前の映像５１４である。５１３は誘導後の視覚センサ位置であり、そこで得られる映像が誘導後の映像５１５である。
【０００４】
ここで、視覚センサ５１１により装柱５０１を遠方から捉え、視覚センサ５１１を第１の碍子５０４に誘導する方法を説明する。より具体的には、視覚センサ５１１を視覚センサ位置５１２から視覚センサ位置５１３に誘導する方法を説明する。
【０００５】
図３は従来技術による視覚センサの誘導方法を示すモデル図であり、図４は従来技術による視覚センサの誘導方法を示すブロック図である。図３は、図５に示した装柱５０１の部分拡大図であり、第１の碍子５０４とアーム５０３の一部のみを示している。２０１及び２０３は視覚センサ５１１の内部に設置された座標系で、２０１が、誘導前の視覚センサ位置５１２の視覚センサの座標系（誘導前の視覚センサ座標系）、２０３が、誘導後の視覚センサ位置５１３の視覚センサの座標系（誘導後の視覚センサ座標系）である。図５に示した視覚センサ５１１の紙面手前方向をＸｓ軸、上方向をＹｓ軸、左方向（視覚センサの視線方向）をＺｓ軸とする。これによれば、視覚センサ５１１の底面はＸｓＺｓ平面と平行となる。誘導後の視覚センサ座標系２０３の各座標軸には、誘導前の視覚センサ座標系２０１のそれらと区別するために、「'」を付している。２０２はマニピュレータのベース部に設置された座標系（マニピュレータ座標系）で、図５に示したマニピュレータ５０７の紙面右方向をＸｒ軸、奥方向をＹｒ軸、上方向をＺｒ軸とする。これによれば、マニピュレータ５０７の底面はＸｒＹｒ平面と平行となる。
【０００６】
２０７は、誘導前の視覚センサ位置５１２から第１の碍子５０４を見た時の、第１の碍子５０４の概略位置（つまり誘導前の視覚センサ座標系２０１の原点に対する第１の碍子５０４の概略位置）を表すベクトルである。２０８はマニピュレータ座標系２０２の原点に対する第１の碍子５０４の概略位置を表すベクトルであり、次の様に、表される。
【０００７】
【数１】
【０００８】
ここで、ｔは転置である。マニピュレータ座標系に関する概略位置２０８は、マニピュレータ５０７と視覚センサ５１１とが校正されているため（Ｔが分かるため）、視覚センサ座標系に関する概略位置２０７から容易に求めることができる。 以後の説明では、マニピュレータ座標系２０２を基準に説明する。なお、視覚センサ座標系に関する概略位置２０７は、方向と距離を測定できるセンサ、例えばレーザレンジファインダを用いて求めることができる。
【０００９】
図４において、１０１は対象物距離であり、視覚センサ５１１が誘導後の視覚センサ位置５１３に来たときの視覚センサ５１１と、第１の碍子５０４との距離であり、すなわち、図３に示す誘導後の視覚センサ座標系２０３の原点（２０５）と計測点２０４との距離である。ここで、対象物距離１０１の大きさをＬとする。
１０６は誘導位置・誘導姿勢算出手段であり、誘導位置算出手段１０２と誘導姿勢算出手段１０３とから構成される。
誘導位置算出手段１０２はマニピュレータ座標系に関する概略位置２０８と対象物距離１０１とを入力し、誘動位置１０４、すなわち、誘導後の視覚センサ座標系２０３の位置を出力する手段である。また誘導姿勢算出手段１０２はマニピュレータ座標系に関する概略位置２０８と誘導位置１０４とを入力し、誘動姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する手段である。
【００１０】
まず最初に、計測点２０４を中心とし、半径をＬとした球面２０６を設定する。球面２０６の方程式は
【００１１】
【数２】
【００１２】
となる。計測点２０４を通り、ＸｒＹｒ平面に平行な平面は
【００１３】
【数３】
【００１４】
となる。球面２０６と視覚センサ座標系に関する概略位置２０７のベクトルとの交点を
【００１５】
【数４】
【００１６】
とすると、視覚センサ座標系に関する概略位置２０７のベクトルを含みＺｒに平行な平面は
【００１７】
【数５】
【００１８】
となる。式（２）、（３）、（５）を連立して解くと、
【００１９】
【数６】
【００２０】
となる。
【００２１】
２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、第１の碍子５０４と同じ高さで、距離Ｌの位置に誘導後の視覚センサ座標系２０３の位置を決定することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。誘導後の視覚センサ座標系２０３のＸｓ'Ｚｓ'平面とマニピュレータ座標系２０２のＸｒＹｒ平面とが平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００２２】
【数７】
【００２３】
となる。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化し
【００２４】
【数８】
【００２５】
とすると、これがＺｓ'軸の単位ベクトルとなる。Ｘｓ'軸は式（７）と（８）との外積演算
【００２６】
【数９】
【００２７】
により求めると、これがＸｓ'軸の単位ベクトルとなる。
【００２８】
式（７）、（８）、（９）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００２９】
【数１０】
【００３０】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（６）、（１０）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００３１】
【数１１】
【００３２】
となる。
以上により、誘導後の視覚センサ座標系２０３を、第１の碍子５０４と同じ高さで、距離Ｌの位置に、視覚センサ５１１の視線が第１の碍子５０４に向かう姿勢で、決定することができる。
【００３３】
【発明が解決しようとする課題】
ところが、この従来の方法では、誘導前の視覚センサ座標系２０１の位置が決まり、視覚センサ座標系に関する概略位置２０７が計測されると、誘導後の視覚センサ座標系２０３の位置と姿勢が一意に決まってしまうため、誘導後の視覚センサ座標系２０３の位置の近傍に障害物があっても、それを避けることができず、視覚センサ５１１を破損してしまうという問題があった。
そこで、本発明は対象物の近傍にある障害物を避けて、視覚センサを対象物に誘導する方法を提供することを目的とする。
【００３４】
【課題を解決するための手段】
上記問題を解決するため、本発明の請求項１の発明は、マニピュレータ手先に視覚センサを搭載し、前記マニピュレータを駆動することで対象物に前記視覚センサを誘導する方法において、前記対象物に向かう方向をＸｒ軸、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面に平行な方向をＹｒ軸、とする直交３軸からなる前記マニピュレータの座標系と、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視線方向をＺｓ軸、前記視覚センサの底面に平行な方向をＸｓ軸、とする直交３軸からなる前記視覚センサの座標系と、を設定し、前記視覚センサで前記対象物を計測して、その計測点を中心とした所定の半径の球面を表す式を設定し、誘導後の前記視覚センサの位置の近傍に障害物があるとき、前記計測点から前記障害物を回避するよう上下方向にオフセットした高さにあって、前記マニピュレータの座標系の底面を表すＸｒＹｒ面に平行な面を表す式を設定し、誘導前の前記視覚センサの座標系の原点から前記計測点までの概略位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面の式を設定し、これら３つの式から得られる２つの解の内、前記視覚センサ側の解が表す位置を誘導後の位置とし、前記計測点と誘導後の前記視覚センサの座標系の原点とを通るベクトルを正規化したものを、誘導後の前記視覚センサの座標系のＺｓ’軸の単位ベクトルとし、前記視覚センサの座標系のＹｓ’軸が前記マニピュレータの座標系のＸｒＺｒ平面と平行であるとしたときの前記Ｙｓ’軸の単位ベクトルと、前記Ｚｓ’軸の単位ベクトルとの内積が０であることを利用し正規化したものを誘導後の前記視覚センサ座標系のＹｓ’軸の単位ベクトルとし、前記Ｚｓ’軸の単位ベクトルと前記Ｙｓ’軸の単位ベクトルとの外積演算から誘導後の前記視覚センサ座標系のＸｓ’軸の単位ベクトルを求め、これら３つの単位ベクトルを要素とした座標系を誘導後の姿勢とし、前記誘導後の位置と前記誘導後の姿勢へ前記視覚センサを誘導する視覚センサの誘導方法とするものである。
【００３５】
【発明の実施の形態】
以下、図に基づいて、本発明の実施例を説明する。
【００３６】
図１は本発明の実施例を示す視覚センサの誘導方法のモデル図であり、図２は同じくブロック図である。
図１は、図５に示した装柱５０１の部分拡大図であり、従来技術と共通する構成要素には同一の符号を付したので説明を省略する。
７０１は従来技術における誘導後の視覚センサ座標系２０３の近傍に存在する障害物である。従来技術の方法で視覚センサ５１１を誘導すると、障害物７０１に衝突するので、これを回避する必要がある。そこで高さオフセット７０２または角度オフセット７０３を与えて、誘導位置の高さあるいは角度を変更する。
図２に示したブロック図は、基本的な構成は図４に示した従来のブロック図と共通するが、対象物距離１０１と概略位置２０８に加えて、オフセット６０１を入力することが異なる。ここで、オフセット６０１は障害物７０１を回避するための修正量であり、前述の高さオフセット７０２または角度オフセット７０３のいずれかを入力するものである。
【００３７】
まず、高さオフセット７０２をオフセット６０１として、誘導位置算出手段１０２に入力して、高さオフセット７０２だけ上方または下方に誘導する方法を説明する。
計測点２０４より高さｈだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる。
【００３８】
【数１２】
【００３９】
式（２）、（５）、（１２）を連立して解くと、
【００４０】
【数１３】
【００４１】
となる。
また、角度オフセット７０３をオフセット６０１として、誘導位置算出手段１０２に入力して、角度オフセット７０３だけ上方または下方に誘導するにはつぎのようにする。
計測点２０４より角度θだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる
【００４２】
【数１４】
【００４３】
式（２）、（５）、（１４）を連立して解くと、
【００４４】
【数１５】
【００４５】
となる。
【００４６】
式（１３）、（１５）において、２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、障害物７０１と衝突を回避して、視覚センサ５１１を誘導することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化したものが式（８）であり、これが誘導後の視覚センサ座標系２０３のＺｓ'軸の単位ベクトルとなる。Ｙｓ'軸がＸｒＺｒ平面と平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００４７】
【数１６】
【００４８】
となる。これと式（８）との内積が０であることを利用し、正規化したものを、
【００４９】
【数１７】
【００５０】
とする。これがＹｓ'軸の単位ベクトルとなる。なお、α、γの正負は所望の方向を取るように決定すれば良い。Ｘｓ'軸は式（８）と（１７）との外積演算により、
【００５１】
【数１８】
【００５２】
となる。これがＸｓ'軸の単位ベクトルとなる。
式（８）、（１７）、（１８）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００５３】
【数１９】
【００５４】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（１３）あるいは（１５）と、（１９）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００５５】
【数２０】
【００５６】
となる。
以上により、誘導後の視覚センサ５１１の位置を、高さ、あるいは角度によってオフセットすることができる。
なお、本発明によるオフセットは上下方向に限られるものではなく、左右方向も選択できることは言うまでもない。
【００５７】
【発明の効果】
以上、説明したように、本発明は、対象物の概略位置に対して、誘導位置をオフセットすることができるので、対象物の近傍に障害物があっても簡単にそれを回避して視覚センサを対象物に誘導することができる。
【図面の簡単な説明】
【図１】 本発明の実施例を示す視覚センサの誘導方法モデル図である。
【図２】 本発明の実施例を示す視覚センサの誘導方法ブロック図である。
【図３】 従来の技術に係る視覚センサの誘導方法モデル図である。
【図４】 従来の技術に係る視覚センサの誘導方法ブロック図であ。
【図５】 装柱に対する配電作業で行われる視覚センサ誘導の概略説明図である。
【符号の説明】
１０１ 対象物距離
１０２ 誘導位置算出手段
１０３ 誘導姿勢算出手段
１０４ 誘導位置
１０５ 誘導姿勢
１０６ 誘導位置・誘導姿勢算出手段
２０１ 誘導前の視覚センサ座標系
２０２ マニピュレータ座標系
２０３ 誘導後の視覚センサ座標系
２０４ 計測点
２０５ 誘導後の視覚センサ座標系の原点
２０６ 球面
２０７ 視覚センサ座標系に関する概略位置
２０８ マニピュレータ座標系に関する概略位置
５０１ 装柱
５０２ 電柱
５０３ アーム
５０４ 第１の碍子
５０５ 第２の碍子
５０６ 第３の碍子
５０７ マニピュレータ
５０８ コントローラ
５０９ ティーチボックス
５１０ 操作者
５１１ 視覚センサ
５１２ 誘導前の視覚センサ位置
５１３ 誘導後の視覚センサ位置
５１４ 誘導前の映像
５１５ 誘導後の映像
６０１ オフセット
７０１ 障害物
７０２ 高さ位置
７０３ 仰角あるいは俯角
【発明の属する技術分野】
本発明は、マニピュレータ手先に搭載された視覚センサを対象物に誘導する視覚センサの誘導方法に関する。
【０００２】
【従来の技術】
本願出願人は先に、特願２０００−２７８７７６において、対象物の概略位置と、誘導位置と前記対象物との距離を与えて、マニピュレータを誘導位置に誘導する方法を提案している。これは、次のような方法である。
【０００３】
図５は配電作業用マニピュレータを用いて視覚センサを誘導する方法の概略説明図である。
図において、５０１は装柱である。装柱５０１は、電柱５０２、アーム５０３、第１の碍子５０４、第２の碍子５０５、第３の碍子５０６から構成される。通常、電柱５０２は地面に対しほぼ垂直に設置されており、アーム５０３は電柱５０２に対し直角に取り付けられている。第１の碍子５０４、第２の得子５０５、第３の碍子５０６は、アーム５０３に固定されている。５０７はマニピュレータであり、その先端に視覚センサ５１１を取り付けている。５１２は誘導前の視覚センサ位置であり、そこで得られる映像が誘導前の映像５１４である。５１３は誘導後の視覚センサ位置であり、そこで得られる映像が誘導後の映像５１５である。
【０００４】
ここで、視覚センサ５１１により装柱５０１を遠方から捉え、視覚センサ５１１を第１の碍子５０４に誘導する方法を説明する。より具体的には、視覚センサ５１１を視覚センサ位置５１２から視覚センサ位置５１３に誘導する方法を説明する。
【０００５】
図３は従来技術による視覚センサの誘導方法を示すモデル図であり、図４は従来技術による視覚センサの誘導方法を示すブロック図である。図３は、図５に示した装柱５０１の部分拡大図であり、第１の碍子５０４とアーム５０３の一部のみを示している。２０１及び２０３は視覚センサ５１１の内部に設置された座標系で、２０１が、誘導前の視覚センサ位置５１２の視覚センサの座標系（誘導前の視覚センサ座標系）、２０３が、誘導後の視覚センサ位置５１３の視覚センサの座標系（誘導後の視覚センサ座標系）である。図５に示した視覚センサ５１１の紙面手前方向をＸｓ軸、上方向をＹｓ軸、左方向（視覚センサの視線方向）をＺｓ軸とする。これによれば、視覚センサ５１１の底面はＸｓＺｓ平面と平行となる。誘導後の視覚センサ座標系２０３の各座標軸には、誘導前の視覚センサ座標系２０１のそれらと区別するために、「'」を付している。２０２はマニピュレータのベース部に設置された座標系（マニピュレータ座標系）で、図５に示したマニピュレータ５０７の紙面右方向をＸｒ軸、奥方向をＹｒ軸、上方向をＺｒ軸とする。これによれば、マニピュレータ５０７の底面はＸｒＹｒ平面と平行となる。
【０００６】
２０７は、誘導前の視覚センサ位置５１２から第１の碍子５０４を見た時の、第１の碍子５０４の概略位置（つまり誘導前の視覚センサ座標系２０１の原点に対する第１の碍子５０４の概略位置）を表すベクトルである。２０８はマニピュレータ座標系２０２の原点に対する第１の碍子５０４の概略位置を表すベクトルであり、次の様に、表される。
【０００７】
【数１】
ここで、ｔは転置である。マニピュレータ座標系に関する概略位置２０８は、マニピュレータ５０７と視覚センサ５１１とが校正されているため（Ｔが分かるため）、視覚センサ座標系に関する概略位置２０７から容易に求めることができる。 以後の説明では、マニピュレータ座標系２０２を基準に説明する。なお、視覚センサ座標系に関する概略位置２０７は、方向と距離を測定できるセンサ、例えばレーザレンジファインダを用いて求めることができる。
【０００９】
図４において、１０１は対象物距離であり、視覚センサ５１１が誘導後の視覚センサ位置５１３に来たときの視覚センサ５１１と、第１の碍子５０４との距離であり、すなわち、図３に示す誘導後の視覚センサ座標系２０３の原点（２０５）と計測点２０４との距離である。ここで、対象物距離１０１の大きさをＬとする。
１０６は誘導位置・誘導姿勢算出手段であり、誘導位置算出手段１０２と誘導姿勢算出手段１０３とから構成される。
誘導位置算出手段１０２はマニピュレータ座標系に関する概略位置２０８と対象物距離１０１とを入力し、誘動位置１０４、すなわち、誘導後の視覚センサ座標系２０３の位置を出力する手段である。また誘導姿勢算出手段１０２はマニピュレータ座標系に関する概略位置２０８と誘導位置１０４とを入力し、誘動姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する手段である。
【００１０】
まず最初に、計測点２０４を中心とし、半径をＬとした球面２０６を設定する。球面２０６の方程式は
【００１１】
【数２】
となる。計測点２０４を通り、ＸｒＹｒ平面に平行な平面は
【００１３】
【数３】
となる。球面２０６と視覚センサ座標系に関する概略位置２０７のベクトルとの交点を
【００１５】
【数４】
とすると、視覚センサ座標系に関する概略位置２０７のベクトルを含みＺｒに平行な平面は
【００１７】
【数５】
となる。式（２）、（３）、（５）を連立して解くと、
【００１９】
【数６】
となる。
【００２１】
２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、第１の碍子５０４と同じ高さで、距離Ｌの位置に誘導後の視覚センサ座標系２０３の位置を決定することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。誘導後の視覚センサ座標系２０３のＸｓ'Ｚｓ'平面とマニピュレータ座標系２０２のＸｒＹｒ平面とが平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００２２】
【数７】
となる。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化し
【００２４】
【数８】
とすると、これがＺｓ'軸の単位ベクトルとなる。Ｘｓ'軸は式（７）と（８）との外積演算
【００２６】
【数９】
により求めると、これがＸｓ'軸の単位ベクトルとなる。
【００２８】
式（７）、（８）、（９）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００２９】
【数１０】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（６）、（１０）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００３１】
【数１１】
となる。
以上により、誘導後の視覚センサ座標系２０３を、第１の碍子５０４と同じ高さで、距離Ｌの位置に、視覚センサ５１１の視線が第１の碍子５０４に向かう姿勢で、決定することができる。
【００３３】
【発明が解決しようとする課題】
ところが、この従来の方法では、誘導前の視覚センサ座標系２０１の位置が決まり、視覚センサ座標系に関する概略位置２０７が計測されると、誘導後の視覚センサ座標系２０３の位置と姿勢が一意に決まってしまうため、誘導後の視覚センサ座標系２０３の位置の近傍に障害物があっても、それを避けることができず、視覚センサ５１１を破損してしまうという問題があった。
そこで、本発明は対象物の近傍にある障害物を避けて、視覚センサを対象物に誘導する方法を提供することを目的とする。
【００３４】
【課題を解決するための手段】
上記問題を解決するため、本発明の請求項１の発明は、マニピュレータ手先に視覚センサを搭載し、前記マニピュレータを駆動することで対象物に前記視覚センサを誘導する方法において、前記対象物に向かう方向をＸｒ軸、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面に平行な方向をＹｒ軸、とする直交３軸からなる前記マニピュレータの座標系と、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視線方向をＺｓ軸、前記視覚センサの底面に平行な方向をＸｓ軸、とする直交３軸からなる前記視覚センサの座標系と、を設定し、前記視覚センサで前記対象物を計測して、その計測点を中心とした所定の半径の球面を表す式を設定し、誘導後の前記視覚センサの位置の近傍に障害物があるとき、前記計測点から前記障害物を回避するよう上下方向にオフセットした高さにあって、前記マニピュレータの座標系の底面を表すＸｒＹｒ面に平行な面を表す式を設定し、誘導前の前記視覚センサの座標系の原点から前記計測点までの概略位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面の式を設定し、これら３つの式から得られる２つの解の内、前記視覚センサ側の解が表す位置を誘導後の位置とし、前記計測点と誘導後の前記視覚センサの座標系の原点とを通るベクトルを正規化したものを、誘導後の前記視覚センサの座標系のＺｓ’軸の単位ベクトルとし、前記視覚センサの座標系のＹｓ’軸が前記マニピュレータの座標系のＸｒＺｒ平面と平行であるとしたときの前記Ｙｓ’軸の単位ベクトルと、前記Ｚｓ’軸の単位ベクトルとの内積が０であることを利用し正規化したものを誘導後の前記視覚センサ座標系のＹｓ’軸の単位ベクトルとし、前記Ｚｓ’軸の単位ベクトルと前記Ｙｓ’軸の単位ベクトルとの外積演算から誘導後の前記視覚センサ座標系のＸｓ’軸の単位ベクトルを求め、これら３つの単位ベクトルを要素とした座標系を誘導後の姿勢とし、前記誘導後の位置と前記誘導後の姿勢へ前記視覚センサを誘導する視覚センサの誘導方法とするものである。
【００３５】
【発明の実施の形態】
以下、図に基づいて、本発明の実施例を説明する。
【００３６】
図１は本発明の実施例を示す視覚センサの誘導方法のモデル図であり、図２は同じくブロック図である。
図１は、図５に示した装柱５０１の部分拡大図であり、従来技術と共通する構成要素には同一の符号を付したので説明を省略する。
７０１は従来技術における誘導後の視覚センサ座標系２０３の近傍に存在する障害物である。従来技術の方法で視覚センサ５１１を誘導すると、障害物７０１に衝突するので、これを回避する必要がある。そこで高さオフセット７０２または角度オフセット７０３を与えて、誘導位置の高さあるいは角度を変更する。
図２に示したブロック図は、基本的な構成は図４に示した従来のブロック図と共通するが、対象物距離１０１と概略位置２０８に加えて、オフセット６０１を入力することが異なる。ここで、オフセット６０１は障害物７０１を回避するための修正量であり、前述の高さオフセット７０２または角度オフセット７０３のいずれかを入力するものである。
【００３７】
まず、高さオフセット７０２をオフセット６０１として、誘導位置算出手段１０２に入力して、高さオフセット７０２だけ上方または下方に誘導する方法を説明する。
計測点２０４より高さｈだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる。
【００３８】
【数１２】
式（２）、（５）、（１２）を連立して解くと、
【００４０】
【数１３】
となる。
また、角度オフセット７０３をオフセット６０１として、誘導位置算出手段１０２に入力して、角度オフセット７０３だけ上方または下方に誘導するにはつぎのようにする。
計測点２０４より角度θだけ上にあって、ＸｒＹｒに平行な平面は次式で与えられる
【００４２】
【数１４】
式（２）、（５）、（１４）を連立して解くと、
【００４４】
【数１５】
となる。
【００４６】
式（１３）、（１５）において、２つある解の内、視覚センサ５１１側の方を取ると、これが誘導後の視覚センサ座標系２０３の位置、すなわち、誘導位置１０４となる。これにより、障害物７０１と衝突を回避して、視覚センサ５１１を誘導することができる。誘導姿勢算出手段１０３では、誘導位置１０４とマニピュレータ座標系に関する概略位置２０８とを入力し、誘導姿勢１０５、すなわち、誘導後の視覚センサ座標系２０３の姿勢を出力する。計測点２０４と誘導後の視覚センサ座標系の原点２０５とを通るベクトルを正規化したものが式（８）であり、これが誘導後の視覚センサ座標系２０３のＺｓ'軸の単位ベクトルとなる。Ｙｓ'軸がＸｒＺｒ平面と平行であるとすると、Ｙｓ'軸の単位ベクトルは
【００４７】
【数１６】
となる。これと式（８）との内積が０であることを利用し、正規化したものを、
【００４９】
【数１７】
とする。これがＹｓ'軸の単位ベクトルとなる。なお、α、γの正負は所望の方向を取るように決定すれば良い。Ｘｓ'軸は式（８）と（１７）との外積演算により、
【００５１】
【数１８】
となる。これがＸｓ'軸の単位ベクトルとなる。
式（８）、（１７）、（１８）の各軸を要素とした誘導後の視覚センサ座標系２０３の姿勢が
【００５３】
【数１９】
で求められる。これにより、第１の碍子５０４に視覚センサ５１１の視線方向が向かっている誘導後の視覚センサ座標系２０３の姿勢を決定することができる。
式（１３）あるいは（１５）と、（１９）より、誘導後の視覚センサ座標系２０３の位置と姿勢Ｔ'は
【００５５】
【数２０】
となる。
以上により、誘導後の視覚センサ５１１の位置を、高さ、あるいは角度によってオフセットすることができる。
なお、本発明によるオフセットは上下方向に限られるものではなく、左右方向も選択できることは言うまでもない。
【００５７】
【発明の効果】
以上、説明したように、本発明は、対象物の概略位置に対して、誘導位置をオフセットすることができるので、対象物の近傍に障害物があっても簡単にそれを回避して視覚センサを対象物に誘導することができる。
【図面の簡単な説明】
【図１】 本発明の実施例を示す視覚センサの誘導方法モデル図である。
【図２】 本発明の実施例を示す視覚センサの誘導方法ブロック図である。
【図３】 従来の技術に係る視覚センサの誘導方法モデル図である。
【図４】 従来の技術に係る視覚センサの誘導方法ブロック図であ。
【図５】 装柱に対する配電作業で行われる視覚センサ誘導の概略説明図である。
【符号の説明】
１０１ 対象物距離
１０２ 誘導位置算出手段
１０３ 誘導姿勢算出手段
１０４ 誘導位置
１０５ 誘導姿勢
１０６ 誘導位置・誘導姿勢算出手段
２０１ 誘導前の視覚センサ座標系
２０２ マニピュレータ座標系
２０３ 誘導後の視覚センサ座標系
２０４ 計測点
２０５ 誘導後の視覚センサ座標系の原点
２０６ 球面
２０７ 視覚センサ座標系に関する概略位置
２０８ マニピュレータ座標系に関する概略位置
５０１ 装柱
５０２ 電柱
５０３ アーム
５０４ 第１の碍子
５０５ 第２の碍子
５０６ 第３の碍子
５０７ マニピュレータ
５０８ コントローラ
５０９ ティーチボックス
５１０ 操作者
５１１ 視覚センサ
５１２ 誘導前の視覚センサ位置
５１３ 誘導後の視覚センサ位置
５１４ 誘導前の映像
５１５ 誘導後の映像
６０１ オフセット
７０１ 障害物
７０２ 高さ位置
７０３ 仰角あるいは俯角
Claims (1)
- マニピュレータ手先に視覚センサを搭載し、前記マニピュレータを駆動することで対象物に前記視覚センサを誘導する方法において、
前記対象物に向かう方向をＸｒ軸、前記マニピュレータの上方向をＺｒ軸、前記マニピュレータの底面に平行な方向をＹｒ軸、とする直交３軸からなる前記マニピュレータの座標系と、前記視覚センサの上方向をＹｓ軸、前記視覚センサの視線方向をＺｓ軸、前記視覚センサの底面に平行な方向をＸｓ軸、とする直交３軸からなる前記視覚センサの座標系と、を設定し、
前記視覚センサで前記対象物を計測して、その計測点を中心とした所定の半径の球面を表す式を設定し、
誘導後の前記視覚センサの位置の近傍に障害物があるとき、前記計測点から前記障害物を回避するよう上下方向にオフセットした高さにあって、前記マニピュレータの座標系の底面を表すＸｒＹｒ面に平行な面を表す式を設定し、
誘導前の前記視覚センサの座標系の原点から前記計測点までの概略位置ベクトルを含み、前記マニピュレータの座標系のＺｒ軸に平行な平面の式を設定し、
これら３つの式から得られる２つの解の内、前記視覚センサ側の解が表す位置を誘導後の位置とし、
前記計測点と誘導後の前記視覚センサの座標系の原点とを通るベクトルを正規化したものを、誘導後の前記視覚センサの座標系のＺｓ’軸の単位ベクトルとし、
前記視覚センサの座標系のＹｓ’軸が前記マニピュレータの座標系のＸｒＺｒ平面と平行であるとしたときの前記Ｙｓ’軸の単位ベクトルと、前記Ｚｓ’軸の単位ベクトルとの内積が０であることを利用し正規化したものを誘導後の前記視覚センサ座標系のＹｓ’軸の単位ベクトルとし、
前記Ｚｓ’軸の単位ベクトルと前記Ｙｓ’軸の単位ベクトルとの外積演算から誘導後の前記視覚センサ座標系のＸｓ’軸の単位ベクトルを求め、
これら３つの単位ベクトルを要素とした座標系を誘導後の姿勢とし、
前記誘導後の位置と前記誘導後の姿勢へ前記視覚センサを誘導することを特徴とする視覚センサの誘導方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001387997A JP3972356B2 (ja) | 2001-12-20 | 2001-12-20 | 視覚センサの誘導方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001387997A JP3972356B2 (ja) | 2001-12-20 | 2001-12-20 | 視覚センサの誘導方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003191194A JP2003191194A (ja) | 2003-07-08 |
| JP3972356B2 true JP3972356B2 (ja) | 2007-09-05 |
Family
ID=27596660
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001387997A Expired - Fee Related JP3972356B2 (ja) | 2001-12-20 | 2001-12-20 | 視覚センサの誘導方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3972356B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5850958B2 (ja) | 2014-01-24 | 2016-02-03 | ファナック株式会社 | ワークを撮像するためのロボットプログラムを作成するロボットプログラミング装置 |
-
2001
- 2001-12-20 JP JP2001387997A patent/JP3972356B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003191194A (ja) | 2003-07-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8964001B2 (en) | Method for displaying measurement effective area in three-dimensional visual sensor and three-dimensional visual sensor | |
| US9050728B2 (en) | Apparatus and method for measuring tool center point position of robot | |
| US11072078B2 (en) | Method for measuring pose of robotic end tool | |
| CN111300481B (zh) | 基于视觉及激光传感器的机器人抓取位姿纠正方法 | |
| WO2020121396A1 (ja) | ロボットキャリブレーションシステム及びロボットキャリブレーション方法 | |
| KR20170058720A (ko) | 레이저 거리 센서를 구비한 용접 로봇 및 그의 교정방법 | |
| US6554144B2 (en) | Boom positioning device for high-position working | |
| KR20210034070A (ko) | 교정 작업 지원 시스템 | |
| JP3972356B2 (ja) | 視覚センサの誘導方法 | |
| JP2001158599A5 (ja) | ||
| JP2008281418A (ja) | 位置姿勢推定方法 | |
| EP3895855A1 (en) | Robot control system and robot control method | |
| JP3215616B2 (ja) | ロボットの誘導方法および誘導システム | |
| WO2023013740A1 (ja) | ロボット制御装置、ロボット制御システム、及びロボット制御方法 | |
| JP4568978B2 (ja) | マニピュレータ装置 | |
| JPH0727408B2 (ja) | 固定３次元視覚併用ロボットハンドリング装置 | |
| KR100993241B1 (ko) | 이동 로봇 모니터링 시스템, 및 방법 | |
| JP2602292B2 (ja) | 溶接システムおよび溶接方法 | |
| KR100326000B1 (ko) | 무인차의 위치보정장치 및 위치보정방법 | |
| CN110672009A (zh) | 基于机器视觉的基准定位、物体姿态调整及图形显示方法 | |
| JPH08322120A (ja) | 高所作業用ブームの位置決め方法および位置決め支援装置 | |
| JP2000094370A (ja) | ロボットの作業表面の傾き測定方法およびその測定装置 | |
| JPH05197416A (ja) | ロボット動作点教示装置 | |
| JP7278637B2 (ja) | 自走式移動装置 | |
| EP3889542B1 (en) | Method for estimating normal vector to die and/or normal vector to attachment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20041118 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20060524 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20060612 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060804 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070111 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070207 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070521 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070603 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100622 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110622 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120622 Year of fee payment: 5 |
|
| LAPS | Cancellation because of no payment of annual fees |