JP7299442B1 - 制御装置、３次元位置計測システム、及びプログラム - Google Patents

Abstract

ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサにより撮像した画像に基づき検出された検出対象の中から、３つ以上の検出対象を選択した組み合わせを複数生成する組合せ生成部と、生成された複数の組み合わせのそれぞれについて計算した、３つ以上の検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、複数の組み合わせから１以上の組み合わせを選択する選択部と、選択された１以上の組み合わせからワークの３次元位置を決定する３次元位置決定部と、を備える制御装置である。

Description

本開示は、制御装置、３次元位置計測システム、及びプログラムに関する。

視覚センサを用いて３次元物体の３次元位置を計測するための様々な計測システムが提案されている。例えば、特許文献１－２は、３次元物体における互いに位置関係が既知の３つの検出対象を３台のカメラでそれぞれ検出し、当該３つの検出対象の検出位置から３次元物体の３次元位置を計測する方法を記載する。

なお、３次元位置計測に関連し、特許文献３は、ステレオカメラを用いた３次元認識処理に用いられる３次元モデル作成する方法を記載する。特許文献４は、コンベヤによって搬送される物品の位置及び姿勢の３次元計測を行う方法の例を記載する。

特開平７－１３６１３号公報 特開昭６２－５４１１５号公報 特開２０１０－１２１９９９号公報 特開２０１９－１２８２７４号公報

特許文献１及び２に記載のように３次元物体上の３つの検出対象を検出し、当該３つの検出対象の検出位置により３次元物体の３次元位置を求める構成の場合、計測結果には、検出対象自体の位置の誤差や、検出対象の検出位置における計測誤差が含まれ得る。よって、３つの検出対象の検出位置による３次元物体の計測では十分な精度が得られない場合が有り得る。また、３つの検出対象のうち、一部の誤差が大きい場合、その誤差に引きずられて全体の誤差、すなわち、３次元物体の３次元位置計測結果が大きくなってしまうことも想定され得る。

検出対象の検出位置に含まれ得る誤差による影響を低減でき、それにより３次元物体の３次元位置の計測の精度を向上し得る技術が望まれている。

本開示の一態様は、ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサにより撮像した画像に基づき検出された検出対象の中から選択した組み合わせを複数生成する組合せ生成部と、生成された前記複数の組み合わせのそれぞれについて計算した、前記検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する選択部と、選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する３次元位置決定部と、を備える制御装置である。

添付図面に示される本発明の典型的な実施形態の詳細な説明から、本発明のこれらの目的、特徴および利点ならびに他の目的、特徴および利点がさらに明確になるであろう。

一実施形態に係るロボット制御装置を含むロボットシステムの機器構成を表す図である。 ワークの実例としての車体及び検出対象を示す図である。 ビジョン座標系及びワーク上の零偏差位置にある各標点に割り当てたセンサ座標系を示す図である。 センサ座標系及び標点の画像平面への射影を示す図である。 ロボット制御装置及び画像処理装置の機能ブロック図である。 ３次元位置計測処理の基本動作を表すフローチャートである。

次に、本開示の実施形態について図面を参照して説明する。参照する図面において、同様の構成部分または機能部分には同様の参照符号が付けられている。理解を容易にするために、これらの図面は縮尺を適宜変更している。また、図面に示される形態は本発明を実施するための一つの例であり、本発明は図示された形態に限定されるものではない。

図１は一実施形態に係るロボット制御装置５０を含むロボットシステム１００の機器構成を表す図である。図１に示すように、ロボットシステム１００は、ロボット１０と、ロボット１０の手先部に搭載した視覚センサ７０と、ロボット１０を制御するロボット制御装置５０と、教示操作盤４０と、画像処理装置２０とを含む。教示操作盤４０及び画像処理装置２０は、ロボット制御装置５０に接続されている。視覚センサ７０は、画像処理装置２０に接続されている。本実施形態に係るロボットシステム１００は、台１（搬送装置上のキャリッジ、架台など）に置かれた３次元物体であるワークＷ上の３つ以上の検出対象を検出することで、ワークＷの３次元位置を高い精度で計測することができる３次元位置計測システムとして構成される。

ロボット１０は、垂直多関節ロボットであるものとする。なお、ロボット１０として、水平多関節ロボット、パラレルリンク型ロボット、双腕ロボット等、作業対象に応じて他のタイプのロボットが用いられても良い。ロボット１０は、手首部に取り付けられたエンドエフェクタによって所望の作業を実行することができる。エンドエフェクタは、用途に応じて交換可能な外部装置であり、例えば、ハンド、溶接ガン、工具等である。図１では、エンドエフェクタとしてのハンド３３が用いられている例を示す。

ロボット制御装置５０は、動作プログラム或いは教示操作盤４０からの指令に従ってロボット１０の動作を制御する。ロボット制御装置５０は、プロセッサ５１（図５）、メモリ（ＲＯＭ、ＲＡＭ、不揮発性メモリ等）、記憶装置、操作部、入出力インタフェース、ネットワークインタフェース等を有する一般的なコンピュータとしてのハードウェア構成を有していても良い。

画像処理装置２０は、視覚センサ７０を制御する機能と、対象物の検出処理等を含む画像処理を行う機能とを有する。画像処理装置２０は、プロセッサ、メモリ（ＲＯＭ、ＲＡＭ、不揮発性メモリ等）、記憶装置、操作部、表示部、入出力インタフェース、ネットワークインタフェース等を有する一般的なコンピュータとしてのハードウェア構成を有していても良い。

なお、図１では、視覚センサ７０の制御及び画像処理の機能を担う画像処理装置を独立した装置としてロボットシステム１００内に配置する構成例を記載しているが、画像処理装置２０としての機能がロボット制御装置５０内に一体として組み込まれていても良い。

教示操作盤４０は、ロボット１０の教示や各種設定を行うための操作端末として用いられる。教示操作盤４０として、タブレット端末等により構成された教示装置を用いても良い。教示操作盤４０は、プロセッサ、メモリ（ＲＯＭ、ＲＡＭ、不揮発性メモリ等）、記憶装置、操作部、表示部４１（図５）、入出力インタフェース、ネットワークインタフェース等を有する一般的なコンピュータとしてのハードウェア構成を有していても良い。

３次元位置計測の対象であるワークＷは、例えば図２に示すような車体である。ワークＷには、互いの位置関係が既知の位置に３つ以上の検出対象（例えば円形孔Ｍ）が設けられている。これら検出対象は、例えば車体の底面に配置される。ロボットシステム１００は、視覚センサ７０によりこの３つ以上の検出対象の位置を検出することで、ワークＷ全体の３次元位置を算出する。ロボットシステム１００は、ワークＷの３次元位置を得て、ワークＷに対する各種作業を適切に実行することができる。

図１では、視覚センサ７０をロボット１０の手先に搭載する場合の構成例を示している。この構成では、ロボット１０により視覚センサ７０を移動させて、視覚センサ７０を、検出対象（円形孔Ｍ）を撮像するためのそれぞれ撮像位置に位置付け、検出対象を撮像して検出するようにする。基準位置にあるワークＷの各検出対象を撮像可能な撮像位置を予めロボット１０に教示しておいても良い。

視覚センサをロボット１０に搭載する構成に代えて、作業空間に固定配置した１以上の視覚センサにより検出対象を撮像して検出する構成としても良い。この場合、ワーク上の複数の検出対象を、それぞれ撮像する複数の視覚センサを配置しても良い。或いは、一つの視覚センサが２以上の検出対象を撮像するような配置としても良い。後者の場合、視覚センサの配置数は、検出対象の合計数よりも少なくすることができる。

視覚センサ７０による検出対象の撮像位置（姿勢）に関しては、視覚センサのどの撮像位置（姿勢）における画像平面も互いに平面になるようにしてはならないという制約に従うようにする。なお、どの画像平面に対する法線ベクトルも互いにかなりの角度を成すようにすることが好ましい。

ロボットシステム１００（ロボット制御装置５０）は、ワークＷ上の３つ以上の検出対象の位置を検出し、その検出位置に基づきワークＷの３次元位置を求める。以下では、基本的な検出手法である、ワーク上の３つの検出対象の位置を検出する手法について説明した上で、それを４つ以上の検出対象に拡張する手法を述べる。その上で、３つ以上の検出対象の検出位置に基づく３次元物体の３次元位置の決定について説明する。

ワークＷ上の３つの検出対象を検出しワークの３次元位置を求める手法について説明する。ワークＷ上の３つの検出対象の位置を検出する「位置検出機能」は、画像処理装置２０の画像処理部（検出部）１２１（図５）の機能として提供されても良い。図３に示すように、ワークＷは、３個の既知点（すなわち検出対象）を備えた剛体と考えることができる。ワークＷが零偏差位置、つまり、理想的公称位置にあるとき、ワークＷ上、もしくはその近傍の原点においたローカル座標系であるビジョン座標系（以下、VCSとも記載する）を考慮する。零偏差を与える位置にある各検出対象（以下、標点とも記載する）にそれぞれ対応する点において、それらの点を始点として３本の直交するベクトルを立て、それらベクトルの大きさを単位長に、またその方向をビジョン座標系VCSの３個のベクトルの方向に平行にする。３本の単位ベクトルが各点において作る小座標系をセンサ座標系１、２、３（それぞれSCS1、SCS2、SCS3とも記載する）と称することとする。これら３つのセンサ座標系の変換は不変である。

ビジョン座標系VCSは、視覚センサ７０の撮像位置（姿勢）に対して固定されている関係にあるものとする。ワークＷに固定した座標系をワーク座標系（BCSとも記載する）と称する。ワークＷがその零偏差位置にあるとき、各標点は３つのセンサ座標系の各原点に正確に対応する。

ワークＷがその零偏差位置から移動するとき、ワークＷが受ける剛体の運動は、VCSをBCSに関係づける変換[T]によって完全に決定される。この変換は、VCSに関して定義された変換であって、この変換によってBCSの位置と方位、従ってワークＷの位置が完全に決定される。

VCSにおける標点の零偏差位置座標及びこの標点が変位したときに占める位置座標が与えられれば、零偏差位置座標と変位した位置座標とはこの変換[T]によって直接関係付けられることとなる。各撮像位置における視覚センサ７０の視野内で標点を検出することによって変換[T]を決定できるようにすることが以下で説明する３次元位置決定機能の目的である。

ワークＷが若干の偏差を持つ位置にあるとき、画像平面上の標点はSCS座標系の原点から離れた位置に移動している。図４に、この場合の、SCS1座標系及び標点P₁の画像平面への射影を示す。一般に画像平面上への３個以上の射影を較正データと組み合わせることによって、３次元物体の公称位置からの偏差の６個の自由度を決定することができる。点P₁がSCS1座標系のX-Y平面上にあるものとの仮定により、各カメラ位置における撮像画像から互いに独立して点P₁の位置を解くことができる。ベクトルA、B、及びPを以下に示すように定義する。uは画像平面上での横軸、vは画像平面上での縦軸である。ハット付きu、vは、それぞれ、画像平面の横軸、縦軸方向の単位ベクトルである。

ベクトルAとベクトルBは、SCS1座標系におけるX方向の単位ベクトルとY方向の単位ベクトルの画像平面への射影である。点P₁のX座標とY座標（つまり、x1とy1）は以下に示す式（１）から（４）により与えられる。

図４を参照し、より一般化した場合は、以下の式（５）から（９）によって表される。この場合は、zは任意の値を取り得るものと仮定される。

これらの式（５）から（９）から、式（１０）から（１１）を得て、x1、y1をz1で表す解を得る。

上記x1、y1は、次のように書き直される。

ここで、α１、β１、γ１、δ１は、以下の式で与えられる定数である。

式（１２）は、x1とy1が共にｚ1の線形関数であることを示す。同様の方程式を他の２つの画像平面についても導出する。方程式の完全な組は、式（１３）から（１５）によって与えられる。式（１３）から（１５）に現れる諸定数は、較正治具を用いた較正により取得することができる。較正治具として、例えば、SCS座標系の互いに直交する座標軸に対応する稜線及び目盛りを有するキューブを、３本の稜線がSCS座標系の互いに直交する座標軸に平行になるように位置付ける。そして、視覚センサ７０により、標点（SCS座標系）を撮像する位置姿勢でキューブを撮像し、キューブの実寸法に関する情報を用い、SCS座標系のX,Y,Z軸の単位ベクトルが画像上でどのようなベクトルに相当するかについての情報（較正データ）を得ることができる。このような較正データは、予め画像処理装置２０の記憶部１２２（図５）等に保存しておく。

方程式（１３）から（１５）は９個の未知数を持つ６個の一次方程式である。これらの方程式を解くために追加の拘束条件として、ワークが剛体であることが考慮される。すなわち、ここでは、ワーク上の標点間の距離は一定であるという条件が用いられる。各SCS座標系の原点を、それぞれ（Ｘ_O1，Ｙ_O1，Ｚ_O1）、（Ｘ_O2，Ｙ_O2，Ｚ_O2）、（Ｘ_O3，Ｙ_O3，Ｚ_O3）と表し、変位後の各標点の座標は、Ｐ₁（Ｘ₁，Ｙ₁，Ｚ₁）、Ｐ₂（Ｘ₂，Ｙ₂，Ｚ₂）、Ｐ₃（Ｘ₃，Ｙ₃，Ｚ₃）と表す。３つのSCS座系の原点間の距離は、以下のように表され、また、変位後の各標点間の距離は式（１６）のように与えられる。

方程式（１３）から（１５）を方程式（１６）に代入することによって、下記の方程式の第１の組（式（１７））を得る。また、それらの方程式を書き換えて方程式の第２の組（式（１８））を得る。これらの方程式においてk,l,mは定数である。

上記第２の方程式の組（式（１８））は、例えば、ニュートン（Newton）の繰り返し法を用いることで解かれる。これらの値が求まったら、それらの値を式（１３）から（１５）に代入しx1,x2,x3及びy1,y2,y3を得る。これによって得られる(x1,y1,z1）、（x2,y2,z2）、（x3,y3,z3）は、各標点の変位後の、各SCS座標系上での位置である。これらは、VSC上における値に変換され得る。これにより、VCSをBCSに関係づける変換[T]を得ることができる。すなわち、変位後のワークＷの３次元位置が得られることとなる。

なお、上記手法では、３つの標点が画像平面上へ正射影されることを仮定している。実射影は、透視投影に近いものであることを考慮して、写像関係の誤差を補正する処理がなされても良い。この誤差を補償するために、以下の方程式によって与えられるような写像関係を実座標軸とその各射影軸との間に確立する。各軸に対するこの写像関係は較正時に各座標軸上で３点以上の測定行い、必要な関係を得るため補間法を使用することによって得ることができる。

以下は、新しいスケールファクタの計算結果を示す。

上記計算手法は３点の位置偏差を最小二乗法を用いて計算するというものである。最小二乗法計算の最後に、座標軸の射影に新しいスケールファクタを乗じて非線形性を補償する。３組の方程式によって与えられる新しいスケールファクタを使用して、上述の各定数α、β、γ、δを各画像平面について計算し直す。その後、最小二乗法による計算を再度行う。

上記計算手法を４つ以上の標点を用いる場合に拡張することを考慮する。ここでは、４つの標点が用いられる場合について記載する。上述のように各標点がSCS座標系のX-Y平面上にあるものと仮定の下で、上述の式（１）から（４）で説明した方程式を４つの標点に関して立てることで、各標点の位置が得られることとなる。

より一般化した場合に関しては、３つの標点について式（１３）から（１５）を得た場合と同様に、下記式（１９）から（２２）のように、４つの標点のx、y座標をzの線形関数として表すことができる。

次に、ワークＷが剛体であることに基づき、４つのSCS座標系の原点間の距離と４つの計測点（標点）間の距離が等しいことに基づき、４つのSCS座標系の原点間の距離、及び４つの標点間の距離に関し次の式を得る。なお、ここでは、４つのSCS座標系の原点間の距離（４つの標点間の距離）としてｄ１２、ｄ２３、ｄ３４、ｄ４１に関する式を立てて方程式（１９）から（２２）の解を求めることを記載するが、更に、４つのSCS座標系の原点間の距離（４つの標点間の距離）としてｄ１３、ｄ２４に関する式を立て、それらの式も考慮に加える形で方程式（１９）から（２２）の解を求めるようにしても良い。

式（１９）から（２２）を式（２３）に代入することで、以下の様に、上記式（１７）及び式（１８）を４つの標点について拡張した式（２４）及び式（２５）を得る。

この方程式を上述の手法の場合と同様に繰り返し法により解き、x1,x2,x3,x4及びy1,y2,y3,y4、すなわち、４つの標点の変位後の位置を得ることができる。そして、４つの標点の検出位置を合成する形で、ワークＷの３次元位置を得る。すなわち、これらの検出位置から、VCSをBCSに関係づける変換[T]を得る。

さらに増加した数の検出対象（標点）について計測を行う場合にも、同様のやり方で拡張できることを理解することができる。

なお、３つ以上の検出対象（標点）の検出位置からワークＷの３次元位置を決定する手法としては、様々な手法を用いることができる。例示として、以下のような各種手法を、適用することができる。なお、以下に例示する手法において、検出対象（標点）の配置に関する条件がある場合にはそれを順守する。
（１）上述の変換[T]のパラメータ（並進及び回転を表すパラメータ）を、連立方程式を解く形で求める手法。
（２）特許文献４（特開２０１９－１２８２７４号公報）に記載されているように、各標点の検出位置に対するカメラの視線に既知形状の多角形（零偏差位置にある標点を結ぶ多角形）を当てはめることでワークの位置及び姿勢を特定する手法。
（３）ワーク上の３つ以上の標点の位置からワーク上の座標系の平面（ＸＹ平面等）を特定することで当該座標系を把握する手法。この場合、例えば、第１の標点を原点、第２の標点がＸ軸方向の位置、第３の標点（及び第４以降の標点）がＸＹ平面上の位置を表すものとして座標系を把握する。
このように３つ以上の検出対象（標点）の検出位置からワークＷの３次元位置を求める計算機能は、ロボット制御装置５０における選択部１５３或いは３次元位置決定部１５４内の機能として実装されていても良い。

図５は、ロボット制御装置５０及び画像処理装置２０の機能ブロック図である。図５に示すように、ロボット制御装置５０は、動作制御部１５１と、組合せ生成部１５２と、選択部１５３と、３次元位置決定部１５４と、を備える。なお、これらの機能ブロックは、ロボット制御装置５０のプロセッサ５１がプログラムを実行することにより実現されるものであっても良い。また、ロボット制御装置５０は、記憶部１５５を備える。

記憶部１５５は、例えば不揮発性メモリ、ハードディスク装置等により構成される。記憶部１５５は、ロボット１０を制御する動作プログラム、視覚センサ７０により撮像された画像に基づきワークの検出等の画像処理を行うプログラム（ビジョンプログラム）、各種設定情報等が格納されている。

動作制御部１５１は、ロボットの動作プログラムにしたがってロボットの動作を制御する。ロボット制御装置５０は、動作制御部１５１が生成する各軸に対する指令に従って各軸のサーボモータに対するサーボ制御を実行するサーボ制御部（不図示）を備えている。動作制御部１５１は、視覚センサ７０を移動させて各検出対象を撮像するための撮像位置に位置付ける機能を担う。

組合せ生成部１５２は、ワークＷ上で検出された検出対象の中から、３つ以上の検出対象を選択した組み合わせを複数生成する機能を提供する。

選択部１５３は、生成された複数の組み合わせのそれぞれから計算した「ずれ量」に基づいて、複数の組み合わせから１以上の組み合わせを選択する機能を提供する。

３次元位置決定部１５４は、選択部１５３により選択された、検出対象の１以上の組み合わせからワークＷの３次元位置情報を決定する機能を提供する。組合せ生成部１５２、選択部１５３及び３次元位置決定部１５４の機能の詳細は後述する。

画像処理装置２０は、画像処理部１２１と記憶部１２２とを備える。記憶部１２２は、例えば不揮発性メモリからなる記憶装置である。記憶部１２２は、検出対象の形状データ、較正データ等の画像処理に必要な各種データを記憶する。画像処理部１２１は、ワークの検出処理等の各種画像処理を実行する。すなわち、画像処理部１２１は、視覚センサ７０が検出対象を含む撮像範囲において撮像した画像上で検出対象を検出する検出部としての機能を有する。

ロボット制御装置５０によるワークＷの３次元計測機能について説明する。図６は、ロボット制御装置５０（プロセッサ５１）による制御の下で実行される３次元位置計測処理の基本動作を表すフローチャートである。

はじめに、画像処理部（検出部）１２１は、視覚センサ７０により検出対象を撮像した画像に基づき検出対象を検出する（ステップＳ１）。ここでは、ロボット１０により視覚センサ７０は各々の検出対象を撮像する撮像位置に位置付けられ、検出対象を含む画像を撮像する。画像処理部（検出部）１２１は、上述した位置検出機能により３つ以上の検出対象の各々の位置（x,y）を得る。

次に、組合せ生成部１５２は、検出された検出対象の中から、３つ以上の検出対象を選択した複数の組み合わせを生成する（ステップＳ２）。例えば、組合せ生成部１５２は、検出された３つ以上の検出対象から全ての可能な組み合わせを生成しても良い。この場合、例えば、検出された検出対象の個数が５個であれば、可能な組み合わせの数は、５つ全部の検出対象を用いる組み合わせの数、５つの検出対象うち４つを用いる組み合わせの数、及び５つの検出対象うち３つを用いる組み合わせの数の合計数となる。

或いは、組合せ生成部１５２は、以下のような規則にしたがって検出対象の組み合わせを生成しても良い。
（規則１）検出された３つ以上の検出対象から除外するものを選択する。ただし、少なくとも３つは検出対象を残すようにする。
（規則２）除外する検出対象の最大数を指定しても良い
（規則３）残す検出対象の最低数を指定しても良い。

除外する検出対象を選択する場合、除外する検出対象を変えることで、複数の検出対象の組み合わせが生成され得る。例えば、８個の検出対象が検出されているとき、除外する最大数の指定が２であれば、（１＋８＋８×７÷２＝３７）通りの検出結果の組み合わせが生成される。

組合せ生成部１５２は、「除外する検出対象の選択」、「除外する検出対象の最大数」、或いは「残す検出対象の最低数」の入力（外部装置からの入力、或いはユーザ入力）を受け付けるように構成されていても良い。ユーザ入力を受け付けるためのユーザインタフェースは、教示操作盤４０の表示部４１に提示されても良い。ユーザ入力は、教示操作盤４０の操作部を介して行われても良い。組合せ生成部１５２は、「除外する検出対象の選択」、「除外する検出対象の最大数」、或いは「残す検出対象の最低数」について予めロボット制御装置５０に設定されている値を用いて組み合わせを生成しても良い。

このように、より多くの検出対象を全体の３次元位置（ワークＷの３次元位置）の計算に取り込むようにすることで、一つ一つの検出対象が含み得る誤差の影響を全体として低減し、３次元位置の計測の精度を高めることが可能となる。

次に、選択部１５３は、生成された検出対象の組み合わせのそれぞれに関し、全体の３次元位置（ワークＷの３次元位置）、及び、組み合わせに含まれる３つ以上の検出対象の検出位置の理想位置からの位置ずれを表す指標（以下、この指標を「位置ずれ」と称することとする）を計算する。そして、選択部１５３は、「位置ずれ」に基づき、１以上の組み合わせを選択する（ステップＳ３）。

例示として、選択部１５３は、「位置ずれ」を以下の様に計算する。ある組み合わせについて全体の３次元位置が位置Ａとして求まったとする。ワークＷ上のｉ番目の検出対象の設計上の位置Ｐｉを使って、ワークＷの３次元位置が位置Ａであるときの検出対象の理想的な位置はＡ・Ｐｉと求められる。この組み合わせにおける検出対象の数をｎとする。選択部１５３は、例えば、位置ずれＤを、Ａ・Ｐｉと、上述の式（２５）で求められるｉ番目の検出対象（標点）の変位後の位置P'ｉとの差Ｋｉに基づき算出しても良い。例えば、選択部１５３は、位置ずれＤを、Ｋｉの平均値ΣＫｉ／ｎとして求めても良い。この場合、位置ずれＤは、ある組み合わせに関し、その組合せに含まれる検出対象の検出位置の理想位置からのずれ量がどの程度であるかについての指標となる。或いは、選択部１５３は、位置ずれＤを、ｉ番目の検出対象の実際の検出位置への視線ＬｉとＡ・Ｐｉとの距離Ｄｉに基づき算出しても良い。例えば、選択部１５３は、位置ずれＤを、Ｄｉの平均ΣＤｉ／ｎとして求めても良い。この場合においても、位置ずれＤは、ある組み合わせに関し、その組合せに含まれる検出対象の検出位置の理想位置からのずれ量がどの程度であるかについての指標となる。

選択部１５３は、生成された組み合わせのそれぞれについて計算された位置ずれＤに基づいて１以上の組み合わせを選択することができる。この場合、選択部１５３は、
（ｒ１）位置ずれＤが小さいほど精度が良い、
との選択基準を用いて組み合わせの選択を行うことができる。
したがって、例えば、選択部１５３は、位置ずれＤの値が小さい所定数の組み合わせを選択しても良く、或いは、位置ずれＤの値が最も小さい一つの組み合わせを選択しても良い。

このように、位置ずれＤに基づいて全体の３次元位置（ワークＷの３次元位置）の算出に用いる組み合わせを選択する構成とすることで、大きな誤差を持つ可能性が高い組み合わせを排除し、３次元位置の計測の精度を高めることが可能となる。

次に、３次元位置決定部１５４は、選択部１５３によって選択された１以上の組み合わせから、ワークＷの最終的な３次元位置を決定する（ステップＳ４）。選択部１５３によって選択されている組み合わせが一つである場合には、３次元位置決定部１５４は、当該一つの組み合わせにより求められているワークＷの位置Ａを、ワークＷの最終的な３次元位置として決定しても良い。

選択部１５３によって選択されている組み合わせが複数ある場合には、３次元位置決定部１５４は、当該複数の組み合わせそれぞれについて求められるワークＷの３次元位置に関する統計量に基づき、ワークＷの最終的な３次元位置を決定しても良い。例えば、３次元位置決定部１５４は、選択されている複数の組み合わせについてそれぞれ求められるワークＷの３次元位置の平均値或いは中央値を、ワークＷの最終的な３次元位置として決定しても良い。

このように、本実施形態に係る３次元位置計測処理によれば、誤差の影響を低減し、３次元物体の３次元位置の計測の精度を向上させることができる。

上記３次元位置計測処理のステップＳ３において組み合わせを選択する場合に、選択部１５３は、生成されている組み合わせの各々における検出対象の個数を更に考慮しても良い。この場合、選択部１５３は、
（ｒ１）位置ずれＤが小さいほど精度が良い、及び
（ｒ２）組み合わせにおける検出対象の数が多いほど精度が良い、
との選択基準を用いて選択を行っても良い。なお、この場合の選択基準（ｒ２）は、検出対象の個数が多いほど、それぞれが含み得る誤差を丸め全体としての位置計測精度を高め得ることに基づく。

一例として、位置ずれＤが良好な（比較的小さい）組み合わせの選択候補が複数ある状況であるとする。この場合、選択部１５３は、複数の選択候補の中から、検出対象の個数が大きい１又は複数の組み合わせを選択しても良い。

上記３次元位置計測処理のステップＳ２において組み合わせを生成する場合に、組合せ生成部１５２は、検出された検出対象から生成し得る組み合わせ中から特定の組み合わせを選択したものを、生成した組み合わせとして出力しても良い。例えば、ステップＳ１において検出されている検出対象の個数が多い状況を考慮する。この場合、生成し得る組み合わせの数が非常に多くなる。このような状況では、組合せ生成部１５２は、生成し得る全ての組み合わせの中からランダムに選択したものを出力しても良い。これにより、多数の組み合わせ候補の中から偏りなく組み合わせを選択して用いることが可能となる。

上記３次元位置計測処理のステップＳ３において、位置ずれＤ、或いは、位置ずれＤ及び検出対象の個数に基づいて選択された組み合わせが多数存在するような状況を考慮する。この場合、当該選択されている組み合わせについてステップＳ２からＳ３の処理を更に１回以上繰り返すことで、選択される組み合わせの数を絞り込むようにしても良い。この場合、
（１）選択部１５３により選択された１以上の組み合わせに含まれている検出対象に基づいて、組合せ生成部１５２は、３つ以上の検出対象を選択した複数の組み合わせ（第２の複数の組み合わせ）を再度生成し、
（２）第２の複数の組み合わせの各々について計算した指標（位置ずれ）に基づいて、選択部１５３が、第２の複数の組み合わせから１以上の組み合わせを再度選択すること、を１以上の回数実行するようにする。

例えば、検出されている検出対象の数が２０個で、組合せ生成部１５２での１回目の組み合わせの生成で「残す検出対象の最低数を１０個とする」との規則で組み合せを生成したところ、選択部１５３により選択された組み合わせの数がかなりの数になっているとする。この場合、組合せ生成部１５２は、選択部１５３により選択されている組み合わせに含まれている検出対象に関し、例えば「残す検出対象の最低数を１５個とする」との規則を適用し、第２の複数の組み合わせの生成を行っても良い。ただし、この場合には、選択部１５３により予め選択されている組み合わせを母集合として、その中から、「残す検出対象の最低数を１５個とする」との規則に適合する組み合わせを選択する形で、第２の複数の組み合わせを生成することとする。選択部１５３は、第２の複数の組み合わせから、上述の選択基準（ｒ１）、或いは、上述の選択基準（ｒ１）及び（ｒ２）に基づいて組み合わせの選択を行っても良い。

また、組合せ生成部１５２の組み合わせの生成と選択部１５３の選択を繰り返すことによる選択の絞り込みに関しては次のように行っても良い。
組合せ生成部１５２が、選択部１５３により選択された１以上の組み合わせから、ある検出位置について算出されている位置ずれを表す指標（例えば、上記Ｋｉ或いはＤｉ）が他の検出位置について算出されている位置ずれを表す指標よりも大きいという基準を満たす１以上の検出位置を削除することで、３つ以上の検出対象を含む複数の組み合わせを再度生成し、再度生成された各組み合わせにおける検出対象について算出された位置ずれを表す指標が所定の条件を満たすまで１以上の回数実行する。この場合において、所定の条件は、再度生成された各組み合わせにおける検出対象についての位置ずれを表す指標の平均値、又は、当該指標の値が所定の値以下であることであっても良い。

具体的には、以下のように動作するようにしても良い。
（ｂ１）組合せ生成部１５２が、選択部１５３により選択された１以上の組み合わせから、「ある検出位置について算出されている差Ｋｉが他の検出位置について算出されている差Ｋｉよりも大きい」という基準を満たす１以上の検出位置を削除することで、３つ以上の検出対象を含む複数の組み合わせを再度生成する動作を、
（ｂ２）生成される組み合わせについてのΣＫｉ／ｎ又はＫｉが所定の値以下になるように１以上の回数実行する。上記（ｂ１）では、例えば、ある組み合わせに含まれる検出対象に関し、差Ｋｉが大きい所定数の検出対象を削除するといった処理を行っても良い。

或いは、組合せ生成部１５２の組み合わせの生成と選択部１５３の選択を繰り返すことによる選択の絞り込みに関しては次のように行っても良い。
（ｃ１）組合せ生成部１５２が、選択部１５３により選択された１以上の組み合わせから、「ある検出位置について算出されている距離Ｄiが他の検出位置について算出されている距離Ｄｉよりも大きい」という基準を満たす１以上の検出位置を削除することで、３つ以上の検出対象を含む複数の組み合わせを再度生成する動作を、
（ｃ２）生成される組み合わせについてのΣＤｉ／ｎ又はＤｉが所定の値以下になるように１以上の回数実行する。上記（ｃ１）では、例えば、ある組み合わせに含まれる検出対象に関し、距離Ｄｉが大きい所定数の検出対象を削除するといった処理を行っても良い。

このように選択の繰り返しを行う構成とすることで、特に、検出対象の個数が多いような状況で、好適な選択候補の絞り込みを高速に行うことができる。

以上説明したように、本実施形態によれば、検出対象の検出位置に含まれ得る誤差の影響を低減でき、それにより３次元物体の３次元位置計測の精度を向上させることができる。

図３に示した機能ブロック図における機能配置は例示であり、ロボットシステム１００内における機能配分に関しては様々な変形例を成し得る。例えば、ロボット制御装置５０における機能の一部を教示操作盤４０側に配置するような構成例も有り得る。

教示操作盤４０とロボット制御装置５０全体をロボット制御装置と定義することもできる。

上述の実施形態におけるロボット制御装置の構成（画像処理装置の機能を統合した場合を含む）は、様々な産業機械の制御装置に適用することができる。

図５に示されるロボット制御装置及び画像処理装置の機能ブロックは、これらの装置のプロセッサが、記憶装置に格納された各種ソフトウェアを実行することで実現されても良く、或いは、ＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェアを主体とした構成により実現されても良い。

上述した実施形態における３次元位置計測処理等の各種の処理を実行するプログラムは、コンピュータに読み取り可能な各種記録媒体（例えば、ＲＯＭ、ＥＥＰＲＯＭ、フラッシュメモリ等の半導体メモリ、磁気記録媒体、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ等の光ディスク）に記録することができる。

本開示について詳述したが、本開示は上述した個々の実施形態に限定されるものではない。これらの実施形態は、本開示の要旨を逸脱しない範囲で、または、特許請求の範囲に記載された内容とその均等物から導き出される本開示の趣旨を逸脱しない範囲で、種々の追加、置き換え、変更、部分的削除等が可能である。また、これらの実施形態は、組み合わせて実施することもできる。例えば、上述した実施形態において、各動作の順序や各処理の順序は、一例として示したものであり、これらに限定されるものではない。また、上述した実施形態の説明に数値又は数式が用いられている場合も同様である。

上記実施形態および変形例に関し更に以下の付記を記載する。
（付記１）
ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサ（７０）により撮像した画像に基づき検出された検出対象の中から、３つ以上の検出対象を選択した組み合わせを複数生成する組合せ生成部（１５２）と、生成された前記複数の組み合わせのそれぞれについて計算した、前記３つ以上の検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する選択部（１５３）と、選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する３次元位置決定部（１５４）と、を備える制御装置（５０）。
（付記２）
前記組合せ生成部（１５２）は、前記検出された検出対象から、全ての可能な前記組み合わせを生成する、付記１に記載の制御装置（５０）。
（付記３）
前記組合せ生成部（１５２）は、前記検出された検出対象から所定数の検出対象を除外または選択して前記組み合わせを複数生成する、付記１に記載の制御装置（５０）。
（付記４）
前記組合せ生成部（１５２）は、前記検出された検出対象から生成可能な前記組み合わせからランダムに選択を行うことで、前記組み合わせを複数生成する、付記１に記載の制御装置（５０）。
（付記５）
前記選択部（１５３）は、生成された前記複数の組み合わせの各々について、
（１）一つの組み合わせから求められる前記ワークの３次元位置を位置Ａ、前記ワーク上のｉ番目の前記検出対象の設計上の位置をＰｉとするとき、前記ワーク上のｉ番目の前記検出対象の理想位置をＡ・Ｐｉとして求め、
（２）当該一つの組み合わせにおけるｉ番目の前記検出対象の検出位置P'ｉとＡ・Ｐｉとの差Ｋｉを当該一つの組み合わせ中の検出対象の各々について求め、当該求められたに差Ｋｉに基づき前記指標を求める、付記１から４のいずれか一項に記載の制御装置（５０）。
（付記６）
前記選択部（１５３）は、前記一つの組み合わせにおける検出対象の数をｎとするとき、前記差Ｋｉの平均値であるΣＫｉ／ｎを前記指標として求める、付記５に記載の制御装置（５０）。
（付記７）
前記選択部（１５３）は、生成された前記複数の組み合わせの各々について、
（１）一つの組み合わせから求められる前記ワークの３次元位置を位置Ａ、前記ワーク上のｉ番目の前記検出対象の設計上の位置をＰｉとするとき、前記ワーク上のｉ番目の前記検出対象の理想位置をＡ・Ｐｉとして求め、
（２）当該一つの組み合わせにおける前記視覚センサからｉ番目の検出対象の検出位置への視線Ｌｉと、Ａ・Ｐｉとの距離Ｄｉを当該一つの組み合わせ中の検出対象の各々について求め、当該求められた距離Ｄｉに基づき前記指標を求める、付記１から４のいずれか一項に記載の制御装置（５０）。
（付記８）
前記選択部（１５３）は、前記一つの組み合わせにおける検出対象の数をｎとするとき、前記距離Ｄｉの平均値であるΣＤｉ／ｎを前記指標として求める、付記７に記載の制御装置（５０）。
（付記９）
前記選択部（１５３）は、前記指標の大きさが小さいほど精度が良いとの選択基準を用いて、前記１以上の組み合わせの選択を行う、付記１から８のいずれか一項に記載の制御装置（５０）。
（付記１０）
前記選択部（１５３）は、前記複数の組み合わせのそれぞれについて計算した前記指標と、前記複数の組み合わせの各々における検出対象の数とに基づいて、前記複数の組み合わせから１以上の組み合わせを選択する、付記１から８のいずれか一項に記載の制御装置（５０）。
（付記１１）
前記選択部（１５３）は、前記複数の組み合わせの各々に関し、
（１）前記指標の大きさが小さいほど精度が良い、及び
（２）組み合わせにおける検出対象の数が多いほど精度が良い、
との選択基準を用いて、前記１以上の組み合わせの選択を行う、付記１０に記載の制御装置（５０）。
（付記１２）
前記３次元位置決定部（１５４）は、選択された前記１以上の組み合わせのそれぞれにより得られる前記ワークの３次元位置の統計量に基づき、前記ワークの３次元位置を決定する、付記１から１１のいずれか一項に記載の制御装置（５０）。
（付記１３）
前記３次元位置決定部（１５４）は、選択された前記１以上の組み合わせのそれぞれにより得られる前記ワークの３次元位置の平均値又は中央値を前記３次元物体の３次元位置として決定する、付記１２に記載の制御装置（５０）。
（付記１４）
前記選択部（１５３）により選択された前記１以上の組み合わせに含まれている検出対象に基づいて、前記組合せ生成部（１５２）が３つ以上の検出対象を選択した複数の組み合わせを再度生成し、再度生成された前記複数の組み合わせの各々について計算した前記指標に基づいて、前記選択部（１５３）が、再度生成された前記複数の組み合わせから１以上の組み合わせを再度選択すること、からなる動作を１以上の回数実行する、付記１から１３のいずれか一項に記載の制御装置（５０）。
（付記１５）
前記組合せ生成部（１５２）が、前記選択部（１５３）により選択された前記１以上の組み合わせから、ある検出位置について算出されている前記位置ずれを表す指標が他の検出位置について算出されている前記位置ずれを表す指標よりも大きいという基準を満たす１以上の検出位置を削除することで、３つ以上の検出対象を含む複数の組み合わせを再度生成し、再度生成された各組み合わせにおける前記検出対象について算出された前記位置ずれを表す指標が所定の条件を満たすまで１以上の回数実行する、付記１から４のいずれか一項に記載の制御装置（５０）。
（付記１６）
前記所定の条件は、再度生成された各組み合わせにおける前記検出対象についての前記位置ずれを表す指標の平均値、又は、当該指標の値が所定の値以下であることである、付記１５に記載の制御装置（５０）。
（付記１７）
視覚センサ（７０）と、ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を前記視覚センサにより撮像した画像に基づき検出する検出部（１２１）と、検出された検出対象の中から、３つ以上の検出対象を選択した組み合わせを複数生成する組合せ生成部（１５２）と、生成された前記複数の組み合わせのそれぞれについて計算した、前記３つ以上の検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する選択部（１５３）と、選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する３次元位置決定部（１５４）と、を備える３次元位置計測システム（１００）。
（付記１８）
前記視覚センサ（７０）を搭載したロボット（１０）と、前記ロボット（１０）を制御して前記視覚センサ（７０）を前記３つ以上の検出対象をそれぞれ撮像するための撮像位置に位置付ける動作制御部（１５１）と、を更に備える付記１７に記載の３次元位置計測システム（１００）。
（付記１９）
コンピュータのプロセッサに、ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサ（７０）により撮像した画像に基づき検出する手順と、検出された検出対象の中から、３つ以上の検出対象を選択した組み合わせを複数生成する手順と、生成された前記複数の組み合わせのそれぞれについて計算した、前記３つ以上の検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する手順と、選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する手順と、を実行させるためのプログラム。

１ 台
１０ ロボット
２０ 画像処理装置
３３ ハンド
４０ 教示操作盤
４１ 表示部
５０ ロボット制御装置
５１ プロセッサ
７０ 視覚センサ
１００ ロボットシステム
１２１ 画像処理部
１２２ 記憶部
１５１ 動作制御部
１５２ 組合せ生成部
１５３ 選択部
１５４ ３次元位置決定部
１５５ 記憶部

Claims (19)

1. ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサにより撮像した画像に基づき検出された検出対象の中から選択した組み合わせを複数生成する組合せ生成部と、
   生成された前記複数の組み合わせのそれぞれについて計算した、前記検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する選択部と、
   選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する３次元位置決定部と、
   を備える制御装置。
2. 前記組合せ生成部は、前記検出された検出対象から、全ての可能な前記組み合わせを生成する、請求項１に記載の制御装置。
3. 前記組合せ生成部は、前記検出された検出対象から所定数の検出対象を除外または選択して前記組み合わせを複数生成する、請求項１に記載の制御装置。
4. 前記組合せ生成部は、前記検出された検出対象から生成可能な前記組み合わせからランダムに選択を行うことで、前記組み合わせを複数生成する、請求項１に記載の制御装置。
5. 前記選択部は、生成された前記複数の組み合わせの各々について、
   （１）一つの組み合わせから求められる前記ワークの３次元位置を位置Ａ、前記ワーク上のｉ番目の前記検出対象の設計上の位置をＰｉとするとき、前記ワーク上のｉ番目の前記検出対象の理想位置をＡ・Ｐｉとして求め、
   （２）当該一つの組み合わせにおけるｉ番目の前記検出対象の検出位置P'ｉとＡ・Ｐｉとの差Ｋｉを当該一つの組み合わせ中の検出対象の各々について求め、当該求められたに差Ｋｉに基づき前記指標を求める、
   請求項１から４のいずれか一項に記載の制御装置。
6. 前記選択部は、前記一つの組み合わせにおける検出対象の数をｎとするとき、前記差Ｋｉの平均値であるΣＫｉ／ｎを前記指標として求める、請求項５に記載の制御装置。
7. 前記選択部は、生成された前記複数の組み合わせの各々について、
   （１）一つの組み合わせから求められる前記ワークの３次元位置を位置Ａ、前記ワーク上のｉ番目の前記検出対象の設計上の位置をＰｉとするとき、前記ワーク上のｉ番目の前記検出対象の理想位置をＡ・Ｐｉとして求め、
   （２）当該一つの組み合わせにおける前記視覚センサからｉ番目の検出対象の検出位置への視線Ｌｉと、Ａ・Ｐｉとの距離Ｄｉを当該一つの組み合わせ中の検出対象の各々について求め、当該求められた距離Ｄｉに基づき前記指標を求める、
   請求項１から４のいずれか一項に記載の制御装置。
8. 前記選択部は、前記一つの組み合わせにおける検出対象の数をｎとするとき、前記距離Ｄｉの平均値であるΣＤｉ／ｎを前記指標として求める、請求項７に記載の制御装置。
9. 前記選択部は、前記指標の大きさが小さいほど精度が良いとの選択基準を用いて、前記１以上の組み合わせの選択を行う、請求項１から４のいずれか一項に記載の制御装置。
10. 前記選択部は、前記複数の組み合わせのそれぞれについて計算した前記指標と、前記複数の組み合わせの各々における検出対象の数とに基づいて、前記複数の組み合わせから１以上の組み合わせを選択する、請求項１から４のいずれか一項に記載の制御装置。
11. 前記選択部は、前記複数の組み合わせの各々に関し、
    （１）前記指標の大きさが小さいほど精度が良い、及び
    （２）組み合わせにおける検出対象の数が多いほど精度が良い、
    との選択基準を用いて、前記１以上の組み合わせの選択を行う、請求項１０に記載の制御装置。
12. 前記３次元位置決定部は、選択された前記１以上の組み合わせのそれぞれにより得られる前記ワークの３次元位置の統計量に基づき、前記ワークの３次元位置を決定する、請求項１から４のいずれか一項に記載の制御装置。
13. 前記３次元位置決定部は、選択された前記１以上の組み合わせのそれぞれにより得られる前記ワークの３次元位置の平均値又は中央値を前記ワークの３次元位置として決定する、請求項１２に記載の制御装置。
14. 前記選択部により選択された前記１以上の組み合わせに含まれている検出対象に基づいて、前記組合せ生成部が３つ以上の検出対象を選択した複数の組み合わせを再度生成し、
    再度生成された前記複数の組み合わせの各々について計算した前記指標に基づいて、前記選択部が、再度生成された前記複数の組み合わせから１以上の組み合わせを再度選択すること、
    からなる動作を１以上の回数実行する、請求項１から４のいずれか一項に記載の制御装置。
15. 前記組合せ生成部が、前記選択部により選択された前記１以上の組み合わせから、ある検出位置について算出されている前記位置ずれを表す指標が他の検出位置について算出されている前記位置ずれを表す指標よりも大きいという基準を満たす１以上の検出位置を削除することで、検出対象を含む複数の組み合わせを再度生成し、再度生成された各組み合わせにおける前記検出対象について算出された前記位置ずれを表す指標が所定の条件を満たすまで１以上の回数実行する、請求項１から４のいずれか一項に記載の制御装置。
16. 前記所定の条件は、再度生成された各組み合わせにおける前記検出対象についての前記位置ずれを表す指標の平均値、又は、当該指標の値が所定の値以下であることである、請求項１５に記載の制御装置。
17. 視覚センサと、
    ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を前記視覚センサにより撮像した画像に基づき検出する検出部と、
    検出された検出対象の中から、検出対象を選択した組み合わせを複数生成する組合せ生成部と、
    生成された前記複数の組み合わせのそれぞれについて計算した、前記検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する選択部と、
    選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する３次元位置決定部と、
    を備える３次元位置計測システム。
18. 前記視覚センサを搭載したロボットと、
    前記ロボットを制御して前記視覚センサを前記３つ以上の前記検出対象をそれぞれ撮像するための撮像位置に位置付ける動作制御部と、を更に備える請求項１７に記載の３次元位置計測システム。
19. コンピュータのプロセッサに、
    ワーク上に存在する互いの位置関係が既知の３つ以上の検出対象を視覚センサにより撮像した画像に基づき検出する手順と、
    検出された検出対象の中から、検出対象を選択した組み合わせを複数生成する手順と、
    生成された前記複数の組み合わせのそれぞれについて計算した、前記検出対象の検出位置の理想位置からの位置ずれを表す指標に基づいて、前記複数の組み合わせから１以上の組み合わせを選択する手順と、
    選択された前記１以上の組み合わせから前記ワークの３次元位置を決定する手順と、
    を実行させるためのプログラム。

Images