JP6338421B2 - 情報処理装置、情報処理装置の制御方法、把持システムおよびプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理装置の制御方法、把持システムおよびプログラムに関するものである。

撮像対象物の光沢性が高く、撮像対象物の有する面同士が凹角を成すように配置されている場合、撮像対象物の間で複数回の光の反射、すなわち、多重反射が発生しやすくなる。多重反射は撮像対象物の間で相互に発生するために相互反射とも呼ばれる。多重反射が発生すると撮像対象物中に向かい合わせの物体が写り込む。写り込みが生じた状態で通常の画像処理を適用すると、写り込み部分が他領域の影響を受けてしまう。

特に、撮像対象物までの距離を測定する、１次元のライン光投影や２次元パターン光投影を用いたアクティブ型の距離計測装置では写り込みによる影響が大きい。

アクティブ型の距離計測装置では、照明装置から投影光が出射する方向、投影光が撮像対象物で反射され撮像装置へ入射する方向に基づいて三角測量の原理で距離計測を行う。

撮像対象物の表面散乱性が高い場合、撮像対象物から撮像装置に入射する光の多くは１次反射光となるため、三角測量の原理による距離計測が有効に機能する。しかし、前述した多重反射による写り込みが発生すると、投影光の出射方向を誤認識することになり、計測距離に誤りが生じノイズとなる。

特許文献１では、多重反射によるノイズの発生の仕方の特性を利用してノイズを除去する方法が開示されている。当該方法では、着目している計測点と隣り合う計測点との間を結ぶベクトルの方向と、距離計測装置に正対する面とのなす角が大きい場合にノイズと判定して除去している。これは、多重反射によるノイズが距離計測装置に正対する面に対して急峻な特性になりがちであるという特性を利用したものである。

特許文献２では、偶数回の反射により像が反転する特性を利用してノイズを除去する方法が開示されている。当該方法では、投影装置から投影されるパターン光の撮像画像上での空間コードの増減方向を予め定めておき、定められたコードの増減方向とは逆向きの場合に多重反射であると判定している。これは、偶数回の反射により像が反転する特性と、多重反射光の支配的な成分が２次反射光成分であることを利用したものである。

特開２００９−１４５１７５号公報 特許第３８００８４２号公報

しかしながら、特許文献１に記載の方法では、多重反射によるノイズ点を有効に除去できる一方で、物体と物体の段差付近でも着目している計測点と隣り合う距離点との関係が急峻となるため、ノイズでない計測点がノイズであると誤判定されて除去されてしまうことがある。

同様に、特許文献２に記載の方法では、偶数回の多重反射によるノイズ点を良好に除去できる一方で、物体と物体の段差付近でも空間コードの増減が逆向きになることがあるため、その付近の計測点が除去されてしまい、段差付近の計測点情報の欠如が精度に影響を及ぼすことがある。

本発明は、上記の課題に鑑みてなされたものであり、距離計測値を利用する情報処理における多重反射によるノイズの影響、および、ノイズ除去処理による副作用の影響の両方を低減する技術を提供する。

上記の目的を達成する本発明に係る情報処理装置は、
パターン光が投影された領域を撮像して前記領域内の複数の計測点までの距離を示す複数の距離計測値を取得する距離計測手段と、
前記取得された複数の距離計測値のそれぞれが前記パターン光の多重反射によるノイズであるか否かを判定するノイズ判定手段と、
前記距離計測手段により取得された複数の距離計測値から前記ノイズと判定された距離計測値を除去した距離計測値のデータを使用して、計測点の有無に基づいて対象物の存在の有無を判定する処理である第１の処理を実施する第１の処理手段と、
前記距離計測手段により取得された複数の距離計測値を使用して、前記対象物の計測点と前記対象物のモデル情報とのフィッティング処理である第２の処理を実施する第２の処理手段と、
を備えることを特徴とする。

本発明によれば、距離計測値を利用する情報処理における多重反射によるノイズの影響、および、ノイズ除去処理による副作用の影響の両方を低減することができる。

実施形態１に係る情報処理装置を含むロボットシステムの概略構成図。 空間符号化法の投影パターン（４ビット）とグレイコードの説明図。 三角測量に基づく距離計測原理の説明図。 多重反射によるノイズ発生のメカニズムの説明図。 隣接計測点とのベクトル方向による多重反射ノイズの判定原理の説明図。 隣接計測点とのベクトル方向による多重反射ノイズの判定の副作用についての説明図。 パレット空判定処理の説明図。 ＣＡＤデータモデルフィッティング処理の説明図。 実施形態１の処理フローの説明図。 実施形態２に係る情報処理部の概略構成図。 干渉判定処理の説明図。 実施形態２の処理フローの説明図。

以下、添付図面を参照し、本発明の好適な実施形態について説明する。なお、以下説明する実施形態は、本発明を具体的に実施した場合の一例を示すもので、特許請求の範囲に記載の構成の具体的な実施例の１つである。

（実施形態１）
本実施形態では、距離計測値から多重反射ノイズを除去して後段の処理を行う場合と、距離計測値から多重反射ノイズを除去せずに後段の処理を行う場合とを、後段の処理の種別に応じて切り替えることで、多重反射によるノイズの影響およびノイズ除去処理による副作用の影響をともに低減する例を説明する。後段の処理の種別としては、ノイズの除去を行う第１の処理（ノイズの影響に敏感である、対象物の存在を判定する処理）、ノイズの除去を行わない第２の処理（相対的にノイズの影響に敏感ではないモデルフィッティング処理）について説明を行う。

先ず、本実施形態に係る情報処理装置１の機能構成例について、図１のブロック図を用いて説明する。情報処理装置１は、計測対象物５および当該計測対象物５が格納されたパレット５１までの距離を計測する距離計測装置１１と、距離計測装置１１による距離値を利用した情報処理を行う情報処理部１２とを有する。情報処理部１２は、計測対象物５を把持するロボット６と接続されており、情報処理の結果に基づき、ロボット６を制御することが可能である。例えば計測対象物５の距離値とモデルデータとに基づいて、計測対象物５の位置姿勢を認識し、ロボット６を制御して計測対象物５をピッキングする把持システムに応用することが可能である。

距離計測装置１１は、計測対象物５にパターン光を投影する投影部３０と、パターン光が投影された計測対象物５を撮像する撮像部２０と、投影部３０及び撮像部２０を制御し、空間符号化法に基づいて計測対象物５に対する距離計測を行う画像処理部４０とを有する。

先ず、投影部３０について説明する。投影部３０は、図１に示すように、光源３１と、照明光学系３２と、表示素子３３と、投影絞り３４と、投影光学系３５とを備える。

光源３１は、例えばハロゲンランプ、ＬＥＤ（Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）などの各種の発光素子である。照明光学系３２は、光源３１から照射された光を表示素子３３へと導く機能を持つ光学系であり、例えば、ケーラー照明や拡散板などの輝度の均一化に適した光学系が用いられる。表示素子３３は、供給されたパターンに応じて、照明光学系３２からの光の透過率、または、反射率を空間的に制御する機能を有する素子であり、例えば、透過型ＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）、反射型ＬＣＯＳ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ ｏｎ Ｓｉｌｉｃｏｎ）、ＤＭＤ（Ｄｉｇｉｔａｌ Ｍｉｃｒｏｍｉｒｒｏｒ Ｄｅｖｉｃｅ）などが用いられる。表示素子３３へのパターンの供給については、投影部３０内に保持されている複数種のパターンを順次表示素子３３に供給しても良いし、外部装置（例えば画像処理部４０）に保持されている複数種のパターンを順次取得して表示素子３３に供給しても良い。投影光学系３５は、表示素子３３から導かれた光（パターン光）を計測対象物５の特定位置に結像させるように構成された光学系である。投影絞り３４は、投影光学系３５のＦナンバーを制御するために用いられる。Ｆナンバーとは、レンズの焦点距離を有効口径で割った値であり、レンズの明るさを示す指標である。F値が小さいほどレンズは明るく（＝レンズを通る光量が多く）、シャッター速度を速くできる。

次に、撮像部２０について説明する。撮像部２０は、図１に示すように、撮像光学系２３と、撮像絞り２２と、撮像素子２１と、を備える。撮像光学系２３は、計測対象物５の特定位置を撮像素子２１上に結像させるよう構成された光学系である。撮像絞り２２は、撮像光学系２３のＦナンバーを制御するために用いられる。撮像素子２１は、例えばＣＭＯＳ（Cｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ-Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）センサ、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）センサなどの各種の光電変換素子である。なお、撮像素子２１で光電変換されたアナログ信号は、撮像部２０内の不図示の制御部により標本化ならびに量子化され、デジタル画像信号に変換される。更にこの制御部は、このデジタル画像信号から、各画素が輝度階調値（濃度値、画素値）を有する画像（撮像画像）を生成し、この撮像画像を適宜撮像部２０内のメモリや画像処理部４０に対して送出する。

次に、画像処理部４０について説明する。図１に示すように、画像処理部４０は、投影撮像制御部４１と、デコード部４２と、距離計測部４３と、多重反射ノイズ判定部４４とを備える。

投影撮像制御部４１は、投影部３０によるパターン光の種別を切り替える度に計測対象物５を撮像するように撮像部２０を制御する。すなわち、撮像部２０と投影部３０とが同期して動作するように制御し、結果として撮像部２０は、それぞれ異なるパターン光が投影された計測対象物５の画像を撮像することができる。投影撮像制御部４１はロボット６からの指示に基づき動作を開始する。

デコード部４２は、撮像部２０が撮像した複数種類のパターン光の撮像画像に基づいて撮像画像の各位置における空間コードをデコードする。

距離計測部４３は、デコード部４２によりデコードされた空間コードと、撮像画像における位置とを用いて、三角測量の原理で、撮像部２０（撮像光学系２３）から計測対象物５までの距離を計測する。

多重反射ノイズ判定部４４は、距離計測部４３により算出された距離値に基づいて多重反射ノイズを判定する。

情報処理部１２は、距離計測装置１１から出力された距離情報を用いて情報処理を行う。情報処理部１２は、空判定部１２１と、位置姿勢取得部１２２とを備えている。空判定部１２１は、パレット５１に計測対象物５が残存しているか否かを判定する。位置姿勢取得部１２２は、計測対象物５の距離点群の情報と計測対象物５のＣＡＤモデル情報とに基づいて、計測対象物５の位置姿勢を算出する。

実施形態１では、投影部３０により例えば図２（ａ）〜図２（ｆ）に示す空間符号化パターン画像を投影する。これらは４ビットの空間符号化法における投影パターンである。図２（ａ）は全点灯パターン、図２（ｂ）は全消灯パターンである。また、図２（ｃ）は１ビットポジティブパターン、図２（ｄ）は１ビットポジティブパターン、図２（ｅ）は２ビットポジティブパターン、図２（ｅ）は３ビットポジティブパターン、そして図２（ｆ）は４ビットポジティブパターンである。図２（ｃ）〜図２（ｆ）の各ビットの空間符号化パターンを撮像した画像で２値化判定を行う。２値化判定とは具体的には明るいパターンの部分を１、暗いパターンの部分を０と判定する。２値化判定の閾値としては例えば図２（ａ）の全点灯パターンの撮像画像と、図２（ｂ）の全消灯パターンの撮像画像との画像輝度値の平均値を用いることができる。

２値化判定が実施されると図２（ｇ）に示すように、撮像画像の各画素に対してグレイコードが求められる。図２（ｇ）は４ビットのグレイコードを示す。グレイコードが求められると、グレイコードから空間コードに一意に変換することができる。

図３は、三角測量に基づく距離計測原理の説明図である。Ｏｐが投影部主点位置、Ｏｃが撮像部主点位置を表している。投影部３０の画像座標ｘｐにおける空間コードＳＣｐが一意に決まると、投影部主点位置Ｏｐと空間コードＳＣｐとを結ぶ直線Ｌｐを引くことができるので出射方向θｐが一意に定まる。同様に撮像部２０の画像座標ｘｃにおける空間コードの位置ＳＣｃが一意に定まると、撮像部主点位置Ｏｃと空間コードの画素位置ＳＣｃとを結ぶ直線Ｌｃを引くことができるので入射方向θｃも一意に定まる。２つの直線ＬｐとＬｃとの交点が計測点ＭＰとなる。投影部３０と撮像部２０の主点間の距離である基線長ＢＬを予め把握しておくことで、三角測量の原理により対象物の距離Ｚｔを算出することができる。

次に、図４（ａ）および図４（ｂ）を参照して、多重反射によりノイズが発生するメカニズムを説明する。図４（ａ）は多重反射による影響が小さい場合の説明図であり、図４（ｂ）は多重反射による影響が大きい場合の説明図である。

図４（ａ）に示すように、多重反射による影響が少ない場合には、投影部３０（Ｏｐ）からの出射方向がＳＣｐａを通る光線Ｌｐａ、撮像部２０（Ｏｃ）への入射方向がＳＣｃを通る光線Ｌｃａとなるため、パレット５１の底面に存在する計測点ＭＰを正常に三角測量することが可能である。

これに対して図４（ｂ）に示すように、多重反射による影響が大きい場合には、投影部３０から出射方向ＳＣｐｂに出射した光はパレットの側面部Ｒｂで一度反射し、パレットの底面部ＭＰに入射する。撮像部２０から見て、直接光による光線Ｌｐａよりも多重反射光による光線Ｌｐｂ'の方が強度が高い場合、光がＬｐｂから来たものだと認識される。このとき、三角測量の原理により、ＬｐｂとＬｃｂの交点Ｎｂが計測点であるとして計算されるため、本来の計測点ＭＰに対してノイズ点（交点Ｎｂ）が発生することになる。

図４（ａ）および図４（ｂ）ではパレット側面部Ｒｂにおける多重反射の例を説明したが、計測対象物５が複雑に山積みされているケースでは計測対象物５の間でも多重反射は発生する。また、計測対象物５が複雑な形状、特に凹面を有するケースでは同一の計測対象物内でも多重反射が発生する。

以上が多重反射によりノイズが発生するメカニズムである。

次に、図５（ａ）−図５（ｃ）を参照して、隣接する計測点とのベクトル方向により多重反射ノイズの発生の有無を判定する原理について説明する。

図５（ａ）に示すように、多重反射が発生していないケースでは、隣接する計測点Ｍｐ１と計測点ＭＰ２は、パレット底面上に存在する。このとき、撮像部のＸ座標軸とパレット底面とは略平行な関係である。そのため、図５（ｂ）に示すように計測点ＭＰ１と計測点ＭＰ２とを結ぶベクトル方向と、撮像部のＸ座標軸とのなす角φａは小さい値になる。

一方、多重反射が発生するケースでは図５（ａ）に示すように隣接する計測点Ｎｂ１と計測点Ｎｂ２とはパレット底面上とは異なる位置に存在することになる。このとき、図５（ｃ）に示すように撮像部のＸ座標軸と、計測点Ｎｂ１と計測点Ｎｂ２とを結ぶベクトル方向のなす角φｂは大きな値となる。

すなわち、多重反射による計測点か否かは、隣接する計測点とのベクトルと撮像部のＸ座標軸とのなす角の大小により判定できる。図５（ａ）−図５（ｃ）では簡単化のためにＸ軸とＺ軸の２次元で表現したが、実際には３次元空間なので、隣接する計測点によるベクトルとＸＹ平面（基準平面）とのなす角を求める。なす角が閾値よりも大きい場合、多重反射によるノイズ点であると判定する。経験的にはなす角の閾値として６０°程度に設定すると好適である。本実施形態では、カメラ座標系におけるＸＹ平面とのなす角と閾値とを比較を例に説明したが、ＸＹ平面ではなくパレットの底面とのなす角と閾値とを比較して判定してもよい。

次に図６を参照して、隣接計測点とのベクトル方向による多重反射ノイズ判定の副作用について説明する。図６（ａ）がノイズ除去前の計測点群を表している。図６（ｂ）が隣接計測点とのベクトル方向により多重反射ノイズを除去した距離点群を表している。図６（ａ）のＮｂ１４で囲った距離点群は、パレット側面部の多重反射によるノイズ点である。多重反射によるノイズ点の隣接点とのベクトル方向はＸ軸とのなす角が大きいため、ノイズ点として除去される。一方で、物体と物体との段差の部分においても、隣接する計測点とのベクトル方向とＸ軸とのなす角が大きくなる。そのため、多重反射除去処理を適用すると段差付近においても計測点が除去されてしまう。

パレットの内部に塊として計測点が存在するか否かを判定する空判定部１２１においては、段差付近の計測点がノイズとして除去されても影響は軽微である。一方で、計測対象物５の距離点群の情報と、計測対象物５のＣＡＤモデル情報とに基づいて計測対象物５の位置姿勢を算出する対象物位置姿勢算出処理においては、段差付近のデータの影響で推定精度が低下する可能性がある。

パレット空判定処理とは、計測対象物を格納するパレット（箱）の内部に計測対象物が残存しているのか、それとも残存していない（以降、空と表現）のかを判定する処理である。パレットが空である場合には、情報処理装置１のユーザにパレットが空であることを通知し、パレットの交換、あるいは、パレットへの計測対象物の補充を促す。

ここで図７（ａ）−図７（ｆ）は、計測対象物の存在を判定するパレット空判定処理の説明図である。図７（ａ）はパレット内が空の状態であり、多重反射ノイズがない状態を表している。図７（ｂ）はパレット内に計測対象物が存在する状態であり、多重反射ノイズがない状態を表している。

パレット空判定処理ではパレットの物体が置かれる面Ｐｂａｓｅと計測点との距離Ｄｉｓを求め、その距離の総和が閾値を超える場合に計測対象物が存在すると判定する。距離の総和が閾値を超えない場合には、パレットが空であると判定する。図７（ａ）のように空の状態においては計測点とＰｂａｓｅとの距離Ｄｉｓはほぼ０となる。図７（ｂ）のように計測対象物が存在する場合には、計測対象物が置かれている領域において距離Ｄｉｓの値は大きくなる。パレットが空、または、計測対象物が存在すると判定するＤｉｓの総和の閾値は計測対象物の大きさに関する情報に基づいて決定すればよい。例えば体積の情報を用いることができる。閾値が適切に設定されている場合、図７（ａ）においてはＤｉｓの総和が閾値を超えることがなく、空であると判定できる。一方、図７（ｂ）の場合には、Ｄｉｓの総和が閾値を超えるので、計測対象物が存在すると判定される。

次に図７（ｃ）及び図７（ｄ）を参照して、多重反射ノイズが存在する場合の影響を説明する。図７（ｃ）はパレット内が空の状態であるが、多重反射ノイズ点Ｎｂ１４が存在する状態を表している。この場合には、計測対象物が存在しないにも関わらず、多重反射ノイズ点Ｎｂ１４とＰｂａｓｅとの間に距離が存在するため、距離がＤｉｓであると判定される。Ｄｉｓの総和が閾値を超える場合には、計測対象物が存在しないにも関わらず、計測対象物が存在すると判定してしまうことになる。図７（ｄ）は計測対象物と多重反射ノイズ点Ｎｂ１４とが存在する状態を表している。この場合には、計測対象物自体も存在するので、Ｄｉｓの総和が大きくなるだけで判定を誤ることはない。

そして、図７（ｅ）は図７（ｃ）で示した多重反射ノイズ点Ｎｂ１４を多重反射ノイズ除去処理により除去した結果を表わしている。Ｎｂ１４が除去されているため、計測点とＰｂａｓｅとの間で発生する距離Ｄｉｓの総和が閾値を超えることはなく、適切に空であると判定することができる。

さらに、図７（ｆ）は図７（ｄ）で示した多重反射ノイズ点Ｎｂ１４を多重反射ノイズ除去処理により除去したものである。さらに計測対象物内部の段差や計測対象物とパレット底面との段差においても計測点が除外される。計測点とＰｂａｓｅとの間の距離Ｄｉｓの総和は一部の計測点が除外されても大きくは変化しない。以上、説明したようにパレットの内部に塊として計測点が存在するか否かを判定するパレット空判定処理においては、段差付近の計測点がノイズとして除去されても影響は軽微である。従って、多重反射ノイズ除去処理による効果が大きいため、積極的に適用すべきであるといえる。

次にＣＡＤデータを用いたモデルフィッティング処理について説明する。この処理は計測対象物の計測点と計測対象物のＣＡＤデータとをフィッティング処理し、計測対象物の位置姿勢を把握することが目的である。計測対象物の位置姿勢を精度良く推定することで、ロボットで計測対象物を把持する際の精度が向上し、ロボットによる後工程への受け渡しを高度に行うことができる。

図８（ａ）−図８（ｆ）を参照して、ＣＡＤデータを用いたモデルフィッティング処理について説明する。図８（ａ）はモデルデータＭＤを示す。説明の簡略化のために、２次元で説明する。モデルデータＭＤは長方形である。また図８（ｂ）は計測対象物の計測点ＭＰを示す。フィッティング処理の初期においては図８（ｃ）に示すように計測対象物の計測点ＭＰとモデルデータＭＤとを凡そ重ね合わせたところからスタートする。計測点ＭＰとモデルデータＭＤ上の点との誤差が最小になるようにモデルデータＭＤの位置と姿勢を変更していく。位置と姿勢を変更する方法としてはＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）アルゴリズムと呼ばれる手法が知られている。このケースでは、計測点とモデルデータＭＤとの対応付けになるため、ｐｏｉｎｔ−ｔｏ−ｐｌａｎｅと呼ばれるＩＣＰアルゴリズムが用いられる。ＩＣＰアルゴリズムは、（１）計測点とモデルデータＭＤ上の最近傍点との対応づけ、（２）計測点とモデルデータＭＤ上の点との誤差を最小化する姿勢変換の算出という２つのステップに分かれる。この２つのステップを繰り返し実施することで、計測点とモデルデータＭＤ上の点との誤差が段々と小さくなり、誤差が一定値未満になったら繰り返し処理を打ち切り、最終結果とする。ｐｏｉｎｔ−ｔｏ−ｐｌａｎｅのアルゴリズムでは着目計測点とその周囲の点から微小平面の近似を行う。着目計測点とモデルデータＭＤとの間で、微小平面の法線方向の距離を求めることで、最近傍点との対応づけを行う。最終結果の例を図８（ｄ）に示す。

図８（ｅ）は多重反射ノイズ除去処理により段差部分の計測点が除去された場合の図を示す。多重反射ノイズ除去処理によりモデルの端に対応する計測点が除去される。具体的には図８（ｂ）のＮＭＰ１、ＮＭＰ２がそれに相当する。モデルフィッティング処理の場合、モデルの姿勢を決定するためには、極力モデルの広い範囲の計測点があることが望ましい。それに対し、多重反射除去処理によって、図８（ｅ）に示すように境界領域の計測点もノイズとして除去してしまう場合があり、これによって姿勢精度の低下が起こりうる。精度の低下は、例えばモデルデータＭＤの位置姿勢の最終結果に位置ずれを起こすなどの現象として現れる。

一方で、図８（ｆ）は部品上に多重反射ノイズ点Ｎ１、Ｎ２が存在する場合の図を示す。モデルフィッティング処理で用いられるＩＣＰアルゴリズムでは、多くの点を用いて処理をする過程においてロバスト推定法によりアウトライヤ点を除くことが可能であるため、必ずしもその処理の前にノイズを除去する必要はない。

したがって、モデルフィッティング処理の入力としては、多重反射ノイズ除去処理を適用していない計測点を入力とした方が望ましい結果が得られるといえる。

次に図９のフローチャートを参照して、実施形態１に係る情報処理装置１及びロボット６を含む部品把持システムの処理手順を説明する。

Ｓ１０１において、投影撮像制御部４１は、ロボット６からの計測指示を受け付けて投影部３０を制御することで、計測対象物５に対してパターン投影を行う。投影パターンは図２で説明したグレイコードパターンが用いられる。

Ｓ１０２において、投影撮像制御部４１は、撮像部２０を制御してパターン投影と同期して撮像処理を実行させる。撮像処理が行われるとパターン撮像画像Ｄ１０が不図示のメモリに格納される。Ｓ１０１とＳ１０２との各処理は投影パターンの枚数分だけ繰り返し実行される。

Ｓ１０３において、デコード部４２は、複数枚のパターン撮像画像Ｄ１０に基づいてグレイコードのデコード処理を実行する。デコード処理の結果は空間コード値Ｄ１１として不図示のメモリに格納される。Ｓ１０４において、距離計測部４３は、空間コード値Ｄ１１に基づいて計測対象物５に対する距離計測処理を実行する。Ｓ１０５において、多重反射ノイズ判定部４４は、Ｓ１０４で計測された距離計測値に基づいて多重反射ノイズ判定処理を実行する。ここでは、判定された多重反射ノイズが距離計測値から除去されたノイズ除去距離計測値Ｄ１３（ノイズと判定された距離計測値除去後の距離計測値のデータ）が生成される。

Ｓ１０６において、空判定部１２１は、ノイズ除去距離計測値Ｄ１３（ノイズと判定された距離計測値除去後の距離計測値のデータ）に基づいてパレット空判定処理を実行する。

Ｓ１０７において、空判定部１２１は、パレットが空であるか否かを判定する。パレットが空であると判定された場合、Ｓ１１０に進む。一方、パレットが空でないと判定された場合、Ｓ１０８に進む。

Ｓ１０８において、位置姿勢取得部１２２は、モデルフィッティング処理を実施する。モデルフィッティング処理の入力データはＳ１０４で取得された距離計測値である。前述したようにモデルフィッティング処理においては、多重反射ノイズを除去していない距離点群を使用した方が推定精度が向上するので、入力としてＳ１０４で取得された距離計測値を用いる。モデルフィッティング処理の結果は計測対象物５の位置姿勢情報Ｄ１４である。位置はカメラ座標系におけるＸＹＺの座標であり、姿勢は計測対象物５の向きに関する情報であり、様々な表現方法が存在する。例えばオイラー角表現や四元数表現、回転軸回転角表現などを用いることができる。

Ｓ１０９において、位置姿勢取得部１２２は、計測対象物５の位置姿勢情報Ｄ１４をロボット６に通知する。ロボット６は、通知された位置姿勢情報Ｄ１４に基づいて把持動作を行う。

Ｓ１１０において、情報処理部１２は、パレット補充通知処理を実行し、パレットが空であることをユーザに通知する。通知方法としては情報処理装置１の不図示の表示端末に視覚的に情報表示することや、不図示の音声出力装置を使用して音声で通知する方法などが挙げられる。以上が実施形態１の処理フローである。

このように後段の処理に応じて、相互反射ノイズを除去した距離点群、除去していない距離点群を切り替えることで、両方の処理を適切に実行することができる。

なお、図９では距離計測値とノイズ除去距離計測値Ｄ１３（ノイズと判定された距離計測値除去後の距離計測値のデータ）とを別々に保持する例を説明した。しかし、ほぼ同サイズで情報量の大きい距離計測値を２つ保持することはメモリを必要以上に使用することとなる。そこで、距離計測値とそれぞれに対応したバイナリの値であるフラグ情報を用いる方法もある。フラグ情報には距離計測値が多重反射ノイズ判定処理において、多重反射ノイズと判定された場合には１を、そうでない場合には０を格納する。後段の処理であるパレット空判定処理においては、フラグ情報の値を参照し、フラグが０である距離点の情報のみを処理に用いる。フラグ情報を活用することで、距離情報を２重に管理するよりもメモリの使用量を削減することができる。

以上説明したように、本発明によれば、多重反射ノイズが影響を及ぼす処理にはノイズ除去処理を実施するため、多重反射による精度低下が低減される。 一方で、ノイズ除去処理による副作用であるジャンプエッジ近傍の距離点群の消失が影響を及ぼす処理にはノイズ除去処理を適用しないことで精度の低下を防止することができる。

そのため、距離計測値を利用する情報処理における多重反射によるノイズの影響、および、ノイズ除去処理による副作用の影響の両方を低減することができる。

（実施形態２）
本実施形態では、距離計測値から多重反射ノイズを除去して後段の処理を行う場合と、距離計測値から多重反射ノイズを除去せずに後段の処理を行う場合とを、後段の処理の種別に応じて切り替えることで、多重反射によるノイズの影響およびノイズ除去処理による副作用の影響をともに低減する別の例を説明する。後段の処理の種別としては、ノイズの除去を行う第１の処理（ノイズの影響に敏感である、距離計測点と対象物を把持するロボットとの干渉状態を判定する処理）、ノイズの除去を行わない第２の処理（相対的にノイズの影響に敏感ではないモデルフィッティング処理）について説明を行う。

実施形態２に係る情報処理装置１の機能構成例は、実施形態１で用いた図１のブロック図と同様である。実施形態２と実施形態との違いは、情報処理部１２が含む機能ブロックである。実施形態１では、情報処理部１２は、空判定部１２１と、位置姿勢取得部１２２とを備えていた。これに対して実施形態２では、図１０に示すように、情報処理部１２は、空判定部１２１の代わりに、干渉判定部１２３を備えている。

干渉判定部１２３は、位置姿勢取得部１２２により位置・姿勢が推定された計測対象物５を、ロボット６が有するロボットハンドで周りの計測対象物５や、パレットに衝突することなく、把持または保持することができるか否かを判定する干渉判定処理を実行する。より具体的には、距離計測結果に基づき計測対象物５を把持するロボット６と、計測点との干渉を判定する処理を実行する。最初の候補で把持できない場合には、別の候補を位置姿勢取得部１２２に要求する。

ロボットハンドとしては、たとえば、ワークの平面部に押しつけることで保持を行う磁石式あるいは吸着式ハンドでもよい。また、二指、三指といった多数の指の開閉により物体を内側あるいは外側から挟み込むことで把持を行うグリッパー式ハンドでもよい。その他、ロボットアームに取り付け可能で把持機構を有するエンドエフェクタであればいかなるものであってもよい。本実施形態では、図１１のようにグリッパー式のロボットハンドを用いて説明する。

ここで図１１（ａ）−図１１（ｆ）を参照して干渉判定処理の詳細について説明する。それぞれパレット５１に複数の計測対象物５が山積みにされている。複数の計測対象物５のうち、位置姿勢取得部１２２により把持候補計測対象物５ｇの位置・姿勢が推定されているものとする。ロボット６のロボットハンドＲｈにより、把持候補計測対象物５ｇを把持することとなる。このとき、ロボットハンドＲｈの形状データは干渉判定部１２３に予め登録されているものとする。また、把持候補計測対象物５ｇのどの部位を、どの方向からロボットハンドＲｈで把持するのかという把持情報も事前に登録されているものとする。ロボットハンドＲｈの形状データ、および、把持情報を用いることで、把持候補計測対象物５ｇの位置姿勢が推定されれば、ロボットハンドＲｈと把持候補計測対象物５ｇとの位置関係が一意に決定される。

干渉判定処理においては、把持候補計測対象物５ｇを把持した状態のロボットハンドＲｈの形状データを、計測距離値データの中に配置する。形状データはボリュームデータ化されて扱われる。ロボットハンドＲｈのボリュームデータの内部に把持候補計測対象物５ｇ上の計測点を除く計測点が存在するか否かが判定される。ボリュームデータの内部に計測点が存在する場合、干渉ありと判定する。一方、ボリュームデータの内部に計測点が存在しない場合、干渉なしと判定する。

図１１（ａ）は干渉がなく、多重反射ノイズが存在しないケースである。ロボットハンドＲｈのボリュームデータの内部に計測点は存在しないので、正しく干渉なしと判定される。図１１（ｂ）は干渉があり、多重反射ノイズが存在しないケースである。ロボットハンドＲｈのボリュームデータの内部に計測点ＭＰｉが存在するので、正しく干渉ありと判定される。

図１１（ｃ）は干渉がなく、多重反射ノイズが存在するケースである。正しくは干渉していないが、ロボットハンドＲｈのボリュームデータの内部に多重反射ノイズＮｂｉが存在するため、誤って干渉ありと判定されてしまう。図１１（ｄ）は干渉があり、多重反射ノイズが存在するケースである。実際には多重反射ノイズが発生しやすい箇所に計測対象物５が配置されているため、多重反射ノイズは観測されない。ロボットハンドＲｈのボリュームデータの内部に計測点Ｍｐｉが存在するので、干渉ありと正しく判定される。

図１１（ｅ）は干渉がなく、多重反射ノイズ除去処理が実施されたケースである。図１１（ｃ）において発生していた多重反射ノイズが多重反射ノイズ除去処理により、取り除かれている。そのため、ロボットハンドＲｈのボリュームデータの内部に計測点は存在しないので、正しく干渉なしと判定される。図１１（ｆ）は干渉があり、多重反射ノイズ除去処理が実施されたケースである。図１１（ｄ）の計測点に対して、段差部分の計測点が除去されている。しかし、段差部分の計測点は一部分に過ぎず、ほとんどの計測点は残存するので、ロボットハンドＲｈのボリュームデータ内部に計測点Ｍｐｉが存在する。したがって、正しく干渉ありと判定される。

以上説明したように干渉判定処理においては多重反射ノイズ除去処理を適用することで、干渉ありと誤って判定するケースを除外することができ、その効果は高い。

次に、図１２のフローチャートを参照して、実施形態２に係る情報処理装置１及びロボット６を含む部品把持システムの処理手順を説明する。図９に示した実施形態１の処理と同一の番号が付与されたステップで行う処理の内容は実施形態１と同一であるため説明を省略する。

Ｓ２０１において、干渉判定部１２３は、干渉判定処理を行う。干渉判定処理の入力はＳ１０５において生成されたノイズ除去距離計測値Ｄ１３（ノイズと判定された距離計測値除去後の距離計測値のデータ）である。前述したように、多重反射ノイズが除去された距離計測値を使用することで、正しく干渉判定を行うことができる。

Ｓ２０２において、干渉判定部１２３は、干渉の有無を判定する。干渉ありと判定された場合には、Ｓ２０３に進む。一方、干渉なしと判定された場合には、Ｓ１０９に進む。

Ｓ２０３において、位置姿勢取得部１２２は、次候補選定処理を実施する。具体的には、モデルフィッティング処理によって位置・姿勢が推定された計測対象物５が干渉ありと判定されたので、次の候補を選定する。その後、Ｓ１０８に戻る。

以上が実施形態２の処理フローである。

以上説明したように、本発明によれば、多重反射ノイズが影響を及ぼす処理にはノイズ除去処理を実施するため、多重反射による精度低下が低減される。 一方で、ノイズ除去処理による副作用であるジャンプエッジ近傍の距離点群の消失が影響を及ぼす処理にはノイズ除去処理を適用しないことで精度の低下を防止することができる。

[変形例]
なお、処理フローの中では割愛したが、Ｓ２０３における次候補選定処理で、次の候補が存在しない場合には、ユーザに把持可能な候補が存在しないことを通知してもよい。

ユーザは計測対象物５の山積み状態を崩すなどの対策が必要となるが、対策後に自動でのピッキング動作を再開することができる。

また、投影部３０として表示素子と投影光学系とを含むプロジェクタを用いた２次元のパターン投影法で距離計測を行う例について説明した。しかし、本発明が適用可能な範囲は上記に限られるものではない。例えば１次元のライン光を走査して計測対象物の距離計測を行う光切断法にも適用できる。

情報処理部１２が実施する、多重反射ノイズを除去しない計測点を利用する処理（第２の処理）としてＣＡＤデータを用いたモデルフィッティング処理を説明したが、本発明が適用できる範囲はこの処理に限定されるものではない。他の適用例としては点群データの関数近似処理と、実測した点群テンプレートを用いたモデルフィッティング処理とが挙げられる。

まず、点群データの関数近似処理について説明する。点群データを近似する関数としては、平面を例に説明する。平面近似処理では、Ｎ点ある点群データの計測値（Ｘｉ、Ｙｉ、Ｚｉ）（ｉ＝１〜Ｎ）に基づいて、平面の式ａＸ＋ｂＹ＋ｃＺ＋ｄ＝０のａ、ｂ、ｃ、ｄの４つのパラメータを推定する。１〜Ｎ点ある計測値に基づいて連立方程式を立て、ａ、ｂ、ｃ、ｄの最小二乗解を求めるのが一般的である。このとき、近似平面からの誤差が大きい点を除外して近似平面を求め直すことで、多重反射ノイズの影響を低減することができる。なお、ここでは平面を例に説明したが、本発明を適用するのが効果的な関数は平面の式に限られるものではなく、例えば球面の方程式（Ｘ−ａ）^２＋（Ｙ−ｂ）^２＋（Ｚ−ｃ）^２＝ｒ^２への関数近似処理などにも適用できる。球面の方程式の場合、球中心座標（ａ，ｂ，ｃ）、球の半径ｒが推定するパラメータである。

次に、実測した点群テンプレートを用いたモデルフィッティング処理について説明する。実測した点群テンプレートとは、例えば予め対象物の全周を三次元計測し、つなぎ合わせて作製されたものである。点群テンプレートを用いたモデルフィッティング処理の場合、点群テンプレートの点と、距離計測装置による計測点とのＩＣＰアルゴリズムによる位置・姿勢算出が行われる。このケースでは、ｐｏｉｎｔ−ｔｏ−ｐｏｉｎｔと呼ばれるＩＣＰアルゴリズムが用いられる。具体的には、対応付けの工程において、各計測点と最近傍の点群テンプレート上の点を探索により求める。続いて、計測点と最近傍点との距離を小さくする位置姿勢変換を求める。対応付けと位置姿勢変換の算出を繰り返し実施することで、最終結果に収束する。

同様に、情報処理部１２が実施する、多重反射ノイズを除去した計測点を利用する処理（第１の処理）として、パレット空判定処理と干渉判定処理について説明したが、本発明が適用できる範囲はこの処理に限定されるものではない。他の適用例としては、未知なシーンからの形状取得が挙げられる。

未知なシーンからの形状取得では、対象物を様々な方向から三次元計測し、全周の三次元点群を得る。その結果をつなぎ合わせることで、対象物全体の三次元形状データが得られる。このような例では対象物の正確な三次元形状データを取得することが求められるので、多重反射ノイズを除去した計測点を入力した方がよい。

一般に局所的なデータを用いて判定をする処理においては、点の存在有無が判定結果を大きく左右するので、多重反射ノイズを除した計測点を利用した方がよい。比較して広い範囲のデータを用いて判定する処理では、多くの点を利用できることからロバスト推定などの統計的な判断基準を利用できるため、多重反射ノイズを除去しない計測点を利用した方がよい。

さらには、多重反射ノイズの除去処理として、隣接する計測点とのベクトル方向を利用する方法を説明したが、本発明が適用できる多重反射ノイズ除去処理はこれに限られるものではない。例えば、特許文献２に示されるような空間コードの増減方向を利用した多重反射ノイズ除去処理にも適用できる。

（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。

１：情報処理装置、１１：距離計測装置、１２：情報処理部、２０：撮像部、３０：投影部、４０：画像処理部、４３：距離計測部、４４：多重反射ノイズ判定部、５：計測対象物、１２１：空判定部、１２２：位置姿勢取得部、１２３：干渉判定部

Claims (11)

1. パターン光が投影された領域を撮像して前記領域内の複数の計測点までの距離を示す複数の距離計測値を取得する距離計測手段と、
   前記取得された複数の距離計測値のそれぞれが前記パターン光の多重反射によるノイズであるか否かを判定するノイズ判定手段と、
   前記距離計測手段により取得された複数の距離計測値から前記ノイズと判定された距離計測値を除去した距離計測値のデータを使用して、計測点の有無に基づいて対象物の存在の有無を判定する処理である第１の処理を実施する第１の処理手段と、
   前記距離計測手段により取得された複数の距離計測値を使用して、前記対象物の計測点と前記対象物のモデル情報とのフィッティング処理である第２の処理を実施する第２の処理手段と、
   を備えることを特徴とする情報処理装置。
2. 前記ノイズ判定手段は、第１の計測点の距離計測値と、前記第１の計測点と隣接する第２の計測点の距離計測値とに基づいて、前記第１の計測点の距離計測値が前記多重反射によるノイズであるか否かを判定することを特徴とする請求項１に記載の情報処理装置。
3. 前記ノイズ判定手段は、前記第１の計測点の距離計測値と、前記第２の計測点の距離計測値とを結ぶベクトルの傾きが閾値よりも大きい場合に、前記第１の計測点の距離計測値を前記多重反射によるノイズと判定することを特徴とする請求項２に記載の情報処理装置。
4. 前記第１の処理は前記第２の処理よりもノイズの影響を受ける処理であることを特徴とする請求項１乃至３の何れか１項に記載の情報処理装置。
5. 前記第１の処理は、前記第２の処理よりも局所的な計測点を使用する処理であることを特徴とする請求項１乃至４の何れか１項に記載の情報処理装置。
6. パターン光が投影された領域を撮像して前記領域内の複数の計測点までの距離を示す複数の距離計測値を取得する距離計測手段と、
   前記取得された複数の距離計測値のそれぞれが前記パターン光の多重反射によるノイズであるか否かを判定するノイズ判定手段と、
   前記距離計測手段により取得された複数の距離計測値から前記ノイズと判定された距離計測値を除去した距離計測値のデータを使用して、前記距離計測手段による計測結果に基づき対象物を把持または保持するロボットと、計測点との干渉を判定する処理を実施する第１の処理手段と、
   前記距離計測手段により取得された複数の距離計測値を使用して、前記対象物の計測点と前記対象物のモデル情報とのフィッティング処理である第２の処理を実施する第２の処理手段と、
   を備えることを特徴とする情報処理装置。
7. 前記対象物にパターン光を投影する投影手段と、
   前記パターン光が投影された前記対象物を撮像する撮像手段と
   をさらに備えることを特徴とする請求項１乃至６の何れか１項に記載の情報処理装置。
8. 請求項１乃至７の何れか１項に記載の情報処理装置と、
   前記対象物を保持または把持する手段と
   を備えることを特徴とする把持システム。
9. 情報処理装置の制御方法であって、
   距離計測手段が、パターン光が投影された領域を撮像して前記領域内の複数の計測点までの距離示す複数の距離計測値を取得する工程と、
   ノイズ判定手段が、前記取得された複数の距離計測値のそれぞれが前記パターン光の多重反射によるノイズであるか否かを判定する工程と、
   第１の処理手段が、前記取得された複数の距離計測値から前記ノイズと判定された距離計測値を除去した距離計測値のデータを使用して、計測点の有無に基づいて対象物の存在の有無を判定する処理である第１の処理を実施する工程と、
   第２の処理手段が、前記取得された複数の距離計測値を使用して、前記対象物の計測点と前記対象物のモデル情報とのフィッティング処理である第２の処理を実施する工程と、
   を有することを特徴とする情報処理装置の制御方法。
10. 情報処理装置の制御方法であって、
    距離計測手段が、パターン光が投影された領域を撮像して前記領域内の複数の点までの距離を示す複数の距離計測値を取得する工程と、
    ノイズ判定手段が、前記取得された複数の距離計測値のそれぞれが前記パターン光の多重反射によるノイズであるか否かを判定する工程と、
    第１の処理手段が、前記取得された複数の距離計測値から前記ノイズと判定された距離計測値を除去した距離計測値のデータを使用して、前記距離計測手段による計測結果に基づき対象物を把持または保持するロボットと、計測点との干渉を判定する処理である第１の処理を実施する工程と、
    第２の処理手段が、前記取得された複数の距離計測値を使用して、前記対象物の計測点と前記対象物のモデル情報とのフィッティング処理である第２の処理を実施する工程と、
    を有することを特徴とする情報処理装置の制御方法。
11. 請求項９又は１０に記載の情報処理装置の制御方法の各工程をコンピュータに実行させるためのプログラム。

Images