JP2015212644A - 情報処理装置およびその方法、計測装置、並びに、作業装置 - Google Patents

Abstract

【課題】 移動中に対象物を撮影する状況においても、対象物の位置姿勢の計測を高精度に行う。【解決手段】 形状モデル入力部110は、計測対象の物体の形状モデル10を入力する。画像入力部120は、画像撮像装置20によって取得される物体40の画像を入力する。距離画像入力部130は、距離画像撮像装置30によって取得される物体40の表面の位置情報を示す距離情報を入力する。劣化度合い推定部150は、画像撮像装置20および距離画像撮像装置30と物体40の間の相対的な動きによって発生する、画像の情報劣化度合いおよび距離情報の情報劣化度合いを推定する。位置姿勢推定部160は、画像および/または距離情報、形状モデル10、並びに、情報劣化度合いに基づき、物体40の位置および/または姿勢を推定する。【選択図】 図1

Description

本発明は、物体の位置姿勢を計測する情報処理に関する。

ロボット技術の発展に伴い、工業製品の組立のような、従来人間が行っていた複雑なタスクをロボットが代行する場面が増えた。ロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。この組立において、把持対象の部品とロボット（ハンド）の間の相対的な位置姿勢を計測する必要がある。

位置姿勢の計測方法として、カメラが撮影した二次元画像から検出される特徴や、距離センサが計測した距離データに対して、物体の三次元形状モデルを当て嵌めるモデルフィッティングがある。

二次元画像に対するモデルフィッティングにおいては、物体の位置姿勢に基づき三次元形状モデルを画像上に投影した場合の投影像が、検出された特徴に当て嵌まるように位置姿勢を計測する。距離データに対するモデルフィッティングにおいては、距離データを表現する距離画像の各点を三次元座標をもつ三次元点群に変換し、三次元空間において三次元形状モデルが三次元点群に当て嵌まるように位置姿勢を計測する。

二次元画像から得られる計測情報と、距離データから得られる計測情報を併用すれば、物体の位置姿勢をより高精度に計測することができる。

特許文献1は、二次元画像から得られる計測情報を利用する位置姿勢の計測方法、距離データから得られる計測情報を利用する位置姿勢の計測方法、それら位置姿勢の計測方法を併用する方法の一つを、計測対象の物体（以下、対象物）の形状に応じて選択する。そして、選択した方法によって対象物の位置姿勢を計測する方法を記載する。特許文献1の方法によれば、対象物の形状に最適な方法により計測が行われるため、高精度かつ高速な計測が可能になる。

しかし、撮像装置が移動しながら対象物を撮影する場合、モーションブラーの影響により位置姿勢の推定精度が低下する。モーションブラーの影響は、形状情報のみでは推定することが困難である。このため、形状情報から観測情報を選択して位置姿勢を計測する特許文献1の方法は、撮像装置が移動しながら対象物を撮影する場合、対象物の位置姿勢を高精度に計測することができない。

特開2012-021958号公報

R. Y. Tsai「A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses」IEEE Journal of Robotics and Automation, vol. RA-3, No. 4, 1987 望月優介、青木公也「静止画像における空間不変でない焦点暈け強度推定手法の検討」ビジョン技術の実利用ワークショップ ViEW2012、IS1-C5、2012年

本発明は、移動中に対象物を撮影する状況においても、対象物の位置姿勢の計測を高精度に行うことを目的とする。

本発明は、前記の目的を達成する一手段として、以下の構成を備える。

本発明にかかる情報処理は、計測対象の物体の形状モデルを入力し、第一の撮像装置によって取得される前記物体の画像を入力し、第二の撮像装置によって取得される前記物体の表面の位置情報を示す距離情報を入力し、前記第一および第二の撮像装置と前記物体の間の相対的な動きによって発生する、前記画像の情報劣化度合いおよび前記距離情報の情報劣化度合いを推定し、前記画像および/または前記距離情報、前記形状モデル、並びに、前記情報劣化度合いに基づき、前記物体の位置および/または姿勢を推定する。

本発明によれば、移動中に対象物を撮影する状況においても、対象物の位置姿勢の計測を高精度に行うことができる。

実施例1の情報処理装置の構成例を示すブロック図。 三次元形状モデルを説明する図。 実施例1の情報処理装置による位置姿勢の推定処理を説明するフローチャート。 劣化度合いの推定処理を説明するフローチャート。 ヤコビアンを説明する図。 位置姿勢の推定処理を説明するフローチャート。 実施例2の情報処理装置の構成例を示すブロック図。 実施例2の情報処理装置による位置姿勢の推定処理を説明するフローチャート。 位置姿勢を推定する情報処理装置の適用例を説明する図。

以下、本発明にかかる実施例の計測および情報処理を図面を参照して詳細に説明する。

［位置姿勢の推定の概要］
実施例1では、計測対象の物体（以下、対象物）を模したモデルデータの形状、並びに、対象物の撮影における相対的な移動方向および速度に基づき、二次元画像および距離データから得られる各観測情報の劣化度合いを予測する。そして、劣化が少ない観測情報を重用して対象物の位置姿勢を推定する方法を説明する。なお、位置姿勢とは、対象物と、当該対象物を撮像する撮像装置の間の位置と姿勢の関係を意味する。

実施例1では、対象物または撮像装置が移動している状態で撮影を行う場合を想定する。移動中に撮影する（以下、移動中撮影）場合、撮影画像にモーションブラーが発生し、観測情報の座標にばらつきが生じるなどの情報劣化が発生し易い。観測情報の劣化は、位置姿勢の推定精度に大きく影響するため、情報劣化がある観測情報は位置姿勢の推定に利用しないことが望ましい。

移動中撮影における情報劣化の主たる原因であるモーションブラーは、対象物の形状および撮像装置と対象物の間の相対的な移動速度によって、その発生傾向が大きく異なる。

例えば、撮像装置により平面部分を備える対象物を撮影する場合、撮像装置が対象物の平面に沿って（平面法線方向に直交する方向に）移動すれば、露光時間における距離の変化が少なく、距離データの対象物の像におけるモーションブラーの発生量は少ない。他方、二次元画像においては対象物が画像上で大きく移動して撮影されるため、二次元画像の対象物の像におけるモーションブラーの発生量は大きい。

一方、撮像装置が対象物の平面法線方向に移動すれば、露光時間における距離の変化が大きく、距離データの対象物の像におけるモーションブラーの発生量は大きい。他方、二次元画像においては像の変化が少ないため、二次元画像の対象物の像におけるモーションブラーの発生量は少ない。

このように、対象物の形状と相対的な移動方向によってモーションブラーの影響が異なるように、二次元画像から得られる観測情報の劣化度合いと、距離データから得られる観測情報の劣化度合いは、対象物の形状と相対的な移動方向によって異なる特性がある。

そこで、対象物の形状と、撮像装置と対象物の間の相対的な位置姿勢速度の情報に基づき、二次元画像および距離データの観測情報それぞれの劣化度合いを予測して、劣化が少ない観測情報を重用して対象物の位置姿勢を推定する。つまり、モーションブラーに基づく各観測情報の劣化度合いを予測し、劣化度合いに基づく重み付けを各観測情報に行って位置姿勢を推定する。対象物の形状に加え、相対的な位置姿勢速度の情報を用いて劣化度合いを予測し、劣化が少ない観測情報を重用して位置姿勢を推定することで、形状情報のみで観測情報を選択する特許文献1の手法と比較して、高精度に物体の位置姿勢を推定することができる。

［装置の構成］
図1のブロック図により実施例1の情報処理装置100の構成例を示す。形状モデル入力部110は、対象物40の形状を表す三次元形状モデル10を入力し、三次元形状モデル10を劣化推定部150に出力する。なお、三次元形状モデル10は、情報処理装置100の内部または外部の記憶装置、あるいは、情報処理装置100が接続された有線または無線ネットワーク上のサーバ装置などに格納されている。

図2により三次元形状モデル10を説明する。図2(a)は物体形状の一例を示し、当該物体形状は、図2(b)に示す局所面特徴および図2(c)に示す局所線特徴によって構成される三次元形状モデル10によって表される。局所面特徴は、三次元位置と三次元法線方向から構成される物体表面上の局所的な三次元平面情報を示す。また、局所線特徴は、三次元位置と三次元線分方向から構成される物体輪郭上の局所的な三次元線分情報を示す。なお、単に「幾何特徴」と称した場合、局所面特徴と局所線特徴の両方を指すことにする。

三次元形状モデル10として保持される形状情報は、対象の物体形状を表す三次元的な幾何情報であればよく、表現形式に制限はない。例えば、単純な三次元点の集合や、稜線を表す三次元ラインの集合、三つの三次元点で定義される面および線の集合として表されるポリゴン形式の形状情報など、他の表現を用いてもよい。

画像入力部120は、画像撮像装置20が出力する二次元画像を入力する。画像撮像装置20は、通常の二次元画像を撮影するカメラである。画像撮像装置20が撮影する二次元画像は、濃淡画像でもよいし、カラー画像でもよい。以下では、画像撮像装置20が濃淡画像を撮影するとして説明を行う。画像撮像装置20の焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、画像撮像装置20として使用する機器の仕様を参照するか、非特許文献1が開示する方法によって事前にキャリブレーションしておく。

距離画像入力部130は、距離画像撮像装置30が出力する距離画像を入力する。距離画像撮像装置30として、対象物40の表面上の点の位置情報である三次元座標を計測して距離情報を示す距離画像を出力する距離センサが用いられる。距離画像は、各画素が奥行き情報を有する画像である。

距離センサとして、波長が異なる色IDを付与したマルチスリットラインを対象物40に照射し、その反射光をカメラで撮像して、三角測量によって距離計測を行うワンショットアクティブ式のセンサを利用することができる。しかし、距離センサは、これに限られるものではなく、光の飛行時間を利用するTime-of-flight方式のセンサを用いてもよい。あるいは、ステレオカメラが撮影した画像から三角測量によって各画素の奥行きを計算するパッシブ式のセンダでもよい。その他、距離画像を計測可能なセンサであれば、如何なるセンサも利用可能である。

画像撮像装置20の光軸と距離画像撮像装置30の光軸が一致し、二次元画像撮像装置20が出力する濃淡画像の各画素と、距離画像撮像装置30が出力する距離画像の各画素の対応関係が既知であると想定する。

ただし、本発明の適用は、濃淡画像と距離画像が同一視点である場合に限られるものではない。例えば、画像撮像装置20と距離画像撮像装置30が異なる位置姿勢にあり、濃淡画像と距離画像がそれぞれ別の視点から撮影されてもよい。その場合、画像撮像装置20と距離画像撮像装置30の間（以下、撮像装置間）の相対的な位置姿勢を既知として、距離画像中の三次元点群を濃淡画像に投影して、濃淡画像の各画素と距離画像の各画素の対応関係を取得する。言い替えれば、同一の対象物40を撮影可能な撮像装置間の相対的な位置姿勢が既知であり、それらの画像の間の対応関係が計算可能であれば、それら撮像装置の位置関係はとくに制限されない。

速度入力部140は、撮像装置（二次元画像撮像装置20および距離画像撮像装置30）と対象物40の間の相対的な位置姿勢の速度（以下、相対速度）をロボット60から入力する。相対速度は、三次元的な相対位置の速度と三次元的な姿勢の速度により構成される六自由度のパラメータにより表現される。なお、以下では、画像撮像装置20と距離画像撮像装置30を合わせた撮像装置を「撮像装置320」と称することにする。

ロボット60は、回転または並進移動軸からなる可動軸を複数有し、撮像装置320の位置姿勢を変更するための可動機器であり、アーム先端に撮像装置320を装着した、六軸の回転可動軸により構成される六自由度ロボットアームである。

撮像装置320は、アーム先端に装着されるものとする。アーム先端から撮像装置320までの位置姿勢は、撮像装置320のオフセット位置姿勢として予め取得し、以後は変化がないものとして保持する。つまり、撮像装置320の位置姿勢は、アーム先端の位置姿勢にオフセット位置姿勢を加えることで算出される。

撮像装置320と対象物40の相対的な位置姿勢が単位時間当りに移動する量を示す相対速度は、位置姿勢の取得時刻の間の撮像装置320の位置姿勢の差分から算出される。例えば、撮像装置320の相対速度は、ロボット60のモーション情報から取得することができる。つまり、ロボット60の制御モジュールは、アーム先端に装着された撮像装置320の位置姿勢における三次元的な速度を計算し、相対速度を示す情報を速度入力部140に入力する。

ロボット60として用いる機器は、上記に限られるものではく、例えば、七軸回転軸を有する垂直多関節型ロボットや、スカラ型ロボット、パラレルリンクロボットでもよい。その他、回転または並進移動軸からなる可動軸を複数有し、モーション情報が取得できれば、如何なる形式のロボットでもよい。

相対速度の取得はロボット60のモーション情報を用いる方法に限られない。例えば、撮像装置320に磁気センサや超音波センサなど六自由度の位置および姿勢を計測する物理センサを取り付け、それらセンサの出力から相対速度を算出してもよい。また、撮像の間における撮像装置320の移動が回転運動に限定することができれば、撮像装置320にジャイロセンサを取り付け、その回転速度を計測してもよい。また、撮像装置320が固定され、対象物40が動く場合、対象物40に上述したセンサを取り付け、センサが出力する情報から相対速度を算出してもよい。言い替えれば、撮像装置320と対象物40の相対速度が算出できれば、如何なる方法を用いてもよく、手段や機器の選択に制限はない。

劣化度合い推定部150は、詳細は後述するが、三次元形状モデル入力部110から入力される三次元形状モデル10および速度入力部140から入力される相対速度から、濃淡画像の観測情報および距離画像の観測情報の情報劣化度合いを推定する。本実施例では、観測情報の劣化度合いとして、各画像のモーションブラー量を推定する。つまり、濃淡画像のエッジが露光時間に画像面を移動した量と、距離画像の三次元点群が測定時間に三次元空間を移動した量をモーションブラー量として推定する。つまり、モーションブラー量は、露光（測定）時間に、各観測情報が移動した量を表す。

観測情報の劣化度合いを示す情報は、モーションブラー量に限られるものではなく、撮像装置320と対象物40の相対的な移動に基づいて観測情報が劣化する量を表すことができる情報であればよい。例えば、距離画像の観測情報の劣化度合いを、三次元座標ではなく、距離計測用パターン光の投影画像中のブラー量として表現してもよい。また、濃淡画像の観測情報の劣化度合いを、濃淡画像のエッジを三次元空間に逆投影して算出した、三次元空間における移動量として表現してもよい。

位置姿勢推定部160は、詳細は後述するが、推定されたモーションブラー量、濃淡画像の観測情報および距離画像の観測情報、並びに、三次元形状モデル10に基づき、撮像装置320と対象物40の間の位置および姿勢を推定する。

なお、情報処理装置100の各構成および後述する処理や機能は、コンピュータ機器である情報処理装置100のCPUが、RAMをワークメモリとして、ROMなどの不揮発性メモリなどの記録媒体に格納されたプログラムを実行することにより実現される。

［位置姿勢の推定処理］
図3のフローチャートにより実施例1の情報処理装置100による位置姿勢の推定処理を説明する。

例えば、図示しない操作パネルを介して位置姿勢の推定の開始が指示されると、情報処理装置100は初期化処理を行う(S11)。初期化処理において、撮像装置320に対する対象物40の位置および姿勢の概略値（以下、概略位置姿勢）が入力される。例えば、対象物40が配置された凡その位置や姿勢が予め分かっているとして、その値が概略値として入力される。

概略位置姿勢の設定方法は上記に限られず、例えば、情報処理装置100が時間軸方向に連続した計測を行うものとして、前回の計測値を概略位置姿勢として用いてもよい。また、過去の位置および姿勢の計測に基づき、対象物40の速度や角速度を時系列フィルタによって推定し、過去の位置および姿勢と推定された速度や加速度から予測される現在の位置および姿勢を概略位置姿勢としてもよい。あるいは、様々な姿勢で撮影した対象物40の画像をテンプレートとして保持し、入力される濃淡画像とテンプレートマッチングにより、対象物40の凡その位置と姿勢を推定し、その推定値を概略位置姿勢としてもよい。

勿論、他のセンサによる対象物40の位置および姿勢の計測が可能な場合は、当該センサの出力値を概略位置姿勢に用いてもよい。センサは、例えばトランスミッタが発する磁界を対象物40に装着されたレシーバが検出して位置および姿勢を計測する磁気式センサや、対象物40上に配置したマーカをシーンに固定されたカメラが撮影して位置および姿勢を計測する光学式センサでもよい。その他、六自由度の位置および姿勢を計測するセンサであれば如何なるセンサでも利用することができる。

次に、画像入力部120は、対象物40の濃淡画像を取得する(S12)。つまり、画像撮像装置20から濃淡画像と、その撮影における露光時間Tiを取得する。

次に、距離画像入力部130は、対象物40の距離画像を取得する(S13)。つまり、距離画像撮像装置30から距離画像と、その撮影（測定）時の測定時間Trを取得する。なお、距離画像には、距離画像撮像装置30から対象物40の表面までの距離が記録されている。前述したように、画像撮像装置20と距離画像撮像装置30の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。

次に、速度入力部140は、相対速度を取得する(S14)。相対速度は、三次元位置の速度情報(Vx, Vy, Vz)および三次元姿勢の速度情報(Vwx, Vwy, Vwz)から構成される六次元ベクトル↑Vとして取得される。
↑V = [Vx Vy Vz Vwx Vwy Vwz]^T … (1)
ここで、↑はベクトルを表す、
Tは転置行列を表す。

相対速度の取得方法は、上述の方法に限られるものではない。例えば、ロボット60の制御モジュールからアーム先端の位置姿勢を取得し、アーム先端の位置姿勢に撮像装置320の位置姿勢オフセットを加えて、所定の時刻の間における撮像装置320の位置姿勢の差分から相対速度を算出してもよい。撮像装置320の六自由度の位置姿勢の速度を取得することができる方法であれば、計算方法およびセンサの選択に制限はない。

次に、劣化度合い推定部150は、取得された相対速度に基づき、濃淡画像の観測情報および距離画像の観測情報の劣化度合いとしてモーションブラー量を推定する(S15)。詳細は後述するが、観測情報に対応付けられる三次元形状モデル10の局所線特徴および局所面特徴ごとにモーションブラー量が推定される。

次に、位置姿勢推定部160は、詳細は後述するが、濃淡画像および距離画像と、三次元形状モデル10の間の対応を算出し、モーションブラー量の推定値に基づき対象物40の位置および姿勢を推定する(S16)。

●劣化度合い推定部
図4のフローチャートにより劣化度合いの推定処理(S15)を説明する。

劣化度合い推定部150は、三次元形状モデル10の局所線特徴のヤコビアンを算出する(S151)。局所線特徴のヤコビアンは、位置姿勢六自由度のパラメータが僅かに変化した際、局所線特徴と画像エッジの間の画像上の距離が変化する割合を表す値である。

図5によりヤコビアンを説明する。図5(a)は、局所線特徴と濃淡画像の画像特徴の間の直線-点間距離の計算を説明する図である。対象物40の概略位置姿勢sに基づき、局所線特徴を画像に投影した場合、局所線特徴と対応する画像特徴の間の符号付距離（以下、第一の対応間距離）err_2Dは下式よって算出される。
err_2D = Nu(u' - u) + Nv(v' - v) … (2)
ここで、(u, v)は局所線特徴を投影位置、
(Nu, Nv)は投影位置の法線方向（単位ベクトル）、
(u', v')は局所線特徴に対応する画像特徴の座標。

対象物40の概略位置姿勢sは六次元ベクトルであり、対象物40の位置を表す三つの要素(s1, s2, s3)と姿勢を表す三つの要素(s4, s5, s6)を有する。姿勢を表す三つの要素は、例えばオイラー角による表現や、方向が原点を通る回転軸を表してノルムが回転角を表す三次元ベクトルなどによって表現される。概略位置姿勢sの各パラメータで、第一の対応間距離err_2Dを偏微分して局所線特徴のヤコビアン行列J_2Dが算出される。
J_2D = [∂err_2D/∂s1 ∂err_2D/∂s2 ∂err_2D/∂s3 ∂err_2D/∂s4 ∂err_2D/∂s5 ∂err_2D/∂s6] …(3)

偏微分の結果として画像特徴の座標(u', v')の項が消え、ヤコビアン行列は、局所線特徴の情報と概略位置姿勢sのみから算出することが可能である。以上の手順を、各局所線特徴に対して行い、局所線特徴ごとのヤコビアンが計算される。

次に、劣化度合い推定部150は、局所線特徴のヤコビアンと相対速度に基づき、局所線特徴のモーションブラー量を算出する(S152)。濃淡画像の露光時間Tiに、撮像装置320が相対速度↑Vで動くことにより発生する、局所線特徴と濃淡画像のエッジの間の距離変化Veは下式により算出される。
Ve = Ti・J_2D・↑V … (4)

距離変化Veはスカラ量であり、露光時間Tiに、画像平面に投影した局所線特徴の二次元位置が移動する量を表す。局所線特徴のモーションブラー量と、局所線特徴に対応する濃淡画像の観測情報のモーションブラー量が同一と仮定すると、距離変化Veは、局所線特徴と対応する濃淡画像の観測情報のモーションブラー量と見做すことができる。以上の処理をすべての局所線特徴に対して行い、すべての局所線特徴についてモーションブラー量を算出する。

次に、劣化度合い推定部150は、三次元形状モデル10の局所面特徴のヤコビアンを算出する(S153)。局所面特徴のヤコビアンは、位置姿勢六自由度のパラメータが僅かに変化した際、局所面特徴と距離点の間の三次元空間の距離が変化する割合を表す値である。

図5(b)は、局所面特徴と対応点（距離画像の三次元点）の間の面-点間距離の計算を説明する図である。対象物40の概略位置姿勢sに基づき、局所面特徴のカメラ座標系に投影した場合の局所面特徴と対応する距離画像の三次元点の間の三次元空間の符号付距離（第二の対応間距離）err_3Dは下式よって算出される。
err_3D = Nx(x' - x) + Ny(y' - y) + Nz(z' - z) … (5)
ここで、(x, y, z)は局所面特徴の投影座標、
(Nx, Ny, Nz)は投影座標の法線方向（単位ベクトル）、
(x', y', z')は局所面特徴に対応する距離画像の三次元点の座標。

そして、局所線特徴のヤコビアンと同様に、仮想カメラの概略位置姿勢sの各パラメータで、第二の対応間距離err_3Dを偏微分して局所面特徴のヤコビアン行列J_3Dが算出される。
J_3D = [∂err_3D/∂s1 ∂err_3D/∂s2 ∂err_3D/∂s3 ∂err_3D/∂s4 ∂err_3D/∂s5 ∂err_3D/∂s6] …(6)

偏微分の結果として距離画像の三次元点の座標(x', y', z')の項が消え、ヤコビアン行列は、局所面特徴の情報と概略位置姿勢sのみから算出することが可能である。以上の手順を、各局所面特徴に対して行い、局所面特徴ごとのヤコビアンが計算される。

次に、劣化度合い推定部150は、局所面特徴のヤコビアンと相対速度に基づき、局所面特徴のモーションブラー量を算出する(S154)。距離画像の測定時間Trに、撮像装置320が相対速度↑Vで動くことによって発生する、局所面特徴と距離画像の距離点の間の距離変化Vrは下式により算出される。
Vr = Tr・J_3D・↑V … (7)

距離変化Vrはスカラ量であり、測定時間Trに、三次元空間において局所面特徴が移動する量を表す。局所面特徴のモーションブラー量と、局所面特徴に対応する距離画像の観測情報のモーションブラー量が同一と仮定すると、距離変化Vrは、局所面特徴と対応する距離画像の観測情報のモーションブラー量と見做すことができる。以上の処理をすべての局所線特徴に対して行い、すべての局所特徴についてモーションブラー量を算出する。

●位置姿勢推定部
図6のフローチャートにより位置姿勢の推定処理(S16)を説明する。

位置姿勢推定部160は、対象物40の概略位置姿勢sに基づき、濃淡画像のエッジと三次元形状モデル10の局所線特徴の間の対応付けを行う(S161)。概略位置姿勢sと校正済みの画像撮像装置30の内部パラメータを用いて、局所線特徴を濃淡画像へ投影し、局所線特徴の投影位置を算出し、濃淡画像のエッジと局所線特徴を対応付ける。各局所線特徴に対応して複数のエッジが検出された場合、画像上で、当該局所線特徴に最も近いエッジが対応付けの対象である。

次に、位置姿勢推定部160は、概略位置姿勢sに基づき、距離画像の三次元点群と三次元形状モデル10の局所面特徴の間の対応付けを行う(S162)。概略位置姿勢sと校正済みの距離画像撮像装置30の内部パラメータを用いて、各局所面特徴を距離画像へ投影し、各局所面特徴に対応する距離画像の距離点を、当該局所特徴面に対応する三次元点として対応付ける。

次に、位置姿勢推定部160は、各局所線特徴に対応する濃淡画像のエッジを示す対応データと、各局所面特徴に対応する三次元点を示す対応データに基づき、対象物40の位置および姿勢を推定する。その際、位置姿勢推定部160は、対応データに基づき、計測データと三次元形状モデル10の間の誤差を最小にする概略位置姿勢sの補正値Δsを算出する。以下では、補正値Δsの算出について詳細に説明する。

濃淡画像のエッジと対応付けられた局所線特徴の数をi-1として、各局所線特徴とエッジの対応間距離をerr_2Di、そのヤコビアンをJ_2Diとする。一方、距離画像中の距離点と対応付けられた局所面特徴の数をj-1として、各局所面特徴と距離点の対応間距離をerr_3Dj、そのヤコビアンをJ_3Djとする。各対応間距離（誤差）を最小化する概略位置姿勢sの補正値Δsは下の線形連立方程式によって表すことができる。
┌ ┐┌ ┐ ┌ ┐
│J_2D0││Δs1│ │err_2D0│
│ ： ││Δs2│ │ ： │
│J_2Di││Δs3│ │err_2Di│
│J_3D0││Δs4│=│err_3D0│ … (8)
│ ： ││Δs5│ │ ： │
│J_3Dj││Δs6│ │err_3Dj│
└ ┘└ ┘ └ ┘

ここで、ヤコビアンをJ、対応間距離（誤差）をEとすると、式(8)を下式で表すことができる。
JΔs = E … (9)

各対応データにモーションブラー量が大きい対応データが存在すると、補正値Δsの精度が低下する。そこで、位置姿勢推定部160は、局所線特徴ごと、局所面特徴ごとに計算されたモーションブラー量に基づき、各対応データの重みを算出する(S163)。言い替えれば、モーションブラー量が小さい対応データを重用するように、線形連立方程式を組み立て、モーションブラー量が大きい対応データには小さな重みを与え、モーションブラー量が小さい対応データには大きな重みを与える。モーションブラー量をvとする場合、重みw(v)は、例えば下式に示すようなTukeyの関数により与えられる。
if (v ≦ c)
w(v) = {1 + (v/c)²}²；
else
w(v) = 0； … (10)
ここで、cは定数。

重みを与える関数は、Tukeyの関数である必要はなく、例えばHuberの関数など、モーションブラー量vが大きい場合は小さな重みを与え、モーションブラー量vが小さい場合は大きな重みを与える関数であればよい。また、定数cは、位置姿勢の推定に有効なモーションブラー量vを規定する閾値であり、位置姿勢の推定において許容する精度を基に設定してもよいし、モーションブラー量vの全体の最大値から決定してもよい。定数cの設定方法に制限はなく、モーションブラー量vに基づいて重みを設定することができればよい。

局所線特徴の対応データに与える重みをw_2Di、局所面特徴の対応データに与える重みをw_3Djとして、重み行列Wを下式のように定義する。
┌ ┐
│w_2D0 0│
│ ： │
W = │ w_2Di │
│ w_3D0 │ … (11)
│ ： │
│0 w_3Dj│
└ ┘

重み行列Wは、対角成分以外はすべて0のLc×Lc正方行列であり、対角成分は重みw_2Di、w_3Djである。この重み行列Wを用いて、式(9)を下式のように変形する。
WJΔs = WE … (12)

位置姿勢推定部160は、次式に示す一般化逆行列を用いて式(12)を解き、補正値Δsを算出する(S164)。
Δs = (J^TWJ)^-1J^TWE … (13)

位置姿勢推定部160は、式(13)によって得られる補正値Δsを用いて、対象物40の概略位置姿勢sを更新する（s+Δs）(S165)。ここでは、非線形最適化手法としてGauss-Newton法を用いる例を説明した。しかし、非線形最適化手法は、これに限られるものではなく、Newton-Raphson法、Levenberg-Marquardt法、最急降下法、共役勾配法などのその他の非線形最適化手法を用いてもよい。

次に、位置姿勢推定部160は、ステップS165で更新した位置姿勢が収束しているか否か、言い替えれば、反復計算が必要か否かを判定する(S166)。つまり、補正値Δsがほぼ0の場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ0の場合は収束と判定する。また、未収束の場合は、処理をステップS161に戻し、更新後の概略位置姿勢sを用いて、再度、位置姿勢の算出処理を行う。

更新した位置姿勢が収束していると判定すると、位置姿勢推定部160は、当該位置姿勢を撮像装置320と対象物40の間の相対的な位置姿勢の最終的な推定値として決定し(S167)、位置姿勢の推定処理を終了する

このように、対象物40と撮像装置320の間の相対速度と対象物40の形状に基づき、濃淡画像および距離画像の観測情報ごとの劣化度合いを判定し、劣化度合いが小さい観測情報が重用されるように重みを設定して位置姿勢を推定する。従って、移動中撮像によりモーションブラーが発生して観測情報の劣化が発生している場合も、位置姿勢を高精度に推定することができる。

以下、本発明にかかる実施例2の計測および情報処理を説明する。なお、実施例2において、実施例1と略同様の構成については、同一符号を付して、その詳細説明を省略する。

実施例1では、対象物40と撮像装置320の相対速度、および、対象物40の形状に基づき、濃淡画像および距離画像の観測情報の劣化度合いとしてモーションブラー量を予測する方法を説明した。実施例2では、相対速度を事前知識として用いずに、取得した濃淡画像および距離画像の観測情報像からモーションブラー量を推定する方法を説明する。

［装置の構成］
図7のブロック図により実施例2の情報処理装置100の構成例を示す。図1に示す構成と異なるのは、速度入力部140を備えないことと、画像入力部120と距離画像入力部130の出力先として劣化度合い推定部150が追加されている点である。

実施例2において、画像入力部120における露光時間の取得および距離画像入力部130における測定時間の取得は必須項目ではない。ただし、距離画像取得部130は、距離画像に加えて、距離画像の算出元である、マルチスリットラインを対象物40に照射して撮影された測定用のパターン光画像を距離画像撮像装置30から取得する。

劣化度合い推定部150は、撮像装置320により撮影された濃淡画像および距離画像から、濃淡画像および距離画像の観測情報の劣化度合いを推定する。詳細は後述するが、劣化度合いとして、モーションブラーを含む観測情報の暈けの程度が推定される。

［位置姿勢の推定処理］
図8のフローチャートにより実施例2の情報処理装置100による位置姿勢の推定処理を説明する。ステップS11からS13の処理は実施例1と略同様であり、詳細説明を省略する。ただし、前述したように、露光時間の取得と測定時間の取得は行われず、距離画像に加えてパターン光画像が取得される。

次に、劣化度合い推定部150は、濃淡画像およびパターン光画像を用いて、濃淡画像および距離画像の観測情報の劣化度合いとしてモーションブラーを含む暈け量を算出する(S24)。つまり、濃淡画像およびパターン光画像それぞれの画素ごとに暈け量を算出し、濃淡画像の暈け量を示す第一の暈け量画像と、距離画像に関する暈け量を示す第二の暈け量画像を生成する。第一および第二の暈け量画像は同一サイズであり、画素ごとの対応が取れているものとする。

画像から画素ごとの暈け算出して暈け量画像を生成する手法は、例えば、非特許文献2に開示された暈けの推定手法を用いればよい。劣化度合い推定部150は、非特許文献2に開示された暈けの推定手法を濃淡画像およびパターン光画像それぞれに適用し、第一および第二の暈け量画像を生成する。

なお、単一画像から暈け量を推定する手法は、非特許文献2に開示された手法に限られず、画像から画素ごとに暈け量を推定することができる方法であれば如何なる手法を用いてもよい。

また、パターン光画像から暈け量を推定せずに、距離画像から直接暈け量を推定し、第二の暈け量画像を生成してもよい。例えば、パターン光画像に暈けが発生すると、そこから算出される距離値は、近傍で同じ平面上にある点の距離値でもばらつきが大きくなることが知られている。そこで、距離画像の画素ごとに近傍領域におけるばらつき量を算出し、ばらつき量を暈け量と見做して、第二の暈け量画像を生成すればよい。

次に、位置姿勢推定部160は、濃淡画像と局所線特徴の間の対応データ、および、距離画像と局所面特徴の間の対応データを生成し、ステップS24で算出された暈け量に基づき、対象物40の位置および姿勢を算出する(S25)。ステップS25の位置姿勢の推定処理は、実施例1のステップS16の処理と略同一であり、ステップS16の処理と異なる点についてのみ説明する。

実施例2では、局所線特徴および局所面特徴ごとのモーションブラー量の算出を行わない。そこで、第一の暈け量画像を参照して濃淡画像の観測情報ごとの暈け量を設定し、第二の暈け量画像を参照して距離画像の観測情報ごとの暈け量を設定して、観測情報ごとの暈け量に基づき各対応データの重みを算出する（図6のS163）。そして、対応データと重みを用いて補正値Δsを算出する（図6のS164）。

このように、濃淡画像およびパターン光画像（または距離画像）から、濃淡画像および距離画像の観測情報の劣化度合いとして暈け量を算出し、暈け量が小さい観測情報が重用されるように重みを設定して位置姿勢を推定する。従って、移動中撮像によりモーションブラーが発生して観測情報の劣化が発生している場合も、位置姿勢を高精度に推定することができる。

以下、本発明にかかる実施例3の計測および情報処理を説明する。なお、実施例3において、実施例1、2と略同様の構成については、同一符号を付して、その詳細説明を省略する。

位置姿勢を推定する情報処理装置100の好適な適用事例として、以下の形態が考えられる。つまり、画像撮像装置20および距離画像撮像装置30により得られる濃淡画像と距離画像を基に対象物40の位置姿勢を推定し、推定結果に基づき産業用ロボットアームによって対象物40の把持などの作業を行う形態である。

図9により位置姿勢を推定する情報処理装置100の適用例を説明する。図9は、実施例1または実施例2の情報処理装置100とロボット60を用いて対象物40の位置および/または姿勢を変更するなどの操作を行う作業装置であるロボットシステムの構成例を示す。

ロボット60は、ロボットコントローラにより制御され、指令された位置に手先を移動させ物体の把持など操作を行う。対象物40は、作業台に載置される位置が変化するため、現在の対象物40の位置姿勢を推定し、ロボット60の把持制御などを行う必要がある。

画像撮像装置20は、通常の二次元画像を撮影するカメラ、距離画像撮像装置30は対象物40の表面の距離を計測する距離センサであり、対象物40の撮影が可能な位置、例えばロボットアームの先端部などに設置されている。

情報処理装置100は、画像撮像装置20および距離画像撮像装置30から得られる濃淡画像と距離画像に基づき対象物40の位置姿勢を推定する。推定された対象物40の位置姿勢は、ロボット60に入力され、対象物40の把持などを行うようにロボットアームが制御される。情報処理装置100による対象物40の位置姿勢の推定により、対象物40の位置が不定でも、ロボット60による対象物40の把持などの作業が可能になる。

［変形例］
上記の実施例においては、観測情報ごとに劣化度合いに基づく重みを設定して位置姿勢を推定する例を説明した。しかし、重みの設定方法は、上記の実施例において説明した方法に限られるものではない。

例えば、濃淡画像の観測情報と距離画像の観測情報のどちらを重視するかという、濃淡画像と距離画像ごとに利用率（重み）を設定して位置姿勢を推定してもよい。この場合、濃淡画像全体で一つの情報劣化度合いを算出し、同様に、距離画像全体で一つの情報劣化度合いを算出する。画像全体の情報劣化度合いの算出方法は、例えば、観測情報ごとの劣化度合いを算出し、それらの平均値、中央値、最大値、最小値などの統計値を画像全体の情報劣化度合いとすればよい。そして、算出した濃淡画像全体の劣化度合いと距離画像全体の劣化度合いから利用率を算出して位置姿勢を推定する。

利用率は、濃淡画像全体の劣化度合いと距離画像全体の劣化度合いの和を求め、各劣化度合いを和で除算する。そして、式(11)におけるw_2Di、w_3Djに利用率を設定して位置姿勢を推定する。その際、一方の劣化度合いが著しく大きく、利用率が0になる場合、一方の観測情報は利用されずに位置姿勢が推定される。

また、画像全体の情報劣化度合いを算出するのではなく、画像の領域ごとに情報劣化度合いを算出して位置姿勢を推定してもよい。つまり、位置姿勢を推定する際に、劣化度合いが大きい観測情報の重みを小さくすることができれば、情報劣化度合いの算出方法や重みの設定方法に制限はない。

また、上記では、三次元形状モデル10として、局所面特徴と局所線特徴により構成されたモデル例に説明した。三次元形状モデルとして他の表現方式を用いてもよい。例えば、三点と三辺および一面により構成されるポリゴンの集合、単純な三次元点の集合、陰関数の組み合わせによりパラメトリックに三次元形状を表現する方法などを用いることもできる。

また、パターン光の投影にはプロジェクタや液晶プロジェクタを用いればよいが、これらに限らず、パターン光の投影ができる方式の装置であればよい。例えば、DMD (digital mirror device)やLCOS (liquid crystal on silicon)を用いるプロジェクタでもよい。

また、上記では、最適化計算ベースの位置姿勢の推定を説明したが、他の方法を用いることもできる。例えば、概略位置姿勢を中心とする所定の範囲において、六自由度の値が網羅的にカバーされるように多数の位置姿勢を生成し、各位置姿勢において観測できる幾何特徴と濃淡画像および距離画像のマッチングを評価して、位置姿勢を推定してもよい。

また、上記では、濃淡画像と距離画像を同時に利用して位置姿勢を推定する例を説明した。しかし、濃淡画像のみ、距離画像のみを用いて位置姿勢を推定する場合も、本発明を適用することができる。

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

110 … 形状モデル入力部、120 … 画像入力部、130 … 距離画像入力部、150 … 劣化度合い推定部、160 … 位置姿勢推定部

Claims (14)

1. 計測対象の物体の形状モデルを入力する第一の入力手段と、
   第一の撮像装置によって取得される前記物体の画像を入力する第二の入力手段と、
   第二の撮像装置によって取得される前記物体の表面の位置情報を示す距離情報を入力する第三の入力手段と、
   前記第一および第二の撮像装置と前記物体の間の相対的な動きによって発生する、前記画像の情報劣化度合いおよび前記距離情報の情報劣化度合いを推定する第一の推定手段と、
   前記画像および/または前記距離情報、前記形状モデル、並びに、前記情報劣化度合いに基づき、前記物体の位置および/または姿勢を推定する第二の推定手段を有する情報処理装置。
2. さらに、前記相対的な動きの速度を入力する第四の入力手段を有し、
   前記第一の推定手段は、前記速度と前記形状モデルに基づき前記情報劣化度合いを推定する請求項1に記載された情報処理装置。
3. 前記第一の推定手段は、前記情報劣化度合いとしてモーションブラー量を推定する請求項2に記載された情報処理装置。
4. 前記第一の推定手段は、前記画像から当該画像の情報劣化度合いを推定し、前記距離情報から当該距離情報の情報劣化度合いを推定する請求項1に記載された情報処理装置。
5. 前記第三の入力手段は、さらに、前記距離画像の測定用のパターン光画像を前記第二の撮像装置から入力し、
   前記第一の推定手段は、前記画像から当該画像の情報劣化度合いを推定し、前記パターン光画像から前記距離画像の情報劣化度合いを推定する請求項1に記載された情報処理装置。
6. 前記第一の推定手段は、前記情報劣化度合いとして画像の暈け量を推定する請求項4または請求項5に記載された情報処理装置。
7. 前記第二の推定手段は、前記情報劣化度合いに基づき、前記物体の位置および/または姿勢を推定する際の、前記画像に関する重み、および、前記距離画像に関する重みを設定する請求項1から請求項6の何れか一項に記載された情報処理装置。
8. 前記第二の推定手段は、前記形状モデルの局所線特徴と前記画像の画像特徴を対応付ける対応データ、および、前記形状モデルの局所面特徴と前記距離情報の位置情報を対応付ける対応データを生成し、前記情報劣化度合いに基づき各対応データの重みを設定し、前記対応データと前記重みを用いて前記物体の位置および/または姿勢を推定する請求項1から請求項6の何れか一項に記載された情報処理装置。
9. 物体を撮影する第一の撮像装置と、
   前記物体の表面の位置情報を示す距離情報を測定する第二の撮像装置と、
   請求項1から請求項8の何れか一項に記載された情報処理装置とを有する計測装置。
10. 請求項9に記載された計測装置と、
    前記計測装置による物体の位置および/または姿勢の推定結果に基づき、前記物体あるいは前記計測装置の第一および第二の撮像装置の位置および/または姿勢を変更する操作手段とを有する作業装置。
11. 前記操作手段は、回転および/または並進移動軸からなる可動軸を有するロボットである請求項10に記載された作業装置。
12. 計測対象の物体の形状モデルを入力し、
    第一の撮像装置によって取得される前記物体の画像を入力し、
    第二の撮像装置によって取得される前記物体の表面の位置情報を示す距離情報を入力し、
    前記第一および第二の撮像装置と前記物体の間の相対的な動きによって発生する、前記画像の情報劣化度合いおよび前記距離情報の情報劣化度合いを推定し、
    前記画像および/または前記距離情報、前記形状モデル、並びに、前記情報劣化度合いに基づき、前記物体の位置および/または姿勢を推定する情報処理方法。
13. コンピュータを請求項1から請求項8の何れか一項に記載された情報処理装置の各手段として機能させるためのプログラム。
14. 請求項13に記載されたプログラムが記録されたコンピュータが読み取り可能な記録媒体。

Images