JP6180086B2 - 情報処理装置及び情報処理方法 - Google Patents

Description

本発明は情報処理装置及び情報処理方法に関し、特に、３次元形状が既知である物体の位置及び姿勢を計測するために用いて好適な技術に関する。

近年、これまで人間が行っていた工業製品の組立のような複雑なタスクをロボットが代わりに行うようになりつつある。ロボットによる工業製品の組立では、ハンドなどのエンドエフェクタによって部品を把持するために、部品とロボット（ハンド）との間の相対的な位置及び姿勢を高精度に計測する必要がある。高精度に物体の位置姿勢を計測できる技術として、カメラから得られる２次元画像（濃淡画像・カラー画像）と距離センサから得られる距離画像を同時に利用して位置姿勢を推定する技術が、以下に示す文献で開示されている。

非特許文献１では、時系列的に入力される２次元画像と距離画像に対し、２次元画像上の特徴を奥行きが不定な３次元の点として扱う。そして、２次元画像上の特徴と３次元モデルとの誤差、距離画像中の３次元点と３次元モデルとの誤差が小さくなるように、カルマンフィルタを用いて物体の位置及び姿勢を更新する手法が開示されている。

特開２００５−２０１８２４号公報

Ｙ． Ｈｅｌ‐Ｏｒ ａｎｄ Ｍ． Ｗｅｒｍａｎ， "Ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｂｙ ｆｕｓｉｎｇ ｎｏｉｓｙ ｄａｔａ ｏｆ ｄｉｆｆｅｒｅｎｔ ｄｉｍｅｎｓｉｏｎｓ， " ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｖｏｌ．１７，ｎｏ．２， ｐｐ．１９５−２０１， １９９５．

従来手法である２次元画像と距離画像を併用する物体位置姿勢推定技術は、撮像装置が自由運動することを想定した手法であるため、時系列的に入力される計測データ間の相対幾何関係は未知であるものとして推定を行っていた。そのため、撮像時刻間に撮像装置が大幅に移動するような場合には、安定性が損なわれる、または、推定精度が低下するなどの問題が発生していた。

一方、ロボットアーム上に撮像装置を設置して当該装置を移動させながら物体を計測する場合。あるいは、ロボットアームが把持して移動している物体を固定した撮像装置で計測するような場合には、撮像時刻間における撮像装置または計測対象物体の移動を観測する情報として、ロボットのモーション情報を参照することが可能である。

ロボットによるモーション情報は、ロボットの制御における繰り返し精度に基づく誤差やキャリブレーション精度に基づく誤差などの誤差要因を含みながらも、位置姿勢推定に利用する情報として確度の高い有用な情報である。しかし、従来手法では、ロボットのモーション情報が得られる場合においても、ロボットモーションを観測情報として有効に利用することはできなかった。

また、単純にモーション情報を利用する方策としては、現状の概略位置姿勢に、モーション情報より得られた位置姿勢差分を加えるなどして位置姿勢を推定する技術として、例えば特許文献１にて提案されているモーションステレオと呼ばれる技術が公知としてある。

しかし、前述したように、ロボットモーションの情報も一定のあいまい性を含んだ情報であるため、そのあいまい性を考慮した上で、位置姿勢の更新を行えることが望ましい。本発明は前述の問題点に鑑み、計測装置と計測対象物体との間の位置または／及び姿勢の関係があいまい性を含んだ場合であっても、高精度に物体の位置姿勢を計測できるようにすることを目的とする。

本発明の情報処理装置は、可動軸を有するロボットに搭載された撮像装置を用いて被測定物体の位置姿勢を推定する情報処理装置であって、前記被測定物体の幾何的特徴を表す３次元形状モデルのデータを保存する３次元形状モデル保存手段と、前記被測定物体を撮像した画像を取得する画像入力手段と、前記撮像装置と前記被測定物体との間の相対的な概略位置姿勢を入力する概略位置姿勢入力手段と、前記概略位置姿勢をもとにして、前記画像に対して前記３次元形状モデルを当てはめることで、前記概略位置姿勢を更新する第１の位置姿勢更新手段と、前記ロボットの前記可動軸の情報に基づいて、前記撮像装置が前記被測定物体を撮像した時刻、または前回の位置姿勢差分情報の取得時刻以降に前記撮像装置が移動した位置姿勢差分量を取得する位置姿勢差分情報入力手段と、前記第１の位置姿勢更新手段による更新後の位置姿勢を、前記位置姿勢差分量に基づいて更新する第２の位置姿勢更新手段と、を備えることを特徴とする。

本発明によれば、２次元画像または距離画像の撮像間において計測対象物体または計測装置の移動に伴うロボットのモーション情報が得られる場合、２次元画像と距離画像に加え、撮像時刻間におけるロボットのモーション情報も観測情報として併用するようにした。これにより、高精度な対象物体の位置姿勢推定を行うことが可能となる。

本発明の実施形態における情報処理装置の構成を示す図である。 ロボットアームと撮像装置、計測対象部品との関係を説明する図である。 位置姿勢推定方法の処理手順を示すフローチャートである。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。
［第１の実施形態：ハンドアイロボット系による実施形態］
本実施形態では、本発明の情報処理装置を、図２（ａ）に示すような撮像装置が先端に装着されたロボットアームが移動しながら、固定された計測対象物体５０を撮像するシーンにおいて、物体位置姿勢推定手法に適用した場合について説明する。

本実施形態では、１回の計測は、濃淡画像撮像、距離画像撮像、ロボットモーションの取得という順番で行う。まず、画像（濃淡画像もしくは距離画像）の計測においては、取得される度に画像に対する３次元形状モデルの当てはめに基づく、計測対象物体（被測定物体）５０の位置姿勢更新を行う。

そして、ロボットアームが移動することによって発生する、撮像間における撮像装置の位置姿勢差分量をロボットアームのモーション情報から取得し、モーション情報に基づいた計測対象物体５０の位置姿勢更新を行う。濃淡画像・距離画像の情報に加え、ロボットのモーションに基づく情報も用いて、計測対象物体５０の位置姿勢の更新処理を行うことで、計測対象物体５０の位置姿勢の推定を高精度に行う。

図１は、本発明の実施形態に係わる情報処理装置１の構成の一例を示す図である。情報処理装置１は３次元形状モデル保存部１１０と、画像入力部１２０と、概略位置姿勢入力部１３０と、位置姿勢差分情報入力部１４０と、第１の位置姿勢更新部１５０、第２の位置姿勢更新部１６０とを具備して構成される。

また、３次元形状モデル保存部１１０は、計測対象物体の幾何的特徴を表す３次元形状モデルのデータを保存し、第１の位置姿勢更新部１５０に接続されている。２次元画像撮像装置２０は、画像入力部１２０に接続されている。距離画像撮像装置３０は、画像入力部１２０に接続されている。ロボット４０は、位置姿勢差分情報入力部１４０に接続されている。なお、本実施形態では、３次元形状モデル保存部１１０に保存された３次元形状モデルデータ１０が、現実に撮像される計測対象物体５０の形状に即していることが情報処理装置１を適用できる条件となる。

次に、情報処理装置１を構成する各部について説明する。
３次元形状モデルデータ１０（以下、３次元形状モデルと呼称する）は、計測対象となる物体（以下、計測対象物体５０、又は単に物体と呼ぶ場合もある）の形状を表す３次元幾何情報である。３次元形状モデル１０は、点の集合や各点を結んで構成される面の情報、面を構成する線分の情報によって定義される。３次元形状モデル１０は、３次元形状モデル保存部１１０に保存され、第１の位置姿勢更新部１５０に入力される。

２次元画像撮像装置２０は、通常の２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では２次元画像撮像装置２０は濃淡画像を出力する。２次元画像撮像装置２０が撮影する画像は画像入力部１２０を介して情報処理装置１に入力される。

カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、または、[Ｒ． Ｙ． Ｔｓａｉ， "Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ， " ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， ｖｏｌ．ＲＡ−３， ｎｏ．４， １９８７．]で開示される方法によって事前にキャリブレーションしておく。

距離画像撮像装置３０は、計測対象である物体表面上の点の３次元情報を計測する距離データ計測部として機能するものであり、本実施形態においては、距離画像撮像装置３０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとしては、波長の異なる色IDを付与したマルチスリットラインを対象に照射、その反射光をカメラで撮像して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。

しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

距離画像撮像装置３０が計測した距離画像は、画像入力部１２０を介して情報処理装置１に入力される。また、距離画像撮像装置３０と２次元画像撮像装置２０の光軸は一致しており、２次元画像撮像装置２０が出力する濃淡画像の各画素と、距離画像撮像装置３０が出力する距離画像の各画素の対応は既知であるとする。

ロボット４０は、回転または並進移動軸からなる可動軸を複数有し、２次元画像撮像装置２０及び距離画像撮像装置３０からなる撮像装置の位置姿勢を変更するための可動機器である。以下、２次元画像撮像装置２０と距離画像撮像装置３０を合わせた機器を撮像装置２３０と呼称する。本実施形態では、アーム先端に撮像装置を装着した、６軸の回転可動軸により構成される６自由度ロボットアームを用いる。

ロボット４０の６軸可動軸の情報は、位置姿勢差分情報入力部１４０を介して、情報処理装置１に入力される。アーム先端に装着された撮像装置２３０の、アーム先端から撮像装置２３０までの位置姿勢は、撮像装置２３０のオフセット位置姿勢としてあらかじめ算出しておき、以後変化しないものとして保持する。撮像装置２３０の位置姿勢は、アーム先端の位置姿勢からオフセット位置姿勢をかけることで算出できる。

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

概略位置姿勢入力部１３０は、撮像装置２３０に対する物体の位置及び姿勢の概略値を入力する。本実施形態では、情報処理装置１は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いる。しかしながら、位置及び姿勢の概略値の入力方法はこれに限るものではない。

例えば、様々な姿勢で撮像した対象物体の画像をテンプレートとして保持しておき、入力する画像に対してテンプレートマッチングを行うことによって、対象物体の大まかな位置と姿勢を推定してもよい。あるいは、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。

センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合にはその値を概略値として用いる。

位置姿勢差分情報入力部１４０は、２次元画像または距離画像の撮像時刻、または、ロボット４０からの位置姿勢差分情報の取得時刻以降の間に、ロボット４０が移動した位置姿勢差分量を入力する。本実施形態では、ロボット４０内の制御モジュールからアーム先端の位置及び姿勢が直接取得できる場合を前提として、撮像時刻間のアーム先端位置姿勢の差分から位置姿勢差分量を算出する。

しかしながら、撮像時刻間の撮像装置２３０の移動量を算出する方法は、これに限るものでない。例えば、ロボット４０の可動軸に合わせて装着されたロータリーエンコーダから各軸回転量を取得する。そして、各軸回転量とロボット４０のリンク長さから、順運動学計算によりアーム先端位置の位置及び姿勢を算出することにより、アーム先端に設置された撮像装置２３０の位置姿勢差分量を計算してもよい。撮像時刻間に移動するアーム先端の位置姿勢差分量が算出できる限り、いずれの手法でもよい。

第１の位置姿勢更新部１５０では、２次元画像撮像装置２０及び距離画像撮像装置３０により撮像された濃淡画像または距離画像に対して、３次元形状モデル保存部１１０に保存された３次元形状モデル１０を対応付ける。そして、得られた観測データを用いて、計測対象物体５０の位置及び姿勢の概略値を基に、計測対象物体５０の位置及び姿勢を更新する。第１の位置姿勢更新部１５０は、２次元画像による概略位置姿勢の更新と、距離画像による概略位置姿勢の更新とを交互に行う。処理の詳細については後述する。

第２の位置姿勢更新部１６０では、第１の位置姿勢更新部１５０により更新された計測対象物体５０の位置及び姿勢を基に、位置姿勢差分情報入力部１４０により取得された位置姿勢差分情報を用いて、計測対象物体５０の位置及び姿勢を更新する。処理の詳細については後述する。

以上が、情報処理装置１の構成の一例についての説明である。なお、情報処理装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。

次に、本実施形態における位置姿勢推定方法の処理手順について説明する。図３は、本実施形態における位置姿勢推定方法の処理手順を示すフローチャートである。
まず、Ｓ３０１では初期化を行う。初期化では、概略位置姿勢入力部１３０により、濃淡画像撮像時刻における撮像装置２３０と物体との相対的な位置姿勢の概略値を取得する。本実施形態では、前述したように、前の時刻において計測された位置及び姿勢を、計測対象物体５０の概略位置姿勢として用いる。

次に、Ｓ３０２では濃淡画像の撮像を行う。２次元画像撮像装置２０から濃淡画像を撮像する。濃淡画像の撮像が終了したら、Ｓ３０２の処理を終了し、Ｓ３０３に進む。
Ｓ３０３では、第１の位置姿勢更新部１５０により、Ｓ３０１で入力された計測対象物体５０の概略位置姿勢を元に、Ｓ３０２において入力された濃淡画像に対して、３次元形状モデル１０が当てはまるように、計測対象物体５０の位置姿勢の更新処理を行う。以下、Ｓ３０３における濃淡画像による位置姿勢更新処理について詳細を述べる。

まず、計測対象物体５０の概略位置姿勢に基づいて、Ｓ３０２において撮像された濃淡画像上のエッジと３次元形状モデル１０との対応付けを行う。計測対象物体５０の概略位置姿勢と校正済みの２次元画像撮像装置２０の内部パラメータを用いて、３次元形状モデル１０の各線分を画像へ投影し、画像上で検出されたエッジと、３次元形状モデル１０とを対応付けする。エッジが、各制御点に対応して複数検出された場合には、複数検出されたエッジのうち、投影された線分に対して画像上で最も近いエッジを対応付ける。

次に、算出した対応データに基づき、３次元形状モデル１０中の各線分と濃淡画像上のエッジとの濃淡画像上の誤差が小さくなるように、計測対象物体５０の概略位置姿勢を元に、位置姿勢の更新処理を行う。本ステップでは、先に算出した対応データに基づき、濃淡画像上に検出されたエッジの座標を観測データとして用いた拡張カルマンフィルタによるフィルタリング処理により、位置姿勢の更新を行う。以下、拡張カルマンフィルタに基づく計測対象物体５０の位置姿勢更新処理について詳細を述べる。

まず、カルマンフィルタの状態空間モデルは、次の状態の予測を行う状態遷移方程式と、状態と観測の関係を表す観測方程式からなる。ここで、本実施形態では、カルマンフィルタによる状態の推定を、フレーム内における対応データによる位置姿勢更新にのみ用いるものとし、フレーム間の時系列的フィルタリングは行わないものとして定式化を行う。計測対象物体５０の状態には特定の運動モデルを仮定せず、静止モデルを適用し、状態ベクトルも位置及び姿勢の6自由度のみを保持する。まず、状態遷移方程式は以下の形で定義される。

ここで、x_iは状態ベクトル、Fは状態遷移行列、wはシステムノイズ、iはカルマンフィルタに入力する観測データ（濃淡画像上で検出されたエッジ）のインデックスを表す。システムノイズwの誤差共分散Σ_wは更新間において混入する想定外の誤差を表すパラメータであり、一般的にチューニングパラメータとなる。本実施形態では、システムノイズの共分散行列は、零行列として与えるものとする。次に、状態ベクトルは、以下の位置及び姿勢からなる６自由度で定義する。

ここで、tx、ty、tzはそれぞれｘ軸、ｙ軸、z軸方向の位置、wx、wy、wzはそれぞれｘ軸、y軸、z軸に沿った回転を表す。また、状態ベクトルは位置・姿勢のみを保持するため、状態遷移行列は単位行列になる。一方、観測方程式は、以下の形で定義される。

ここで、y_iは観測ベクトル（濃淡画像上で検出されたエッジの座標）、h(x_i)は、x_iの状態における画像による観測（すなわち、３次元形状モデル１０の濃淡画像上への投影）を表す式、vは、観測ノイズを表す。観測ノイズの共分散行列Σｖは観測ベクトルのあいまい性を定義するパラメータである。本ステップの場合、濃淡画像中に検出されたエッジ座標のあいまい性を表している。検出座標のあいまい性は、濃淡画像の撮像環境やエッジ検出方法に大きく影響されるパラメータである。本実施形態では、平均0、標準偏差0.5画素のノイズが乗るものとして観測ノイズの共分散行列Σｖを定義する。

拡張カルマンフィルタの処理では、以上に定義した状態遷移方程式と観測方程式に基づいて予測と更新を繰り返すことで、状態ベクトル（すなわち、計測対象物体５０の位置姿勢）の更新を行う。以下、予測ステップと更新ステップの処理の詳細について、述べる。まず、予測ステップでは、状態ベクトルとその誤差共分散P_iの予測を行う。状態ベクトルの予測は、以下の式により処理を行う。

状態遷移行列が単位ベクトルのため、予測ステップにおいて状態ベクトルの値は変化しない。状態ベクトルの誤差共分散P_iの予測処理は、先に定義したシステムノイズの誤差共分散Σ_wを用いて、以下の式で行う。

ここで、予測処理の最初期の段階では、誤差共分散行列P_iの初期値を与える必要がある。状態ベクトルの誤差共分散P_iは、現在の状態ベクトルがどれだけあいまい性を持つかを表すパラメータであるため、その初期値の設定により、以後の観測データによる更新量が大きく左右されることになる。

定性的には、誤差共分散が大きいと観測データによる更新が大きく、誤差共分散が小さいと観測データによる更新が小さく行われる。本実施形態では、誤差共分散の初期値P₀の設定では、画像撮像間に発生する計測対象物体５０の想定外のずれを考慮し、平均０、位置に関する標準偏差が計測対象物体５０のサイズ１０％、姿勢に関する標準偏差が１０度の範囲であいまい性があるとして設定する。

次に、更新ステップでは、観測データに基づく状態ベクトルとその誤差行分散の更新を行う。状態ベクトルの更新は以下の式により処理が行われる。

K_iは観測データから算出される誤差にかかるカルマンゲインである。カルマンゲインは、先に定義した観測ノイズの共分散行列Σ_vを用いて、以下の式により算出する。

ここで、Hは、画像による観測を表す式h(x)をx_iにより偏微分したものであり、一般的には画像ヤコビアンと呼ばれる。画像による観測を表す式h(x)とエッジによる誤差計量方法及びエッジに基づく画像ヤコビアンの導出に関しては、本発明の本質に関わるものではないため、詳細な説明は省略する。処理の詳細については、[立野、小竹、内山、"実部品のビンピッキングのための距離・濃淡画像を最尤に統合する高精度高安定なモデルフィッティング手法"、第１３回画像の認識・理解シンポジウム（ＭＩＲＵ２０１０）、ＯＳ５−１、２０１０]を参照されたい。また、状態ベクトルの誤差共分散行列の更新は以下の式により処理が行われる。

以上に示す予測ステップと更新ステップの処理を、得られた観測データ（濃淡画像上で検出されたエッジ）の分だけ繰り返すことで、計測対象物体５０の位置姿勢の更新を行う。全ての観測データによる更新処理が終了したら、Ｓ３０３の処理を終了し、Ｓ３０４に進む。

Ｓ３０４では、距離画像撮像装置３０からの距離画像の取得を行う。本実施形態では、距離画像は撮像装置２３０から計測対象物体５０の表面までの距離が格納されているものとする。前述のように、２次元画像撮像装置２０と距離画像撮像装置３０の光軸は一致しているため、濃淡画像の各画素と距離画像の各画素の対応は既知である。

次に、Ｓ３０５では、第１の位置姿勢更新部１５０により、Ｓ３０３で更新された計測対象物体５０の位置姿勢を元に、Ｓ３０４において入力された距離画像に対して、３次元形状モデル１０が当てはまるように、計測対象物体５０の位置姿勢の更新処理を行う。以下、Ｓ３０５における距離画像による位置姿勢更新処理について詳細を述べる。

まず、Ｓ３０３で更新された計測対象物体５０の位置姿勢に基づいて、Ｓ３０４において計測された距離画像中の３次元点群と、３次元形状モデル１０との対応付けを行う。計測対象物体５０の位置姿勢と校正済みの距離画像撮像装置３０の内部パラメータを用いて、３次元形状モデル１０を構成する各面を、距離画像上に投影する。そして、投影した各面に対応する距離画像上の距離点群を、各面に対応する３次元点として保持する。

次に、検出した対応データに基づき、３次元形状モデル１０中の各面に対応する距離画像中の３次元点との３次元空間中の誤差が小さくなるように、Ｓ３０３で更新された計測対象物体５０の位置姿勢を元に、位置姿勢の更新処理を行う。本ステップでは、検出した対応データに基づき、３次元モデル中の各面に対応した距離画像上の距離点の座標を観測データとして用いた拡張カルマンフィルタによるフィルタリング処理を行うことにより、位置姿勢の更新を行う。本ステップで用いる、状態ベクトルx_jは、Ｓ３０３において更新に用いた状態ベクトルx_iを継続して用いるものとする。

一方、Ｓ３０４における位置姿勢推定後に計測対象物体５０に未知のずれが発生することを考慮し、誤差共分散P_jに関しては、Ｓ３０３において記述した誤差共分散の初期化を行う。本処理に関する、処理の大枠は、Ｓ３０３と同様である。Ｓ３０３における処理との違いは、観測値の誤差共分散行列Σ_vと、観測を表す式h(x)、計測データの誤差計量方法及び計測データに基づく画像ヤコビアンの４点である。

まず、観測ノイズの誤差共分散Σ_vは、距離画像中の距離点群の観測におけるあいまい性を表す。距離点群のあいまい性は、距離画像撮像装置３０の距離撮像方式や撮像環境に大きく影響するパラメータである。本実施形態では、距離点群の観測値の誤差共分散Σ_vは、平均０、標準偏差１ｍｍのノイズが乗るものとして設定する。

また、h(x)は３次元空間中の観測を表す式に、観測データの誤差計量方法は、３次元点と３次元面に関する誤差計量に、画像ヤコビアンは、３次元面と点に基づく画像ヤコビアンに置き換えて処理を行う。

以上に示すh(x)と誤差計量方法及び画像ヤコビアンの導出に関しては、本発明の本質に関わるものではないため、詳細な説明は省略する。処理の詳細については、[立野、小竹、内山、"実部品のビンピッキングのための距離・濃淡画像を最尤に統合する高精度高安定なモデルフィッティング手法"、第１３回画像の認識・理解シンポジウム（ＭＩＲＵ２０１０）、ＯＳ５−１、２０１０]を参照されたい。Ｓ３０３と同様に、得られた観測データ（３次元形状モデル１０中の各面に対応した距離画像上の距離点群）の分、予測ステップと更新ステップの処理を繰り返すことで、計測対象物体５０の位置姿勢の更新を行う。全ての観測データによる更新処理が終了したら、Ｓ３０５の処理を終了し、Ｓ３０６に進む。

Ｓ３０６では、位置姿勢差分情報入力部１４０から、Ｓ３０４における距離画像撮像以降に、撮像装置２３０が移動した位置姿勢差分量の算出を行う。まず、位置姿勢差分情報入力部１４０を介し、ロボット４０内の制御モジュールからロボットアーム先端の位置姿勢を取得する。そして、ロボットアーム先端から撮像装置２３０までの位置姿勢オフセットをかけることで、Ｓ３０４における距離画像の撮像時刻と、現時刻（Ｓ３０６における時刻）の撮像装置２３０の位置姿勢を計算する。そして、両時刻における撮像装置２３０の位置姿勢の差分を求めることで、距離画像撮像時刻以降に撮像装置２３０が移動した位置姿勢差分量を算出する。

次に、Ｓ３０７では、第２の位置姿勢更新部１６０により、Ｓ３０５で更新された計測対象物体５０の位置姿勢を元に、Ｓ３０６において算出された位置姿勢差分量に基づいて、計測対象物体５０の位置姿勢の更新処理を行う。以下、位置姿勢差分量に基づく位置姿勢更新処理について詳細を述べる。

本ステップでは、Ｓ３０５で更新された計測対象物体５０の位置姿勢を元に、Ｓ３０６において算出された位置姿勢差分量とそのあいまい性に基づいて、位置姿勢の更新処理を行う。まず、状態ベクトルxは、Ｓ３０３において更新に用いた状態ベクトルx_jを継続して用いるものとする。

一方、Ｓ３０６における位置姿勢推定後に計測対象物体５０に未知のずれが発生することを考慮し、誤差共分散Pに関しては、Ｓ３０３において記述した誤差共分散の初期化を行う。本処理に関する、処理の流れの大枠は、Ｓ３０３と同様である。大きく異なる点としては、観測方程式が線形方程式になるという点である。本ステップにおける観測方程式は、以下の形で定義される。

ここで、yは観測ベクトル（位置姿勢差分行列による更新結果の位置及び姿勢）、Hは観測行列（単位行列）、vは、観測ノイズを表す。ここでの観測ノイズの共分散Σ_vは、観測ベクトル（すなわち、Ｓ３０６において算出された位置姿勢差分量）のあいまい性を定義するパラメータである。本実施形態においては、ロボットアームの先端位置姿勢の差分として位置姿勢差分量を求めているため、そのあいまい性は、ロボットアームの先端位置姿勢のあいまい性と同義となる。

ロボットアームの先端位置姿勢のあいまい性は、アーム剛性やギアのバックラッシュなどの機構的要因、モータの制御精度や機構的振動の抑制による影響などの制御的要因といった多様な要因によって発生する。定性的には、可動軸が多軸になるほど、可動軸の方向が同じ方向に重なるほど、そのあいまい性は高くなるといえる。

本実施形態では、位置及び姿勢の６自由度空間において等方誤差が乗ることを想定し、位置姿勢差分量のあいまい性は、平均０、標準偏差は、位置姿勢差分量の１％の範囲であいまい性があるものとして設定する。また、本ステップにおける更新式は、Ｓ３０３及びＳ３０５と同様である（式６）、（式７）、（式８）。ただし、（式７）及び（式８）におけるHに関しては、画像ヤコビアンではなく、（式９）における観測行列をそのまま用いる。

また、Ｓ３０３及びＳ３０５では、３次元形状モデル１０に対応したエッジまたは距離点群の数だけ観測データがあったが、本ステップにおける観測データは、Ｓ３０６において算出された位置姿勢差分量のみである。そのため、１観測ベクトル分の予測ステップと更新ステップの処理を行って、計測対象物体５０の位置姿勢の更新を行う。位置姿勢差分量に基づく更新処理が終了したら、Ｓ３０７の処理を終了し、Ｓ３０８に進む。

Ｓ３０８では、Ｓ３０７で推定された位置姿勢に基づいて、ロボット４０に制御指令値を出力する。本実施形態では、Ｓ３０４において推定する前の計測対象物体５０の位置姿勢と、Ｓ３０７における推定後の計測対象物体５０の位置姿勢との差分を、ロボット座標系に変換することによって、ロボット４０に出力する制御指令値を計算する。そして、制御指令値をロボット４０の制御モジュールに出力することで、ロボット４０のアーム先端を計測対象物体５０へと移動させるべく、操作を行う。ロボット４０への制御指令値を出力したら、Ｓ３０８の処理を終了し、Ｓ３０９に進む。

Ｓ３０９では、本位置姿勢推定処理の動作を終了する入力がなされたかどうかを判定する。入力された場合には終了し、入力されなかった場合にはＳ３０２に戻り、新たな画像を取得して再度位置姿勢算出を行う。

前述したように、本実施形態では、２次元画像または距離画像の撮像間において計測対象物体５０または計測装置の移動に伴うロボット４０のモーション情報が得られる場合において、次のようにした。すなわち、２次元画像と距離画像に加え、撮像時刻間におけるロボット４０のモーション情報も観測情報として併用するようにした。これにより、高精度な対象物体の位置姿勢推定を行うことを可能とした。

［第１の変形例：観測データの取得順番は問わない例］
前述の実施形態では、観測データの取得順として、濃淡画像を撮影した後、距離画像を撮影し、その後、ロボット４０のモーション情報を取得する事例について説明した。しかしながら、観測データの取得順は、特に制限があるものではなく、いずれの順に処理を行ってよい。

例えば、まずロボット４０のモーション情報を取得・位置姿勢更新した後に、距離画像の撮像と位置姿勢更新処理を行ってもよい。また、濃淡画像を撮像・位置姿勢更新後に、ロボット４０のモーション情報を取得・位置姿勢更新を行うなどの順番に処理を行っても特に問題はない。観測データの取得と位置姿勢更新の順番に関しては、特に制限はなく、いずれの順番で処理を行ってもよい。

［第２の変形例：濃淡画像または距離画像を用いた処理を省略してもよい例］
前述の実施形態では、濃淡画像と距離画像を両方撮像し、処理を行う場合について説明していた。しかし、本発明の情報処理装置は、濃淡画像と距離画像を両方撮影する場合に限るものではない。

例えば、距離画像のみが撮像される、あるいは、濃淡画像が撮像される場合でも、本発明の情報処理装置を適用できる。この場合、前述の実施形態から、濃淡画像または距離画像に関する処理が省略されるのみで、処理自体に変更はない。また、前述の第１の変形例で述べたとおり、画像の撮像とロボットモーション情報の取得との順番も、特に制限されるものではない。ロボットモーション情報の取得と、濃淡画像あるいは距離画像の撮像処理を行う限り、濃淡画像または距離画像の取得を省略することに特に制限は無く、いずれの方法をとってもよい。

［第３の変形例：撮像装置は固定、対象物体がロボットアームにより把持されて移動する場合の例］
前述の実施形態では、図２（ａ）に示したように、撮像装置２３０がロボットアーム先端に装着されて、ロボットアームの移動により撮像装置２３０の位置姿勢が変化する場面において、本発明の情報処理装置を適用する場合について説明した。しかし、本発明の情報処理装置の適用は、前述のケースに限るものでなく、例えば、図２（ｂ）に示すように、撮像装置２３０は固定されており、ロボットアームによって把持された対象物体がアームの稼働に伴って移動する場合においても、同様に適用可能である。

この場合、Ｓ３０６において、位置姿勢差分情報入力部１４０から取得する位置姿勢移動差分情報は、前述の実施形態においては撮像時刻間の撮像装置２３０の位置姿勢移動差分情報であった。これに対し、本ケースでは、撮像時刻間の計測対象物体５０の位置姿勢差分情報となる。ただし、本情報処理装置が算出する位置姿勢は、撮像装置２３０と計測対象物体５０との相対的な位置姿勢であるため、処理自体は、前述の実施形態と変わりは無い。以上のように、本発明による情報処理装置は、撮像装置２３０が移動し計測対象物体５０が固定される場合でも、撮像装置２３０が固定され計測対象物体５０が移動する場合でも、いずれの場合においても適用することは可能であり、特に制限は無い。

［第４の変形例：位置姿勢推定処理は、カルマンフィルタに基づく手法に限らない例］
前述の実施形態では、観測データと概略位置姿勢に基づく位置姿勢の更新方法として、拡張カルマンフィルタ及びカルマンフィルタの枠組みに基づく処理を行っていた。しかし、位置姿勢更新処理の方法としてはこれに限るものではない。

例えば、非線形方程式に適用可能な非線形時系列フィルタとして、パーティクルフィルタを用いて更新処理を行ってもよい。この場合、拡張カルマンフィルタのような観測方程式の線形化を行わず、代わりに、ある一定範囲内でパーティクルと呼ばれる状態ベクトルの仮説に基づくランダムサンプリングを行うことで、尤もらしい状態ベクトルの更新処理を行う。以上に示すように、計測対象物体５０の位置姿勢を更新する手法としては、観測データとそのあいまい性に基づいて位置姿勢の更新処理が行える手法である限り、手法の選択に特に制限はなく、いずれでもよい。

［第５の変形例（撮像装置移動量の取得はロボットのモーション情報に限らない例）］
前述の実施形態では、撮像時刻間に撮像装置２３０が移動した位置姿勢差分量を算出する方法として、撮像時刻間のロボット４０のアーム先端位置姿勢の差分から位置姿勢差分量を算出する方法を述べた。しかし、撮像装置２３０の移動量を算出する手段はロボットのモーション情報を用いる方法に限るものでない。

例えば、撮像装置２３０に、磁気センサや超音波センサなど６自由度の位置及び姿勢を計測する物理センサを取り付け、それらより撮像時刻間の位置姿勢差分量を算出してもよい。また、撮像間における撮像装置２３０の移動が、回転運動のみに限定できる場面であれば、撮像装置２３０にジャイロセンサを取り付け、その回転差分量のみを計測してもよい。撮像時刻間の撮像装置２３０の位置姿勢差分量が算出できる限り、いずれの方法を用いてもよく、手段・機器の選択に特に制限はない。

［第６の変形例（ロボットによる対象物体操作を行う例）］
前述の実施形態では、ロボット４０のアーム先端に装着された撮像装置により、固定された計測対象物体５０の３次元位置姿勢の算出する方法について説明した。しかし、情報処理装置１は、対象物体の３次元位置姿勢の算出に限るものでなく、算出した位置姿勢を用いて、ロボット４０により対象物体に対して作業を行ってもよい。

例えば、ロボット４０のアーム先端に対象物体を把持することが可能なエンドエフェクタを装着し、対象物体に対して把持作業を行ってもよい。この場合、エンドエフェクタとしては、モータ駆動可能なハンドを用いてもよいし、空気圧で対象物体を吸着する吸着パッドを用いてもよく、対象物体に対する作業に応じて適切に選択する限り、選択に制限は特にない。ロボットアームとエンドエフェクタとの相対位置姿勢のキャリブレーション作業は本実施形態の実施に先立って公知の技術によって予め行われているものとする。

これによって、算出した計測対象物体の位置姿勢をパレットが置かれた空間に固定されたワークスペース座標系に変換することが可能になる。さらに、ワークスペース座標系で指定した位置姿勢にハンドが設定されるようにロボットを制御することが可能になる。エンドエフェクタによる対象物体に対する作業としては、先にあげた把持・吸着に限らず、計測対象物体５０を他の物体へ組み付ける組付作業や対象物体の外観検査など、他の作業を行ってもよく、ロボット４０による計測対象物体５０への作業内容には、特に制限はない。

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

１ 情報処理装置、１０ ３次元形状モデルデータ、２０ ２次元画像撮像装置、３０ 距離画像撮像装置、４０ ロボット、１１０ ３次元形状モデル保存部、１２０ 画像入力部、１３０ 概略位置姿勢入力部、１４０ 位置姿勢差分情報入力部、１５０ 第１の位置姿勢更新部、１６０ 第２の位置姿勢更新部

Claims (8)

1. 可動軸を有するロボットに搭載された撮像装置を用いて被測定物体の位置姿勢を推定する情報処理装置であって、
   前記被測定物体の幾何的特徴を表す３次元形状モデルのデータを保存する３次元形状モデル保存手段と、
   前記被測定物体を撮像した画像を取得する画像入力手段と、
   前記撮像装置と前記被測定物体との間の相対的な概略位置姿勢を入力する概略位置姿勢入力手段と、
   前記概略位置姿勢をもとにして、前記画像に対して前記３次元形状モデルを当てはめることで、前記概略位置姿勢を更新する第１の位置姿勢更新手段と、
   前記ロボットの前記可動軸の情報に基づいて、前記撮像装置が前記被測定物体を撮像した時刻、または前回の位置姿勢差分情報の取得時刻以降に前記撮像装置が移動した位置姿勢差分量を取得する位置姿勢差分情報入力手段と、
   前記第１の位置姿勢更新手段による更新後の位置姿勢を、前記位置姿勢差分量に基づいて更新する第２の位置姿勢更新手段と、
   を備えることを特徴とする情報処理装置。
2. 前記画像入力手段は、２次元画像及び／または距離画像を取得し、前記第１の位置姿勢更新手段は、前記２次元画像及び／または前記距離画像に対して、前記３次元形状モデルを当てはめることで、前記概略位置姿勢を更新することを特徴とする請求項１に記載の情報処理装置。
3. 前記第１の位置姿勢更新手段は、前記２次元画像による概略位置姿勢の更新処理と、前記距離画像による概略位置姿勢の更新処理とを交互に行うことを特徴とする請求項２に記載の情報処理装置。
4. 前記第１の位置姿勢更新手段における前記概略位置姿勢の更新処理は、非線形時系列フィルタを用い、前記第２の位置姿勢更新手段における前記概略位置姿勢の更新処理は、線形または非線形時系列フィルタを用いることを特徴とする請求項１〜３の何れか１項に記載の情報処理装置。
5. 算出した被測定物体の位置姿勢に基づいて、回転及び／または並進移動軸からなる可動軸を有するロボットによって、被測定物体または撮像装置の位置及び／または姿勢を変更する位置姿勢操作手段をさらに有することを特徴とする請求項１〜４の何れか１項に記載の情報処理装置。
6. 可動軸を有するロボットに搭載された撮像装置を用いて被測定物体の位置姿勢を推定する情報処理方法であって、
   前記被測定物体の幾何的特徴を表す３次元形状モデルのデータを保存する３次元形状モデル保存工程と、
   前記被測定物体を撮像した画像を取得する画像入力工程と、
   前記撮像装置と前記被測定物体との間の相対的な概略位置姿勢を入力する概略位置姿勢入力工程と、
   前記概略位置姿勢をもとにして、前記画像に対して前記３次元形状モデルを当てはめることで、前記概略位置姿勢を更新する第１の位置姿勢更新工程と、
   前記ロボットの前記可動軸の情報に基づいて、前記撮像装置が前記被測定物体を撮像した時刻、または前回の位置姿勢差分情報の取得時刻以降に前記撮像装置が移動した位置姿勢差分量を取得する位置姿勢差分情報入力ステップと、
   前記第１の位置姿勢更新ステップにおける更新後の位置姿勢を、前記位置姿勢差分量に基づいて更新する第２の位置姿勢更新ステップと、
   を含むことを特徴とする情報処理方法。
7. 可動軸を有するロボットに搭載された撮像装置を用いて被測定物体の位置姿勢を推定する情報処理方法の各工程をコンピュータに実行させるためのプログラムであって、
   前記被測定物体の幾何的特徴を表す３次元形状モデルのデータを保存する３次元形状モデル保存工程と、
   前記被測定物体を撮像した画像を取得する画像入力工程と、
   前記撮像装置と前記被測定物体との間の相対的な概略位置姿勢を入力する概略位置姿勢入力工程と、
   前記概略位置姿勢をもとにして、前記画像に対して前記３次元形状モデルを当てはめることで、前記概略位置姿勢を更新する第１の位置姿勢更新工程と、
   前記ロボットの前記可動軸の情報に基づいて、前記撮像装置が前記被測定物体を撮像した時刻、または前回の位置姿勢差分情報の取得時刻以降に前記撮像装置が移動した位置姿勢差分量を取得する位置姿勢差分情報入力工程と、
   前記第１の位置姿勢更新ステップにおける更新後の位置姿勢を、前記位置姿勢差分量に基づいて更新する第２の位置姿勢更新工程と
   をコンピュータに実行させるためのプログラム。
8. 請求項７に記載のプログラムを記憶したことを特徴とするコンピュータ読み取り可能な記憶媒体。

Images