JP2016109630A - 情報処理装置、情報処理方法、プログラム - Google Patents

Abstract

【課題】 物体の位置及び姿勢を精度よく計測することを目的とする。【解決手段】 対象物体の概略位置姿勢を取得し、前記対象物体を非接触センサで計測することにより、前記対象物体の位置情報を取得し、前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得し、前記取得した前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得手段で取得した接触位置の位置情報の精度を示す第２の精度情報とを取得し、前記概略位置姿勢と、前記第１の精度情報と、前記第２の精度情報とに基づいて、前記対象物体の形状情報を、前記対象物体の位置情報と前記接触位置の位置情報とに対応づけることにより、前記対象物体の位置姿勢を導出する。【選択図】 図１

Description

本発明は、３次元形状が既知である物体の位置及び姿勢を計測する技術に関する。

産業用ロボットによる製品の自動組み立てにおいて、ロボットが部品を把持するためには、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。位置及び姿勢を計測する方法としては、ロボットや環境に取り付けたカメラで撮影する画像上から検出される特徴や、距離センサから得られる距離画像に対して物体の３次元形状モデルをあてはめるモデルフィッティングによる計測法がある（非特許文献１）。

特開２０１２‐１１５３１号公報

Ｔ．Ｄｒｕｍｍｏｎｄ ａｎｄ Ｒ．Ｃｉｐｏｌｌａ，"Ｒｅａｌ−ｔｉｍｅ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ ｏｆ ｃｏｍｐｌｅｘ ｓｔｒｕｃｔｕｒｅｓ，"ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２４，ｎｏ．７，ｐｐ．９３２−９４６，２００２．

複雑な部品組み立てなどの作業では、常に部品位置姿勢を取得するのに好適な画像が撮影できない場合が発生する。特に部品を把持した際には、ロボットハンドにより部品が隠されるため、部品の位置姿勢算出に必要な特徴が欠損し位置姿勢の算出に失敗する場合がある。さらにロボットハンド付近に設置したカメラからでは物体の一部しか撮影できない場合が多く、部品の位置姿勢算出に失敗しやすい。そのため従来は位置姿勢算出に必要な特徴が十分撮影できるように計測装置を増やしたり、撮影位置を調整したりする必要があった。

特許文献１では、検出手段で撮影画像から対象物体の位置を求め、求めた位置をロボットハンドが接触した位置に基づいて修正する方法が開示されている。しかしながら特許文献１の方法では、上述したように物体の一部しか撮影できない場合のように、検出手段から対象物体の位置が求められない場合には、位置の修正が難しいという課題があった。

本発明は、以上の課題に鑑みてなされたものであり、物体の一部についての計測情報が取得できない場合であっても、高精度に位置姿勢の計測を行うことを目的とする。

上記の課題を解決するために、本発明の情報処理装置は、例えば、対象物体の形状情報を保持するデータ保持手段と、前記対象物体の概略位置姿勢を取得する概略位置姿勢取得手段と、前記対象物体を非接触センサで計測することにより、前記対象物体の位置情報を取得する位置情報取得手段と、前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得手段と、前記位置情報取得手段で取得した前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得手段で取得した接触位置の位置情報の精度を示す第２の精度情報とを取得する手段と、前記概略位置姿勢と、前記第１の精度情報と、前記第２の精度情報とに基づいて、前記データ保持手段で保持する対象物体の形状情報を、前記対象物体の位置情報と前記接触位置の位置情報とに対応づけることにより、前記対象物体の位置姿勢を導出する位置姿勢導出手段とを備えることを特徴とする。

また、本発明の情報処理装置は、例えば、対象物体の形状情報を保持するデータ保持手段と、前記対象物体の概略位置姿勢を取得する概略位置姿勢取得手段と、前記対象物体を非接触センサで計測することにより、前記対象物体の位置情報を取得する位置情報取得手段と、前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得手段と、前記位置情報取得手段で取得した前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得手段で取得した接触位置の位置情報の精度を示す第２の精度情報とを取得する手段と、前記概略位置姿勢と、前記第１の精度情報と前記第２の精度情報とに基づいて、前記対象物体の位置情報と前記接触位置の位置情報とのうち、いずれか一方を選択し、該選択された位置情報と前記対象物体の形状情報とを対応づけることにより、前記対象物体の位置姿勢を算出する位置姿勢導出手段とを備えることを特徴とする。

本発明により、物体の一部についての計測情報が取得できない場合であっても、高精度に位置姿勢の計測を行うことができる。

第１の実施形態における情報処理装置１の装置と対象物体の関係を示す図である。 第１の実施形態における情報処理装置１の接触位置及び距離計測データの模式図である。 第１の実施形態における情報処理装置１の構成を示す図である。 第１の実施形態における処理の手順を示すフローチャートである。 第１の実施形態における情報処理装置１の接触位置及び２次元計測データの模式図である。 第２の実施形態における情報処理装置２の構成を示す図である。 第３の実施形態における情報処理装置３の構成を示す図である。 第３の実施形態における処理の手順を示すフローチャートである。 第４の実施形態における情報処理装置４の装置と対象物体の関係を示す図である。 本発明の情報処理装置のハードウェア構成を示す図である。

本発明にかかる各実施形態を説明するのに先立ち、各実施形態に示す情報処理装置が実装されるハードウェア構成について、図１０を用いて説明する。

図１０は、本実施形態における情報装置のハードウェア構成図である。同図において、ＣＰＵ１０１０は、バス１０００を介して接続する各デバイスを統括的に制御する。ＣＰＵ１０１０は、読み出し専用メモリ（ＲＯＭ）１０２０に記憶された処理ステップやプログラムを読み出して実行する。オペレーティングシステム（ＯＳ）をはじめ、本実施形態に係る各処理プログラム、デバイスドライバ等はＲＯＭ９２０に記憶されており、ランダムアクセスメモリ（ＲＡＭ）１０３０に一時記憶され、ＣＰＵ１０１０によって適宜実行される。また、入力Ｉ／Ｆ１０４０は、外部の装置（表示装置や操作装置など）から情報処理装置１で処理可能な形式で入力信号として入力する。また、出力Ｉ／Ｆ１０５０は、外部の装置（表示装置）へ表示装置が処理可能な形式で出力信号として出力する。

これらの各機能部は、ＣＰＵ１０１０が、ＲＯＭ１０２０に格納されたプログラムをＲＡＭ１０３０に展開し、後述する各フローチャートに従った処理を実行することで実現されている。また例えば、ＣＰＵ１０１０を用いたソフトウェア処理の代替としてハードウェアを構成する場合には、ここで説明する各機能部の処理に対応させた演算部や回路を構成すればよい。

（第１の実施形態）
第１の実施形態では、ロボットハンドによる接触位置及び距離センサによる非接触計測データの精度情報に応じて、対象物体の位置姿勢を計測する方法について述べる。本実施形態では非接触計測データ及び接触位置の精度情報である誤差分布の特性を考慮し、誤差分布の小さい非接触計測データ及び接触位置を重視して位置姿勢を算出する。これにより、精度高く対象物体の位置姿勢を算出することができる。

本実施形態では図１に示すように、ロボットハンド１００２を取り付けたロボットアーム１００１に、さらに距離センサ１００３を取り付け、対象物体を距離計測する。ロボットハンド１００２で対象物体を把持した際には、図２の撮影画像１１０１に示すような距離計測点１１０５の集合が計測される。撮影画像１１０１は、距離センサが備える撮像装置によって撮像された画像である。把持時には距離センサ１００３が対象物体１００４に接近するため、撮影画像１１０１のように対象物体の一部しか計測できない場合が多い。このような場合、６自由度ある対象物体の位置姿勢を算出しようとしても、撮影画像面に直交する位置姿勢の成分を決定できない場合がある。そこで本実施形態では距離計測点に加えてロボットハンドの接触位置情報を用いることで、対象物体の位置姿勢の算出を可能にする。さらに距離計測点１１０５と接触点１１０６の計測精度を表す誤差分布を考慮し、誤差分布の小さい非接触計測データ及び接触位置を重視して位置姿勢を算出することでより精度の高い位置姿勢を算出することができる。

＜構成＞
図３は情報処理装置１のブロック構成図である。以下に本実施形態における情報処理装置１の構成について述べる。

本実施形態は、所定の位置姿勢に置かれた対象物体を把持するシーンを想定する。把持する際に対象物体の位置姿勢がずれるため、その対象物体の位置姿勢を非接触計測データ及びロボットハンドの接触位置に基づいて算出する。

情報処理装置１は、データ保持部１０２、概略位置姿勢取得部１０３、非接触計測データ取得部１０４、接触位置取得部１０５、精度取得部１０６、位置姿勢算出部１０７を有する。そして、ロボットハンドによる接触位置及び非接触センサによる計測データの精度に応じて、対象物体の位置姿勢を精度高く計測する。さらに情報処理装置１は、ロボットコントローラ１００に算出した位置姿勢を送り、ロボットアーム・ハンド１０１を制御して、対象物体を把持させる。なお、図１のロボットハンド１００２とロボットアーム１００１が、図３のロボットアーム・ハンド１０１に対応している。

ロボットアーム・ハンド１０１は、６軸のロボットアームとロボットアームの先端に取り付けられたロボットハンドである。ロボットコントローラ１００５による制御に従って、ロボットアームの先端を入力された任意の位置・姿勢に移動することができる。また、ロボットアームの各関節角情報とロボットアーム・ハンド１０１の形状情報に基づいて、ロボットアームの基準座標系におけるロボットアームの各関節の位置姿勢及びロボットハンドの位置姿勢を出力する。ロボットハンドは指令を受けてロボットハンドを開閉し、物体を把持する。ロボットハンドには接触センサが取り付けられており、この接触センサによって、ロボットハンドが把持している接触位置情報を取得することができる。

非接触センサ１００３は、計測対象である物体表面上の点までの距離情報を計測する。本実施形態では、距離値を各画素に格納した距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。距離センサとしては、プロジェクタ等により対象物体に照射したパターン光の反射光をカメラで撮像し、三角測量により距離を計測するアクティブ式のものを利用する。しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。これらのアクティブ式の距離センサは、表面のテクスチャが少ない物体が対象であるとき好適である。また、ステレオカメラが撮像する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。パッシブ式の距離センサは、表面のテクスチャが十分ある物体を対象とする場合に好適である。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

データ保持部１０２は、対象物体の位置姿勢を推定する際に用いる対象物体の形状情報を保持する。本実施形態では対象物体の表面の３次元点の集合を形状情報とする。対象物体の形状情報は位置姿勢算出部１０７にて利用する。データ保持部１０２は、さらに以下の情報を保持する。（１）ロボットアームの基準座標における対象物体の概略位置姿勢。（２）ロボットアームと非接触センサとの相対位置姿勢。（３）ハンドに取り付けた接触センサとロボットハンドとの相対位置姿勢。（４）非接触計測データ及び接触位置の精度情報などのキャリブレーション情報。これらキャリブレーション情報は事前に計測しておく。

概略位置姿勢取得部１０３は、対象物体のロボットアームの基準座標における概略位置姿勢を取得する。対象物体の概略位置姿勢は位置姿勢算出部１０７で利用する。またロボットコントローラ１００に送信され、ロボットアーム・ハンド１０１の制御に利用する。

非接触計測データ取得部１０４は、ロボットアームに取り付けた非接触センサ１００３で計測した非接触計測データとして、ロボットアームの基準座標における３次元点の集合を取得する。非接触計測データは位置姿勢算出部１０７にて利用する。

接触位置取得部１０５は、ロボットハンドに取り付けた接触センサを用いて接触を検知した時のロボットハンドの接触位置のロボットアームの基準座標における３次元位置の集合を取得する（接触位置情報取得）。取得した接触位置は位置姿勢算出部１０７にて利用する。

精度情報取得部１０６は、非接触計測データ取得部１０４で取得した３次元点群（非接触計測データ）の精度情報、及び接触位置取得部１０５で取得した接触位置の３次元位置の精度情報を取得する。ここで精度情報とは各３次元点の誤差分布（ばらつき値）のことである。ここでは誤差分布として正規化共分散行列を用いる。誤差分布は、事前に非接触センサおよび接触センサそれぞれで、予め３次元位置が既知の物体に対して複数回計測を行うことで、取得しておく。誤差分布は、３次元座標をキーとした誤差分布のテーブルを非接触計測データ、接触位置データそれぞれについて作成し、精度情報としてデータ保持部１０２で保持する。保持された精度は位置姿勢算出部１０７にて利用する。

位置姿勢算出部１０７は、対象物体の位置姿勢を算出する（位置姿勢導出）。具体的には、以下の情報に基づいて対象物体の位置姿勢を算出する。（１）データ保持部１０２で保持する対象物体の形状情報。（２）概略位置姿勢取得部１０３で取得した対象物体の概略位置姿勢。（３）非接触計測データ取得部１０４で取得した非接触計測データ。（４）接触位置取得部１０５で取得したロボットハンドの接触位置。（５）精度取得部１０６で取得した非接触計測データ及びロボットハンドの接触位置の精度。具体的には、概略位置姿勢を初期値として公知のＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔｓ）アルゴリズムを用いて算出する。算出した位置姿勢をロボットコントローラ１００に送信し、ロボットアーム・ハンド１０１を制御することで、対象物体の組み付けや整列配置などを高精度に実現できる。

＜処理の流れ＞
図４に示すフローチャートを用いて、本実施形態の情報処理装置１の処理の流れを説明する。

＜ステップＳ１０１０＞
ステップＳ１０１０では、データ保持部１０２で保持している対象物体の形状情報、すなわち対象物体の表面の３次元点の集合を取得する。前記３次元点は対象物体の基準座標系における３次元点である。

＜ステップＳ１０２０＞
ステップＳ１０２０では、概略位置姿勢取得部１０３は、対象物体のロボットアームの基準座標における概略位置姿勢を取得する。具体的にはデータ保持部１０２で保持するロボットアームの基準座標における対象物体の概略位置姿勢を読み出す。

＜ステップＳ１０３０＞
ステップＳ１０３０では、概略位置姿勢取得部１０３は、ステップＳ１０２０で取得した対象物体の概略位置姿勢をロボットコントローラ１００に送信する。そして、ロボットコントローラ１００は、ロボットアーム・ハンド１０１を制御して対象物体の所定の位置を把持する。

＜ステップＳ１０４０＞
ステップＳ１０４０では、非接触計測データ取得部１０４は、非接触計測データとして、ロボットアームの基準座標における３次元点の集合を取得する。

具体的には、ロボットアームに取り付けた非接触センサ１００３から３次元点群を取得する。非接触センサ１００３から取得した３次元点群は非接触センサ１００３のローカル座標系上の３次元点群である。よって、非接触センサ１００３とロボットアームとの相対位置姿勢、非接触センサを取り付けたロボットアームの位置姿勢に基づいて、非接触センサ１００３のローカル座標系における３次元点座標をロボットアームの基準座標系に変換する。

＜ステップＳ１０５０＞
ステップＳ１０５０では、接触位置取得部１０５は、接触位置として、ロボットアームの基準座標におけるロボットハンドが対象物体に接触している位置の３次元座標の集合を取得する。具体的には、ロボットハンドに取り付けた不図示の接触センサから、ロボットハンドが対象物体に接触している位置を取得する。接触センサから取得した接触位置は接触センサ上のローカル座標系の３次元点である。よって、接触センサとロボットハンドとの相対位置姿勢と、ロボットアームとロボットハンドの相対位置姿勢と、ロボットハンドの位置姿勢に基づいて、接触位置のローカル座標系における３次元位置をロボットアームの基準座標系に変換する。

＜ステップＳ１０６０＞
ステップＳ１０６０では、精度取得部１０６は、ステップＳ１０４０及びステップＳ１０５０で取得した非接触計測データ及び接触位置の精度情報を取得する。具体的にはデータ保持部１０２で保持する精度情報である非接触計測データの誤差分布テーブルから、ステップＳ１０４０で取得した非接触計測データの３次元点集合の各３次元点に対応する誤差分布を、各３次元点の精度情報として取得する。接触位置に関しても同様で、データ保持部１０２で保持する接触位置の誤差分布テーブルから、ステップＳ１０５０で取得した接触位置の３次元点集合の各３次元点に対応する誤差分布を、各３次元点の精度情報として取得する。接触センサの接触位置に関する誤差分布は、接触位置の面の法線に略直交する方向に広がる分布である。

＜ステップＳ１０７０＞
ステップＳ１０７０では、位置姿勢算出部１０７は、ロボットアームの基準座標における対象物体の位置姿勢を算出する。位置姿勢算出には公知のＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔｓ）アルゴリズムを用いる。ＩＣＰアルゴリズムは、２つのＮ次元点群間の相対位置姿勢を、対応付けと位置姿勢算出を繰り返し行うことにより算出する方法である。

ステップＳ１０４０及びステップＳ１０５０で取得したロボットアームの基準座標における非接触計測データ及び接触位置を一方の３次元点群とする。そして、もう一方をステップＳ１０１０で取得した対象物体の基準座標系における３次元点群とすれば、ロボットアームの基準座標系における対象物体の位置姿勢を算出できる。具体的には以下の手順で算出する。

ステップＳ１０４０で取得したロボットアームの基準座標における非接触計測データの３次元点の集合をＶ、ステップＳ１０５０で取得したロボットアームの基準座標における接触位置の３次元点の集合をＣとする。ロボットアームの基準座標系における対象物体の位置姿勢、すなわち対象物体の基準座標系からロボットアームの基準座標系への変換行列をＭとする。

（１）変換行列Ｍの初期値として、ステップＳ１０２０で取得した対象物体のロボットアームの基準座標における概略位置姿勢を設定する。

（２）ステップＳ１０１０で取得した対象物体の基準座標系における、対象物体表面の３次元点集合Ｑを変換行列Ｍで座標変換する。変換後の３次元点集合をＭＱとする。

（３）ロボットアームの基準座標系における、非接触計測データの３次元点集合Ｖ及び接触位置の３次元点集合Ｃと、対象物体表面点集合ＭＱとの間で近傍探索を行う。ＶとＭＱとの対応点集合を（ｖ、ｑ）∈Ｓｖ、ＣとＭＱとの対応点集合を（ｃ、ｑ）∈Ｓｃとする。ここで、ｖ∈Ｖ、ｃ∈Ｃ、ｑ∈ＭＱである。

（４）対応付けた３次元点をそれぞれｐ、ｑとすると、対応間距離ｅを以下の式で表す。

ステップＳ１０６０で取得したＶとＣの誤差分布、つまり正規化共分散行列をそれぞれΣｖｉ、Σｃｊとする。そして以下の式を最大化する変換行列Ｍ’を最尤推定により算出する。

ここでＮ（ｅ，Σ）は期待値ｅ、正規化共分散行列Σの正規分布を表す。

（５）変換行列Ｍに（４）で求めた変換行列Ｍ’を代入し、変換行列Ｍが収束するまで（２）〜（５）を繰り返す。

以上の計算により、対象物体の基準座標系からロボットアームの基準座標系への変換行列Ｍ、すなわちロボットアームの基準座標系における対象物体の位置姿勢を算出する。

アクティブステレオやパッシブステレオなどのステレオ計測による非接触センサで計測した３次元点は図２の距離計測点１１０５のように奥行き方向に伸びた楕円形の誤差分布になる。この距離計測点１１０５の集合のみを用いて６自由度ある対象物体の位置姿勢を算出しようとしても、図２のような対象物体が一部しか計測できないシーンでは撮影画像面に直交する位置姿勢の成分を決定できない。

それに対して接触点１１０６は接触センサの設置されているロボットハンドの表面の法線方向には確度が高いが、法線に直交する方向の確度は低い。そのため図２の距離計測点１１０６のようにロボットハンドの表面の法線に直交する方向に広がりをもった誤差分布となる。本実施形態では、この距離計測点１１０５と傾向の異なる誤差分布を持つ接触点１１０６を距離計測点１１０５と同時に利用して対象物体の位置姿勢を算出する。このように傾向の異なる非接触センサの非接触計測データとロボットハンドの接触位置の精度情報（誤差分布）を考慮することにより、互いの精度情報（誤差分布）の性質を補いあって対象物体の位置姿勢を精度高く算出することができる。

（変形例）
上記構成では、非接触計測データ取得部１０４で取得するのは非接触センサで計測した３次元点群であった。しかしながら非接触計測データ取得部１０４で取得するのはカメラで撮影した画像から抽出した画像特徴であっても良い。例えば画像特徴がエッジ点である場合、図５のようなシーンで本発明が特に有効である。エッジ点は図５に示すエッジ点の楕円のように画像から検出されたエッジ方向には確度が低いが、エッジ方向に垂直な方向への確度は高い。また画像面の奥行き方向については１つのエッジ点から決まる情報はなく、奥行き方向に無限に広がりをもつ誤差分布となる。図５のシーンでは、エッジ点のみを用いて位置姿勢を算出した場合、撮影画像１２０１に直交する位置姿勢成分の確度は高いが、画像面の奥行き方向の位置姿勢成分の確度は低くなる。このときに図５のように対象物体を把持したロボットハンドの接触点１２０６の誤差分布を考慮し、上記の方法と同様に、エッジ点１２０５と接触点１２０６の誤差分布を同時利用して対象物体の位置姿勢を算出する。これにより、互いの誤差分布（精度情報）の性質を補いあって対象物体の位置姿勢を精度高く算出することができる。

さらにロボットアームに距離センサとカメラの両方を取り付け、非接触計測データ取得部１０４で距離センサからの３次元点群とカメラからの画像特徴の両方を非接触計測データとしてもよい。

また上記の構成では誤差分布を利用した最尤推定により、非接触計測データと接触位置を同時利用して精度高く対象物体の位置姿勢を算出する例を示した。しかしながら同時利用の方法は最尤推定でなくても良い。例えば、非接触計測データの誤差の分散と接触点の誤差の分散を重みとし、重み付き最小二乗法などを用いて対象物体の位置姿勢を算出しても良い。重みは分散に限らず、画像特徴や距離計測点などの幾何特徴の検出信頼度や、接触点を検知する信号強度に基づいて決めても良いし、検出された点の数に応じて決めても良い。

以上述べた構成で、傾向の異なる非接触計測データとロボットハンドの接触位置の精度情報（誤差分布）を考慮し、誤差分布の小さい非接触計測データ及び接触位置を重視して位置姿勢を算出する。これにより、互いの精度情報（誤差分布）の性質を補いあって対象物体の位置姿勢を精度高く算出することができる。

（第２の実施形態）
第２の実施形態では、ロボットハンドによる接触位置及び距離センサによる非接触計測データの精度情報に応じて、精度の高いデータのみを利用するように、計測データを切り替えて対象物体の位置姿勢を算出する方法について述べる。

距離センサは反射輝度の低い物体や、動いている物体、計測可能範囲外の物体などを計測した場合に計測精度が低下することが多い。またロボットハンドに取り付けた接触センサも、把持した物体が滑ったり、柔軟であったりする場合に計測精度が低下する場合がある。このような信頼できない低精度の計測データを用いて対象物体の位置姿勢を算出すると、誤った位置姿勢を算出する場合がある。そこで本実施形態では、接触位置及び距離センサによる非接触計測データの精度に応じて利用するデータを切り替えることで、精度の低い計測データが混入しても対象物体の位置姿勢を精度高く算出する方法について述べる。

＜構成＞
本実施形態の構成を図６に示す。第１の実施形態との差分についてのみ説明する。

非接触計測データ取得部２０４は、ロボットアームに取り付けた距離センサで計測した非接触計測データとして、ロボットアームの基準座標における３次元点及びその計測信頼度の集合を取得する。計測信頼度は値が小さいほど信頼度が高い数値であるとする。例えばＴＯＦ（Ｔｉｍｅ−ｏｆ−Ｆｌｉｇｈｔ）センサであれば反射強度などに基づいて計測信頼度を算出でき、ステレオセンサであればパターンマッチングのマッチング度合いなどに基づいて計測信頼度を算出できる。

接触位置取得部２０５は、ロボットハンドに取り付けた接触センサを用いて接触を検知した時のロボットハンドの接触位置のロボットアームの基準座標における３次元位置及びその計測信頼度の集合を取得する。計測信頼度は値が小さいほど信頼度が高い数値であるとする。接触センサの信頼度はセンサで検出した信号の強度や分散などに基づいて算出できる。

精度取得部２０６では、非接触計測データ取得部２０４で取得した３次元点群の精度として非接触計測データ取得部２０４で取得した計測信頼度を設定する。同様に接触位置取得部２０５で取得した接触位置の３次元位置の精度として、接触位置取得部２０５で取得した計測信頼度を設定する。

位置姿勢算出部２０７は、対象物体の位置姿勢を算出する。具体的には、第１の実施形態と同様に、以下の情報に基づいて算出する。（１）データ保持部２０２で保持する対象物体の形状情報。（２）概略位置姿勢取得部２０３で取得した対象物体の概略位置姿勢。（３）非接触計測データ取得部２０４で取得した非接触計測データ。（４）接触位置取得部２０５で取得したロボットハンドの接触位置。（５）精度取得部２０６で取得した非接触計測データ及びロボットハンドの接触位置の精度。ただし、このとき精度取得部２０６で取得した精度に基づいて、位置姿勢算出に利用するデータを選択して算出する。

＜処理の流れ＞
本実施形態の処理の流れは図４に示す第１の実施形態のフローチャートと同様であり、以下ではその差分について説明する。

ステップＳ１０１０〜Ｓ１０３０は第１の実施形態と同様である。

＜ステップＳ１０４０＞
ステップＳ１０４０では、非接触計測データ取得部２０４は、非接触計測データとして、ロボットアームの基準座標における３次元点及びその計測信頼度の集合を取得する。第１の実施形態と同様に、距離センサとロボットアームとの相対位置姿勢と、距離センサを取り付けたロボットアームの位置姿勢に基づいて、距離センサのローカル座標系における３次元点座標をロボットアームの基準座標系に変換する。

＜ステップＳ１０５０＞
ステップＳ１０５０では、接触位置取得部２０５は、接触位置として、ロボットアームの基準座標におけるロボットハンドが対象物体に接触している位置の３次元座標及びその計測信頼度の集合を取得する。第１の実施形態と同様に、接触センサとロボットハンドとの相対位置姿勢と、ロボットアームとロボットハンドの相対位置姿勢と、ロボットハンドの位置姿勢に基づいて、接触位置のローカル座標系における３次元位置をロボットアームの基準座標系に変換する。

＜ステップＳ１０６０＞
ステップＳ１０６０では、精度取得部２０６は、ステップＳ１０４０及びステップＳ１０５０で取得した非接触計測データ及び接触位置の精度として、各々の計測信頼度を設定する。

＜ステップＳ１０７０＞
ステップＳ１０７０では、位置姿勢算出部２０７は、ロボットアームの基準座標における対象物体の位置姿勢を算出する。第１の実施形態と同様に、位置姿勢算出にはＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔｓ）アルゴリズムを用いる。ただし、位置姿勢算出に利用する非接触計測データはその精度が所定の閾値ｔ１より大きいもののみとする。また接触点もその精度が所定の閾値ｔ２より大きいもののみを利用する。よって精度が閾値より大きい非接触計測データと対象物体表面点との対応点集合を（ｖ、ｑ）∈Ｓｖ’、接触点と対象物体表面点との対応点集合を（ｃ、ｑ）∈Ｓｃ’とすると、以下の式を最小化する変換行列Ｍ’を最小二乗推定により算出できる。

以上の処理により接触位置及び距離センサによる非接触計測データの精度に応じて利用するデータを切り替えることで、精度の低い計測データが混入しても対象物体の位置姿勢を精度高く算出することができる。

（変形例）
上記第２の実施形態は、非接触計測データと接触点の各点ごとに精度をチェックして位置姿勢算出に利用する点を選択していた。しかしながら、非接触計測データの平均精度や最小精度、最大精度などをチェックして、所定の閾値よりも精度が大きければ非接触計測データ全体を利用せず、接触点のみで位置姿勢を算出しても良い。また反対に接触点の平均精度などが所定の閾値よりも大きければ非接触計測データのみで位置姿勢を算出しても良い。さらに、非接触計測データと接触点の精度がどちらも閾値より小さければ、非接触計測データ及び接触位置の少なくとも一方を再度計測し直し、精度取得、位置姿勢算出をやり直しても良い。また、非接触計測データと接触点のうち、精度の高い方のみを利用しても良い。

以上述べた構成で、非接触計測データとロボットハンドの接触位置の精度に応じて、精度の高いデータのみを利用するように、データを切り替えることで、精度の低い計測データが混入しても対象物体の位置姿勢を精度高く算出することができる。

（第３の実施形態）
第３の実施形態では、ロボットハンドによる接触位置及び距離センサによる非接触計測データから算出される位置姿勢の曖昧性がより小さくなるように対象物体の把持位置を決定することで、より精度の高い対象物体の位置姿勢を算出する方法について述べる。

本実施形態の装置構成は第１の実施形態で説明した図１と同様である。本実施形態では、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の把持位置を決定した上で対象物体を把持・接触し、距離計測点１１０５と接触点１１０６の計測精度を考慮することで、より精度の高い対象物体の位置姿勢を算出することができる。

本実施形態では、山積みされた対象物体の内の１つを把持するシーンを想定する。対象物体の山積みを非接触計測して把持する物体の概略位置姿勢を取得し、把持位置を決定して把持する。把持する際に対象物体の位置姿勢がずれるため、その対象物体の位置姿勢を非接触計測データ及びロボットハンドの接触位置に基づいて算出する。

＜構成＞
図７は本実施形態で説明する情報処理装置３のブロック構成図である。以下に本実施形態における情報処理装置３の構成について述べる。

情報処理装置３は、第１の実施形態と同様に、ロボットコントローラ１００５に算出した位置姿勢を送り、ロボットアーム・ハンド１０１を制御して、対象物体を把持させる。情報処理装置３は、データ保持部３０２、概略位置姿勢取得部３０３、非接触計測データ取得部３０４、接触位置取得部３０５、精度取得部３０６、位置姿勢算出部３０７に加えて、接触位置決定部３０８を有する。

情報処理装置３の構成のうち、第１の実施形態と差分があるものについてのみ説明する。

概略位置姿勢取得部３０３は、対象物体のロボットアームの基準座標における概略位置姿勢を取得する。ここでは、ロボットアームに取り付けている距離センサ１００３で対象物体を含むシーンを３次元計測し、得られたロボットアームの基準座標における３次元点の集合から対象物体の位置姿勢を検出する。把持位置決定部３０８は、対象物体の把持位置を決定する。決定した把持位置はロボットコントローラ３００に渡され、対象物体を把持する際の制御に利用する。

＜処理の流れ＞
図７に示すフローチャートを用いて、本実施形態の情報処理装置３の処理の流れを説明する。ただし、本実施形態の処理の流れは図４に示す第１の実施形態のフローチャートと同様の部分があり、以下ではその差分について説明する。

ステップＳ３０４０〜Ｓ３０７０は第１の実施形態のＳ１０４０〜Ｓ１０７０と同様である。

＜ステップＳ３０１０＞
ステップＳ３０１０では、概略位置姿勢取得部３０３は、データ保持部３０２で保持している対象物体の形状情報、すなわち対象物体の表面の３次元点及び法線の集合を取得する。前記３次元点は対象物体の基準座標系における３次元点である。次にステップＳ３０２０で行う対象物体の認識で用いる辞書を構築する。対象物体の検出にはＳｐｉｎ Ｉｍａｇｅｓを用いた認識手法を利用する。Ｓｐｉｎ Ｉｍａｇｅｓ特徴量は距離画像や３次元点群から算出する局所特徴量である。よって、対象物体の形状情報からＳｐｉｎ Ｉｍａｇｅｓ特徴量を抽出し辞書として保持しておく。なお、事前に辞書作成を行ってデータ保持部３０２に格納しておき、それを読み出しても良い。Ｓｐｉｎ Ｉｍａｇｅｓを用いた認識手法については以下の非特許文献に開示されている。
Ａ．Ｅ． Ｊｏｈｎｓｏｎ ａｎｄ Ｍ． Ｈｅｂｅｒｔ，“Ｕｓｉｎｇ Ｓｐｉｎ Ｉｍａｇｅｓ ｆｏｒ Ｅｆｆｉｃｉｅｎｔ Ｏｂｊｅｃｔ Ｒｅｃｏｇｎｉｔｉｏｎ ｉｎ Ｃｌｕｔｔｅｒｅｄ ３Ｄ Ｓｃｅｎｅｓ，”ＩＥＥＥ Ｔｒａｎｓ． ｏｎ ＰＡＭＩ，ｖｏｌ．２１，ｎｏ．５，ｐｐ．４３３−４４９，１９９９．
＜ステップＳ３０２０＞
ステップＳ３０２０では、概略位置姿勢取得部３０３は、対象物体のロボットアームの基準座標における概略位置姿勢を取得する。具体的にはロボットアームに取り付けている距離センサ１００３で対象物体を含むシーンを３次元計測し、得られたロボットアームの基準座標における３次元点の集合から対象物体の位置姿勢を検出する。前述したとおり、対象物体の検出にはＳｐｉｎ Ｉｍａｇｅｓを用いた認識手法を利用する。計測した３次元点の集合から、Ｓｐｉｎ Ｉｍａｇｅｓ特徴量を抽出し、ステップＳ３０１０で準備した辞書と照合し、対象物体の位置姿勢を算出する。さらに算出した位置姿勢を初期値として、対象物体の形状情報と計測した３次元点の集合に対してＩＣＰアルゴリズムを適用して対象物体の位置姿勢をリファインし、概略位置姿勢とする。ここでは、対象物体表面の３次元点及び法線を局所平面とみなして、対応付けた計測３次元点との点と平面との距離を最小化することによって、対象物体の位置姿勢を算出する。

＜ステップＳ３０２５＞
ステップＳ３０２５では、把持位置決定部３０８は、対象物体の把持位置を決定する。ステップＳ３０２０で概略位置姿勢を算出する際に用いた距離センサ１００３で３次元計測して取得した３次元点の集合（計測データ）を利用して、概略位置姿勢の曖昧性を算出し、その曖昧性を解消する把持位置を決定する。

概略位置姿勢の曖昧性は、対象物体の形状情報（対象物体の表面の３次元点及び法線の集合）と計測した３次元点の集合（計測データ）からヤコビ行列を求めて算出する。ヤコビ行列とは、対象物体の位置姿勢を算出する際に着目する幾何特徴の対応間距離が変化したときに、位置姿勢６自由度のパラメータが変化する度合いを表す値である。具体的には、３次元空間中での点と平面の距離を位置及び姿勢の関数としたときの、位置及び姿勢の各要素に関する一次の偏微分係数を要素とする行列である。幾何特徴のヤコビ行列をみることで、その幾何特徴が位置姿勢６自由度のどのパラメータに影響を与え易いかがわかる。ヤコビ行列の特定のパラメータが小さい場合、そのパラメータへの影響が小さい、すなわちそのパラメータが決まらず位置合わせに失敗する、または十分な精度で位置合わせができないことを表す。以下では幾何特徴のヤコビ行列の導出方法を示す。

計測データの偶然誤差およびデータ量・空間分布と位置姿勢算出の偶然誤差の関係は、以下の非特許文献に、計測データの偶然誤差による位置姿勢推定結果の偶然誤差への伝播としてモデル化する手法が提案されている
Ｗ． Ｈｏｆｆ ａｎｄ Ｔ． Ｖｉｎｃｅｎｔ，“Ａｎａｌｙｓｉｓ ｏｆ ｈｅａｄ ｐｏｓｅ ａｃｃｕｒａｃｙ ｉｎ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ，”ＩＥＥＥ Ｔｒａｎｓ． Ｖｉｓｕａｌｉｚａｔｉｏｎ ａｎｄ Ｃｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ，Ｖｏｌ ６， Ｎｏ．４，２０００．
ステップＳ３０２０で述べた通り、位置姿勢の算出には、対象物体の表面の３次元点及び法線（形状情報）を局所平面とし、対応付けた計測３次元点（計測データ）との距離を最小化するＩＣＰアルゴリズムで行っている。ここで、計測データはロボットアームの基準座標系、対象物体の形状情報は対象物体の基準座標系のデータである。対象物体の形状情報を表す表面点の３次元位置と法線のロボットアーム基準座標系における３次元座標を（ｘ，ｙ，ｚ）^Ｔ、法線方向を（ｎ_ｘ，ｎ_ｙ，ｎ_ｚ）^Ｔ（単位ベクトル）、計測データの３次元点の座標を（ｘ’，ｙ’，ｚ’）^Ｔとする。そうすると、（数１）で表していた３次元空間での符号付き距離ｅは以下の式より算出できる。
ｅ＝ｎ_ｘ（ｘ’−ｘ）＋ｎ_ｙ（ｙ’−ｙ）＋ｎ_ｚ（ｚ’−ｚ）
そして、対象物体の基準座標系からロボットアームの基準座標系への変換パラメータｓ＝（ｓ１，ｓ２，ｓ３，ｓ４，ｓ５，ｓ６）^Ｔで、対応間距離ｅを偏微分することによって、ヤコビ行列Ｊ_３Ｄは以下のように算出できる。

ここで

で

は対象物体の基準座標系からロボットアームの基準座標系への姿勢変換行列、（ｘ_ｍ，ｙ_ｍ，ｚ_ｍ）^Ｔは対象物体の基準座標系での形状情報の３次元点の座標を表す。この１×６のヤコビ行列Ｊ_３Ｄを対応点数ｍだけ連結したｍ×６行列のヤコビ行列Ｊを用いると、位置姿勢と観測データ間の微小変化は、（数７）で示すように、位置姿勢（の微小変化Δ）の一次近似による誤差の関数として表すことができる。これは、位置姿勢の微小変化による、観測データ位置の微小変化を表す関係式である。

この式より、計測データの共分散行列Ｃｏｖ（ｅ）から誤差逆伝播の枠組みに基づき位置姿勢の共分散行列Ｃｏｖ（Δ）を算出する。精度取得部３０６より、計測データの精度として計測データの各３次元点の標準偏差を取得し、それをσ_{ｒａｎｇｅ ｊ}とする。共分散行列Ｃｏｖ（ｅ）は、σ_{ｒａｎｇｅ ｊ}を用いて以下の（数８）で表される。

計測データの共分散行列Ｃｏｖ（ｅ）、および、各計測データに対応するヤコビ行列Ｊから、位置姿勢の共分散行列Ｃｏｖ（Δ）を計算すると、以下になる。

この式に基づき、推定する位置姿勢パラメータの曖昧性を表す共分散行列Ｃｏｖ（Δ）を算出する。この共分散行列Ｃｏｖ（Δ）の最大固有ベクトルｕが、概略位置姿勢として算出している位置姿勢の最も曖昧な方向を表す。

対象物体の把持位置、すなわちステップＳ３０７０で位置姿勢算出に用いる接触点は、図２の距離計測点１１０６のようにロボットハンドの表面の法線に直交する方向に広がりをもった誤差分布を持つ。よって位置姿勢算出の曖昧性を減らすために、前記最大固有ベクトルｕに最も近い法線をもつ点を対象物体の形状情報から選択し、把持位置とする。

＜ステップＳ３０３０＞
ステップＳ３０３０では、把持位置決定部３０８は、ステップＳ３０２５で決定した把持位置と、ステップＳ３０２０で取得した対象物体の概略位置姿勢をロボットコントローラ３００に送り、ロボットアーム・ハンド３０１を制御し、対象物体を把持する。

＜ステップＳ３０７０＞
ステップＳ３０７０では、位置姿勢算出部３０７は、ステップＳ３０１０と同様に、対象物体の位置姿勢を算出する。ただし、対応間距離ｅを（数４）の対象物体の表面の３次元点及び法線（形状情報）が表す局所平面と対応付けた計測３次元点（計測データ）として、（数３）を最大化する変換行列Ｍ’を最尤推定により算出する。

＜ステップＳ３０８０＞
ステップＳ３０８０では、位置姿勢算出部３０７は、ステップＳ３０２５で説明した方法で、ステップＳ３０７０で算出した位置姿勢の曖昧性を表す共分散行列を算出する。ただし、ステップＳ３０７０では計測データとして非接触計測データと接触点の両方を用いて位置姿勢を算出するので、曖昧性を算出する際の計測データも非接触計測データと接触点の両方を用いる。算出した共分散行列の最大固有値（算出した位置姿勢の最もばらつきが大きい方向の分散を表す）が所定の閾値以下であれば位置姿勢の算出を終了し、大きければステップＳ３０２０に戻って計測をやり直す。

これにより、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の把持位置を決定した上で対象物体を把持・接触し、傾向の異なる非接触計測データとロボットハンドの接触位置の精度を考慮して、対象物体の位置姿勢を精度高く算出することができる。

＜変形例＞
上記ステップＳ３０２５の把持位置決定方法では、算出した位置姿勢の曖昧性のみから把持位置を決定した。それに加えて、ロボットハンドの形状情報をさらに利用して、決定した把持位置が把持可能かチェックしても良い。例えば、ロボットハンドの形状情報と、対象物体の位置姿勢と形状情報やシーンの３次元計測データを用いて、算出した把持位置を把持可能かシミュレーションし、対象物体の表面点の集合からその法線が前記最大固有ベクトルｕに近い順に探しても良い。

以上述べた構成で、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の把持位置を把持することにより、傾向の異なる非接触計測データとロボットハンドの接触位置を確実に得ることができるため、対象物体の位置姿勢を精度高く算出することができる。

（第４の実施形態）
第４の実施形態では、対象物体を把持した後に、ロボットハンドによる接触位置及び距離センサによる非接触計測データから算出される位置姿勢の曖昧性が小さくなるように、ロボットハンドを制御して接触位置を追加する。これにより精度の高い位置姿勢を算出する方法について述べる。

本実施形態の装置構成を図８に示す。本実施形態で述べる方法では、算出した対象物体の位置姿勢の曖昧性がより小さくなるような対象物体表面上の点や面を決定し、ロボットハンド３００２の一部である接触指３００７で触ることにより、接触位置を追加する。追加した接触位置と追加前の接触位置の集合、距離センサ３００３で取得した非接触計測データを用いて、再度位置姿勢を算出することで、より精度の高い対象物体の位置姿勢を算出することができる。

本実施形態では、第３の実施形態と同様に、山積みされた対象物体の内の１つを把持するシーンを想定する。対象物体の山積みを非接触計測して把持する物体の概略位置姿勢を取得し、把持位置を決定して把持する。把持する際に対象物体の位置姿勢がずれるため、その対象物体の位置姿勢を非接触計測データ及びロボットハンドの接触位置に基づいて算出する。

＜構成＞
本実施形態の構成は、図７で示される第３の実施形態と同様で、以下ではその差分について説明する。

接触位置決定部３０８は、第３の実施形態と同様に対象物体の把持位置を決定する。さらに接触位置を追加取得するために、接触指３００７を接触させる位置も決定する。決定した把持位置及び接触位置はロボットコントローラ３００に渡され、対象物体を把持及び接触する際の制御に利用する。

＜処理の流れ＞
図８に示すフローチャートを用いて、本実施形態の情報処理装置４の処理の流れを説明する。本実施形態の処理の流れは第３の実施形態と同様であり、以下ではその差分について説明する。

ステップＳ３０１０は第３の実施形態と同様である。ステップＳ３０２０〜Ｓ３０８０の最初のループは第３の実施形態と同様である。ただし２週目以降のループでは、把持し直すのではなく、接触指３００７を接触させて接触位置を追加する。以下では２週目以降のループでの各ステップの処理を説明する。

＜ステップＳ３０２０＞
ステップＳ３０２０は、概略位置姿勢取得部３０３は、対象物体のロボットアームの基準座標における概略位置姿勢を取得する。具体的には前の周回のステップＳ３０７０で算出した対象物体の位置姿勢を概略位置姿勢として設定する。

＜ステップＳ３０２５＞
ステップＳ３０２５では、接触位置決定部３０８は、対象物体の接触位置を決定する。概略位置姿勢として設定した前の周回のステップＳ３０７０で算出した対象物体の位置姿勢の曖昧性を算出し、その曖昧性を解消する把持位置を決定する。即ち、対象物体の位置姿勢算出に利用した非接触計測データ及び接触位置、対象物体の形状情報（対象物体の表面の３次元点及び法線の集合）からヤコビ行列を求めて算出する。接触位置の算出方法は実施形態３のステップＳ３０２５の把持位置の算出方法と同様である。

＜ステップＳ３０３０＞
ステップＳ３０３０は、ステップＳ３０２５で決定した接触位置と、ステップＳ３０２０で取得した対象物体の概略位置姿勢をロボットコントローラ３００に送信し、ロボットアーム・ハンド３０１を制御し、対象物体の表面にロボットハンドの指を接触させる。

＜ステップＳ３０４０＞
ステップＳ３０４０では、非接触計測データ取得部３０４は、非接触計測データとして、ロボットアームの基準座標における３次元点の集合を取得する。なお前の処理ループで取得した非接触計測データを記憶しておいて再び利用しても良い。

＜ステップＳ３０５０＞
ステップＳ３０５０では、接触位置取得部３０５は、ステップＳ３０３０でロボットハンドが接触した位置を取得する。取得した接触位置と、前の周回のときに取得した接触位置を合わせた３次元座標の集合を、接触位置として出力し、以降のステップで利用する。

＜ステップＳ３０６０＞
ステップＳ３０６０では、精度取得部３０６は、ステップＳ３０４０及びステップＳ３０５０で取得した非接触計測データ及び接触位置の精度を取得する。

＜ステップＳ３０７０＞
ステップＳ３０７０では、位置姿勢算出部３０７は、第３の実施形態のステップＳ３０７０と同様に、対象物体の位置姿勢を算出する。

＜ステップＳ３０８０＞
ステップＳ３０８０では、位置姿勢算出部３０７は、第３の実施形態のステップＳ３０８０と同様に、ステップＳ３０２５で説明した方法で、ステップＳ３０７０で算出した位置姿勢の曖昧性を表す共分散行列を算出する。算出した共分散行列の最大固有値（算出した位置姿勢の最もばらつきが大きい方向の分散を表す）が所定の閾値以下であれば位置姿勢の算出を終了し、大きければステップＳ３０２０に戻って別の接触位置を計測する。

これにより、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の接触位置を逐次追加して対象物体の位置姿勢を算出することができ、精度の高い対象物体の位置姿勢を取得することができる。

＜バリエーション＞
第４の実施形態では、図８に示すような接触用の指３００７を用いて接触位置を追加した。しかしながら、接触位置を追加するのに用いる装置はそれに限るものではない。ロボットアームが複数あれば、対象物体を把持していない一方のロボットアーム・ハンドを用いて接触位置を追加しても良い。また対象物体が接触時に動かなければ把持せずに接触位置を追加しても良い。

以上述べた構成で、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の接触位置を逐次追加する。これにより、傾向の異なる非接触計測データとロボットハンドの接触位置を確実に得ることができるため、対象物体の位置姿勢を精度高く算出することができる。

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

＜各実施形態の効果＞
第１の実施形態では、傾向の異なる非接触計測データとロボットハンドの接触位置の精度（誤差分布）を考慮し、誤差分布の小さい非接触計測データ及び接触位置を重視して位置姿勢を算出する。これにより、計測装置を増やすことなく、互いの精度（誤差分布）の性質を補いあって対象物体の位置姿勢を精度高く算出することができる。

第２の実施形態では、非接触計測データとロボットハンドの接触位置の精度に応じて、精度の高いデータのみを利用するように、計測データを切り替える。これにより、計測装置を増やすことなく、精度の低い計測データが混入しても対象物体の位置姿勢を精度高く算出することができる。

第３の実施形態では、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の位置を把持する。これにより、傾向の異なる非接触計測データとロボットハンドの接触位置を確実に得ることができるため、計測装置を増やすことなく、対象物体の位置姿勢を精度高く算出することができる。

第４の実施形態では、算出する位置姿勢の曖昧性がより小さくなるような対象物体上の接触位置を逐次追加する。これにより、傾向の異なる非接触計測データとロボットハンドの接触位置を確実に得ることができるため、計測装置を増やすことなく、対象物体の位置姿勢を精度高く算出することができる。

＜定義＞
ロボットコントローラは、ロボットアームやロボットハンドを任意の位置姿勢への移動及び把持動作を制御し、ロボットアームやロボットハンドの位置姿勢や関節角度などのロボットの状態を出力できれば良い。ロボットコントローラは、ロボットアームの先端の位置姿勢や把持対象物体の位置姿勢、ロボットアームの関節角情報を受信してロボットアーム・ハンドを制御しても良い。

ロボットアームは、１つ以上の稼働部位があるロボットであればどのような形態であっても構わない。腕が複数あっても良く、複数のロボットで構成しても良い。ロボットは６軸ロボットであっても、スカラロボットやパラレルリンクロボットであっても良く、その形態は問わない。また実体を持たないシミュレーション上のロボットであっても良い。

ロボットハンドは部品を把持できる機構があり、把持や接触を検知できればどのような形態であっても構わない。２指であっても多指であっても良いし、吸着ハンドであってもかまわない。接触計測用の指があっても良い。また実体を持たないシミュレーション上のロボットハンドであっても良い。

データ保持部は、対象物体の位置姿勢を推定する際に用いる対象物体やロボットアームの形状情報やキャリブレーション情報を保持できれば良い。対象物体の形状情報は３次元点や法線など幾何特徴の集合であっても良い。３次元形状のメッシュモデルであっても、ＣＡＤモデルのような解析曲面モデルであっても良い。キャリブレーション情報は、以下の情報であってもよい。ロボットアームの基準座標における対象物体の概略位置姿勢。ロボットアームと距離センサとの相対位置姿勢。ロボットハンドに取り付けた接触センサとロボットハンドとの相対位置姿勢。非接触計測データ及び接触位置の精度情報、計測装置のカメラパラメータなど。上記データを保持する記憶装置はＨＤＤやＳＳＤ、メモリ、テープドライブなど、データを保持できればどのようなものでも構わない。

概略位置姿勢取得部は、対象物体のロボットアームの基準座標における概略位置姿勢を取得できれば良い。データ保持部から位置姿勢を読み出しても良いし、ロボットアームや環境に取り付けた距離センサやカメラなどで対象物体を計測し、その計測データから対象物体の位置姿勢を取得しても良い。計測データからの位置姿勢算出にはＳｐｉｎ Ｉｍａｇｅｓなどの特徴量を用いた認識手法であっても良いし、テンプレートマッチングに基づいた手法であっても良い。ＩＣＰアルゴリズムなどを用いたモデルフィッティング手法を用いてもかまわない。

非接触計測データ取得部は、ロボットアームの基準座標における対象物体の非接触計測データを取得できれば良い。非接触計測データはロボットアームや環境に取り付けた距離センサで計測しても良いし、予め計測していたデータを記憶装置から読み出しても良い。非接触計測データは２次元画像や距離画像であっても良いし、３次元点やその法線、３次元特徴の集合であっても良い。２次元画像から検出したエッジや特徴点などの幾何特徴の集合であっても良く、非接触で対象物体を計測した情報であれば良い。取得した非接触計測データが局所座標系であれば、データ保持部で保持するキャリブレーション情報を用いて、ロボットアームの基準座標に変換すれば良い。

接触位置取得部は、ロボットアームの基準座標における、対象物体に接触した位置が取得できば良い。接触位置は接触センサで直接接触を検知しても良いし、ロボットハンドや指にかかる負荷を検知して間接的に接触を検知しても良い。予め計測していたデータを記憶装置から読み出しても良い。取得した接触位置が局所座標系であれば、データ保持部で保持するキャリブレーション情報を用いて、ロボットアームの基準座標に変換すれば良い。

精度取得部は、非接触計測データ取得部で取得した非接触計測データの精度、及び接触位置取得部で取得した接触位置の精度情報を取得できれば良い。予め計測していたデータを記憶装置から読み出しても良い。精度情報は、非接触計測データ及び接触位置の精度を表していればどのような表現であってもかまわない。誤差分布であっても良いし、計測信頼度であっても良い。また似た計測点が多いほど精度を高く設定しても良いし、撮影画像の明るさや幾何特徴の強度であっても良い。誤差分布は正規化共分散行列であっても良いし、正規分布の分散値であっても良い。事前に複数回計測を行って作った３次元座標をキーとした誤差分布のテーブルであっても良いし、そのテーブルに関数を当てはめたものや、モデル化してパラメータ化したものでも構わない。テーブルや関数のキーは３次元座標に限らず、画素の明るさや幾何特徴の強度、面の向きであっても良い。

位置姿勢算出部は、以下の情報に基づいて、対象物体のロボットアームの基準座標における位置姿勢を算出できれば良い。（１）データ保持部で保持する対象物体の形状情報。（２）概略位置姿勢取得部で取得した対象物体の概略位置姿勢。（３）非接触計測データ取得部で取得した非接触計測データ。（４）接触位置取得部で取得したロボットハンドの接触位置。（５）精度取得部で取得した非接触計測データ及びロボットハンドの接触位置の精度。位置姿勢の算出にはＩＣＰアルゴリズムを用いても良いし、対象物体の形状情報から抽出した幾何特徴と、非接触計測データや接触位置から抽出した幾何特徴の類似度を測り、類似する幾何特徴の距離に基づいて位置姿勢を算出しても良い。また、対応する対象物体の形状情報と、非接触計測データや接触位置との距離は点間距離を用いても良いし、点と面との距離を用いても良い。また位置姿勢の算出には最尤推定や最小二乗推定などの最適化手法を用いても良いし、前記距離に基づく評価値が最も良い位置姿勢を全探索により探索しても良い。

また非接触計測データ及び接触位置の精度は最尤推定における共分散行列や、最小二乗推定における重みとして利用しても良いし、前記評価値の重みとして利用しても良い。さらに精度が所定の閾値以下のデータは位置姿勢に利用しなくても良い。非接触計測データの平均精度や最小精度、最大精度などをチェックして、所定の閾値よりも精度が大きければ非接触計測データ全体を利用せず、接触点のみで位置姿勢を算出しても良い。また反対に接触点の平均精度などが所定の閾値よりも大きければ非接触計測データのみで位置姿勢を算出しても良い。さらに、非接触計測データと接触点の精度がどちらも閾値より小さければ、非接触計測データ及び接触位置の少なくとも一方を再度計測し直し、精度取得、位置姿勢算出をやり直しても良い。また、非接触計測データと接触点のうち、精度の高い方のみを利用しても良い。

接触位置決定部は、算出する位置姿勢の曖昧性がより小さくなるような対象物体の把持位置及び接触位置の少なくとも一方を決定できれば良い。把持及び接触位置は、算出した対象物体の位置姿勢の最も曖昧な方向に最も近い法線をもつ点や面を対象物体の形状情報から選択しても良い。さらにロボットハンドの形状情報を利用して把持及び接触位置が接触可能かチェックし、接触可能な位置のみを選択しても良い。

Claims (13)

1. 対象物体の形状情報を保持するデータ保持手段と、
   前記対象物体の概略位置姿勢を取得する概略位置姿勢取得手段と、
   前記対象物体を非接触センサで計測することにより、前記対象物体の位置情報を取得する位置情報取得手段と、
   前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得手段と、
   前記位置情報取得手段で取得した前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得手段で取得した接触位置の位置情報の精度を示す第２の精度情報とを取得する手段と、
   前記概略位置姿勢と、前記第１の精度情報と、前記第２の精度情報とに基づいて、前記データ保持手段で保持する対象物体の形状情報を、前記対象物体の位置情報と前記接触位置の位置情報とに対応づけることにより、前記対象物体の位置姿勢を導出する位置姿勢導出手段とを備えることを特徴とする情報処理装置。
2. 対象物体の形状情報を保持するデータ保持手段と、
   前記対象物体の概略位置姿勢を取得する概略位置姿勢取得手段と、
   前記対象物体を非接触センサで計測することにより、前記対象物体の位置情報を取得する位置情報取得手段と、
   前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得手段と、
   前記位置情報取得手段で取得した前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得手段で取得した接触位置の位置情報の精度を示す第２の精度情報とを取得する手段と、
   前記概略位置姿勢と、前記第１の精度情報と前記第２の精度情報とに基づいて、前記対象物体の位置情報と前記接触位置の位置情報とのうち、いずれか一方を選択し、該選択された位置情報と前記対象物体の形状情報とを対応づけることにより、前記対象物体の位置姿勢を算出する位置姿勢導出手段とを備えることを特徴とする情報処理装置。
3. 前記第１の精度情報は、前記非接触センサにより計測されるデータの誤差分布であることを特徴とする請求項１または２に記載の情報処理装置。
4. 前記第２の精度情報は、前記接触位置における前記対象物体の面の法線に略直交する方向に広がりのある誤差分布であることを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
5. 前記位置姿勢導出手段は、前記第１の精度情報と前記第２の精度情報とに基づいて、前記対象物体の位置情報と、前記接触位置の位置情報とのうち、より確からしい位置情報に大きい重みをつけることにより、前記対象物体の位置姿勢を算出することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記第１の精度情報および前記第２の精度情報は、位置情報の誤差分布の広がりを示し、
   前記位置姿勢導出手段は、前記対象物体の位置情報と前記接触位置の位置情報それぞれの誤差分布の広がりに応じて、前記対象物体の位置情報と前記接触位置の位置情報に重みを設定し、該重みに基づいて前記対象物体の位置姿勢を算出することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
7. 前記位置情報取得手段で取得した前記対象物体の位置情報に基づいて、前記接触センサを接触させる接触位置を決定する接触位置決定手段をさらに有することを特徴とする請求項１乃至６のいずれか１項に記載の情報処理装置。
8. 前記非接触センサは、撮像装置であり、前記位置情報は、前記撮像装置により取得される画像から抽出される前記対象物体の特徴であることを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記接触センサは、前記対象物体を把持するためのロボットハンドに取り付けられていることを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
10. 請求項１乃至９のいずれか１項に記載の情報処理装置と、
    前記対象物体を保持または把持する手段を備えることを特徴とするシステム。
11. 対象物体の概略位置姿勢を取得する概略位置姿勢取得工程と、
    前記対象物体を非接触センサで計測することにより得られる、前記対象物体の位置情報を取得する位置情報取得工程と、
    前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得工程と、
    前記位置情報取得工程において取得された前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得工程において取得された接触位置の位置情報の精度を示す第２の精度情報とを取得する工程と、
    前記概略位置姿勢と、前記第１の精度情報と、前記第２の精度情報とに基づいて、データ保持手段で保持される対象物体の形状情報を、前記対象物体の位置情報と前記接触位置の位置情報とに対応づけることにより、前記対象物体の位置姿勢を導出する位置姿勢導出工程とを備えることを特徴とする情報処理方法。
12. 対象物体の概略位置姿勢を取得する概略位置姿勢取得工程と、
    前記対象物体を非接触センサで計測することにより得られる、前記対象物体の位置情報を取得する位置情報取得工程と、
    前記対象物体に対して、接触センサを接触させることにより、該接触センサが接触した接触位置の位置情報を取得する接触位置情報取得工程と、
    前記位置情報取得工程において取得された前記対象物体の位置情報の精度を示す第１の精度情報と、前記接触位置取得工程において取得された接触位置の位置情報の精度を示す第２の精度情報とを取得する取得工程と、
    前記概略位置姿勢と、前記第１の精度情報と、前記第２の精度情報とに基づいて、前記対象物体の位置情報と前記接触位置の位置情報とのうち、いずれか一方を選択し、該選択された位置情報と前記対象物体の形状情報とを対応づけることにより、前記対象物体の位置姿勢を算出する位置姿勢導出工程とを備えることを特徴とする情報処理方法。
13. コンピュータを、請求項１乃至１０の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images