WO2022190534A1 - 認識装置、ロボット制御システム、認識方法、およびプログラム - Google Patents

Abstract

所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識装置は、過去において前記認識装置が認識した前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定手段と、前記推定手段が推定した位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識手段と、を有する。

Description

認識装置、ロボット制御システム、認識方法、およびプログラム

　本発明は、認識装置、ロボット制御システム、認識方法、およびプログラムに関する。

　従来、各姿勢の或る物体の特徴を示すテンプレートと、撮像装置が当該物体を撮像して取得した画像（計測画像）とを照合することによって、当該物体の位置姿勢を認識（推定）することが行われている。

　特許文献１では、距離センサから得た距離に応じてテンプレートのサイズを変えて、照合を行うシステムが開示されている。これによれば、システムが保持するテンプレートの数を少なくすることができる。そして、距離に応じてテンプレートを選択することにより照合回数を少なくすることができる。

米国特許第９６５９２１７号明細書

　しかし、特許文献１のようにテンプレートの数を少なくしても、物体の姿勢ごとにテンプレートを用意して、それぞれのテンプレートと画像とを照合しなければならないため、未だに多くの回数の照合が必要であった。このため、物体の位置姿勢を推定するために、多くの処理が必要であった。

　そこで、本発明は、物体を計測した計測画像を用いて、物体の位置姿勢を推定する場合に、処理数を低減できる技術を提供することを目的とする。

　上記目的を達成するために本発明は、以下の構成を採用する。

　すなわち、本発明の一側面に係る認識装置は、所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識装置であって、過去において前記認識装置が認識した前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定手段と、前記推定手段が推定した位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識手段と、を有することを特徴とする認識装置である。

　このような構成によれば、テンプレートを用いずに、高速に物体の位置姿勢を推定することができる。そして、３次元モデルと計測画像の比較の前に、物体の位置姿勢を大まかに把握することができるので、比較における３次元モデルの物体の位置姿勢の範囲を限定することができる。従って、より高速に、かつ、より効率的に、物体の位置姿勢の認識をすることができる。

　上記認識装置において、前記計測範囲を設定する設定手段をさらに有し、前記推定手段は、さらに、前記推定手段が推定した位置または位置姿勢に基づき前記所定の物体が存在する範囲である推定範囲を推定し、前記設定手段は、前記推定範囲を前記計測範囲として設定してもよい。これによれば、計測範囲を限定することができるので、より高速に計測画像を取得することが可能になる。

　上記認識装置において、前記推定手段は、過去の少なくとも２つの時点間における前記所定の物体の位置姿勢の変化に基づき、現在における前記所定の物体の位置姿勢を推定してもよい。これによれば、簡単な方法（つまり、少ないステップ）で所定の物体の位置姿勢を推定できる。このため、さらに、高速に、かつ、効率的に、物体の位置姿勢の認識をすることができる。

　上記認識装置において、前記計測画像は、撮像センサによって計測された結果を表し、前記撮像センサの位置姿勢は、ロボットによって制御され、前記推定手段は、過去における前記所定の物体の位置姿勢と現在におけるロボットの位置姿勢とに基づき、現在における前記所定の物体の位置姿勢を推定してもよい。これによれば、過去の１つの時点の所定の物体の位置姿勢しか推定に用いることができない場合にも、現在における前記所定の物体の位置姿勢を推定できる。

　上記認識装置において、前記計測画像は、被写体までの距離を各画素が表す距離画像であってもよい。

　上記認識装置において、前記過去における前記所定の物体の位置姿勢の前記認識手段による認識結果が存在しない場合には、前記推定手段は、現在における前記所定の物体の位置姿勢を推定せず、前記認識手段は、前記所定の物体の特徴量を示す複数のテンプレートのそれぞれと前記計測画像との照合結果に応じて前記所定の物体の位置姿勢を認識してもよい。これによれば、過去の所定の物体の位置姿勢の認識結果がない場合にも、現在における前記所定の物体の位置姿勢を推定できる。

　また、上記認識装置と、前記計測範囲を計測する撮像センサと、前記認識装置が認識した結果に基づき、前記所定の物体の位置姿勢を制御するロボットと、を有することを特徴とするロボット制御システムであってもよい。これによれば、高速に所定の物体の位置姿勢を認識できるため、ロボットを効率的に制御することができる。

　本発明は、上記手段の少なくとも一部を有する装置として捉えてもよいし、電子機器や制御システム、情報処理システム、情報処理装置、認識装置、認識システム、ロボットとして捉えてもよい。また、本発明は、上記処理の少なくとも一部を含む制御方法、照合方法、認識方法、ロボット制御方法として捉えてもよい。また、本発明は、かかる方法を実現するためのプログラムやそのプログラムを非一時的に記録した記録媒体（記憶媒体）として捉えることもできる。なお、上記手段および処理の各々は可能な限り互いに組み合せて本発明を構成することができる。

　本発明によれば、物体を計測した計測画像を用いて、物体の位置姿勢を推定する場合に、処理数を低減できる。

図１は、実施形態１に係る認識システムを説明する図である。 図２は、実施形態１に係るテンプレート生成装置と認識装置の構成図である。 図３は、実施形態１に係る物体の位置姿勢の認識処理のフローチャートである。 図４は、実施形態１に係る過去情報を用いた認識処理のフローチャートである。 図５は、実施形態２に係る過去情報を用いた認識処理のフローチャートである。 図６は、認識システムの座標系を説明する図である。 図７は、認識システムの他の構成を説明する図である。

　以下、本発明を実施するための実施形態について図面を用いて記載する。

＜適用例＞
　以下では、或る物体の３次元モデル（３次元データ）に基づく当該物体の特徴量を示すテンプレートと、撮像センサが当該物体を計測した結果を表す画像（計測画像）とを照合する認識システム１は、過去の物体の位置姿勢に応じて現在の物体の位置姿勢を推定する。そして、認識システム１は、推定した位置姿勢を初期値にした物体の３次元モデル（３次元データ）と計測画像とを比較することによって、物体の位置姿勢を認識する。なお、位置姿勢とは、以下の実施形態では、位置および姿勢であるが、技術的な矛盾が生じなければ、位置または姿勢であってもよい。

　これによれば、テンプレートを用いずに、高速に物体の位置姿勢を推定することができる。そして、３次元モデルと計測画像の比較の前に、物体の位置姿勢を大まかに把握することができるので、比較における３次元モデルの物体の位置姿勢の範囲を限定することができる。従って、より高速に、かつ、より効率的に、物体の位置姿勢の認識をすることができる。

＜実施形態１＞
［認識システムの構成］
　図１を参照して、実施形態１に係る認識システム１の構成を説明する。認識システム１は、物体２（所定の物体）についての３次元モデルと物体２を計測した結果を表す計測画像とを比較することによって、物体２の位置姿勢を認識する。認識システム１は、撮像センサ１０、テンプレート生成装置２０、認識装置３０、記憶装置４０、ロボット制御装置５０、ロボット６０を有する。また、認識システム１は、把持物体３を物体２に接続するロボット６０を制御するロボット制御システムでもある。

　撮像センサ１０は、現在の物体２を含む計測範囲を計測することによって計測画像を取得する。本実施形態では、撮像センサ１０は、左レンズを介して被写体を撮像した画像と右レンズを介して被写体を撮像した画像を取得し、その２つの画像（左右画像）の差異を比較することによって、距離画像を計測画像として取得する３次元センサである。距離画像は、各画素が撮像センサ１０から被写体までの距離を示す。なお、撮像センサ１０は、三角測量計測やＴｏＦ（Ｔｉｍｅ　ｏｆ　Ｆｌｉｇｈｔ）方式など任意の方法によって、距離画像を取得してよい。また、計測画像は、各画素が被写体の温度を示す温度画像であってもよいし、通常の光学画像（被写体の色や輝度を表現した画像）であってもよい。

　テンプレート生成装置２０は、物体２を事前に計測した３次元モデル、または、物体２を設計する際に用いた物体２の３次元モデルに基づき、物体２の特徴量を示すテンプレートを生成する。例えば、３次元モデル（３次元データ）は、点群データによって物体２を表現するデータでもよいし、ＣＡＤデータのように物体２の形状や構造を定義するデータでもよい。

　認識装置３０は、撮像センサ１０が取得した計測画像を用いて、物体２の現在の位置姿勢の推定処理を行う。また、認識装置３０は、推定結果に基づき、物体２の現在の位置姿勢を認識する。

　記憶装置４０は、物体２の３次元モデル、テンプレート生成装置２０が生成したテンプレート、認識装置３０が照合した照合結果、または／および認識装置３０が認識した物体２の位置姿勢の情報を記憶（記録）する。記憶装置４０は、ハードディスク（ＨＤＤ）やメモリ（ＲＡＭ；Ｒａｍｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）を有するサーバなどであり得る。また、記憶装置４０は、テンプレート生成装置２０および認識装置３０に対して抜き差し可能な記憶媒体であってもよい。記憶装置４０には、互いに異なる姿勢の物体２（互いに異なる視点から見た物体２）の特徴を表す複数のテンプレートが記憶される。

　ロボット制御装置５０は、物体２の位置姿勢の認識結果に基づき、ロボット６０の姿勢（位置姿勢）を制御する。ロボット制御装置５０は、ロボット６０の把持部６１によって把持された把持物体３が物体２に接続するように、ロボット６０の姿勢を制御する。

　ロボット６０は、ロボット制御装置５０によって姿勢が制御される。ロボット６０は、把持物体３を把持する把持部６１と、ロボット６０の端部を固定する土台６２を有する。従って、ロボット６０は、把持物体３の位置姿勢を制御することができる。また、把持部６１の一部において、撮像センサ１０が固定（接続）される。

（テンプレート生成装置の内部構成）
　図２を参照して、テンプレート生成装置２０の内部構成を説明する。テンプレート生成装置２０は、制御部２０１、情報取得部２０２、投影画像生成部２０３、特徴量算出部２０４、テンプレート生成部２０５、情報出力部２０６を有する。

　制御部２０１は、非一時的に記憶媒体に記憶されたプログラムに従って、テンプレート生成装置２０の各機能部を制御する。

　情報取得部２０２は、物体２の３次元モデル（３次元データ；３次元画像データ）を取得する。情報取得部２０２は、物体２の３次元モデルを、記憶装置４０から取得してもよいし、他の外部装置から取得してもよい。また、情報取得部２０２は、撮像センサ１０の撮像パラメータ（カメラ焦点距離、画像の中心座標、レンズ歪みの補正係数）を取得する。

　投影画像生成部２０３は、３次元モデルを２次元画像に変換することによって投影画像を生成する。具体的には、投影画像生成部２０３は、各姿勢の物体２を２次元画像によって表した投影画像を生成する。このとき、撮像センサ１０の撮像パラメータによって計測画像が変化するため、投影画像生成部２０３は、撮像パラメータによって補正した投影画像を生成する。

　特徴量算出部２０４は、３次元モデルまたは投影画像に基づき、投影画像における各画素（各領域）の特徴量を算出する。ここで、特徴量とは、エッジ特徴量（エッジ方向ヒストグラム）や、法線特徴量（法線方向ヒストグラム）でありえる。しかし、これに限らず、特徴量は、距離情報、温度情報、色彩情報であってもよい。

　テンプレート生成部２０５は、特徴量算出部２０４が算出した特徴量を各画素が示す２次元画像であるテンプレートを生成する。また、テンプレート生成部２０５は、テンプレートの基となった投影画像に対応する物体２の姿勢の情報を、当該テンプレートに付加する。

　情報出力部２０６は、テンプレート生成部２０５が生成したテンプレートを記憶装置４０に出力する。

　なお、本実施形態では、投影画像生成部２０３は、３次元モデルから複数の投影画像を生成する。このとき、複数の投影画像は、互いに異なる姿勢の物体２を２次元画像によって表した画像である。その後、特徴量算出部２０４は、複数の投影画像のそれぞれに対して、特徴量を算出する。そして、テンプレート生成部２０５は複数の投影画像のそれぞれについてテンプレートを生成して、情報出力部２０６は、複数のテンプレートを記憶装置４０に出力する。

（認識装置の内部構成）
　図２を参照して、認識装置３０の内部構成について説明する。認識装置３０は、制御部３０１、画像取得部３０２、情報取得部３０３、特徴量算出部３０４、範囲設定部３０５、推定部３０６、照合部３０７、認識部３０８、結果出力部３０９を有する。

　制御部３０１は、非一時的に記憶媒体に記憶されたプログラムに従って、認識装置３０の各機能部を制御する。

　画像取得部３０２は、撮像センサ１０から計測画像を取得する。なお、画像取得部３０２は、撮像センサ１０から計測画像を取得する必要はなく、例えば、記憶装置４０に記憶された計測画像を取得してもよい。

　情報取得部３０３は、記憶装置４０から複数のテンプレートを取得する。

　特徴量算出部３０４は、計測画像の各画素（各領域）の特徴量を算出する。

　範囲設定部３０５は、物体２が存在する可能性のある範囲（推定部３０６が推定した推定範囲）を、撮像センサ１０が計測する対象の範囲（計測範囲）に設定する。

　推定部３０６は、過去における物体２の位置姿勢に基づき、現在における物体２の位置姿勢を推定する。なお、推定部３０６は、時間および工数のかかるテンプレートを用いた照合を実行しない。このため、推定部３０６は、テンプレートを用いた照合によって物体２の位置姿勢を推定する場合よりも高速に、物体２の位置姿勢を推定できる。また、推定部３０６は、推定した物体２の位置または位置姿勢から、物体２が存在する可能性のある範囲（推定範囲）を推定する。推定範囲は、例えば、推定した物体２の位置を中心とした所定の大きさの直方体状、円柱状または球状などの任意の形状の範囲である。

　照合部３０７は、情報取得部３０３が取得したテンプレートと、計測画像との照合を行う。そして、照合部３０７は、合致度が所定値よりも大きければ、照合が成功したと判定する。一方、照合部３０７は、合致度が所定値よりも以下であれば、照合が失敗したと判定する。また、照合部３０７は、照合が成功したと判定された際のテンプレートから、大まかな物体２の位置姿勢を推定（認識）する。具体的には、照合部３０７は、テンプレートに付加された物体２の姿勢の情報と、計測時のロボット６０の姿勢の情報とに基づき、物体２の位置姿勢を推定することができる。

　認識部３０８は、推定部３０６または照合部３０７が推定した物体２の位置姿勢を３次元モデルの物体２の位置姿勢の初期値として、３次元モデルの物体２を計測画像（計測データ）に現れる物体２に合わせ込む。この合わせ込みは、例えば、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ　Ｃｌｏｓｅｓｔ　Ｐｏｉｎｔ）アルゴリズムを用いた３次元モデルの物体２と計測画像の物体２との各点の比較によって実現できる。認識部３０８は、３次元モデルを計測画像に合わせ込んだ結果に応じて、計測画像における詳細な物体２の位置姿勢を認識する。

　結果出力部３０９は、照合結果や物体２の位置姿勢の認識結果を記憶装置４０や外部装置に出力する。物体２を駆動するためのロボット制御装置５０などに結果出力部３０９が認識結果を出力すれば、ロボット制御装置５０は、物体２の位置姿勢に応じて、ロボット６０（把持物体３）を所定の位置姿勢にするための制御を行うことができる。

　また、テンプレート生成装置２０および認識装置３０は、例えば、ＣＰＵ（プロセッサ）、メモリ、ストレージなどを備えるコンピュータにより構成することができる。その場合、図２に示す構成は、ストレージに格納されたプログラムをメモリにロードし、ＣＰＵが当該プログラムを実行することによって実現されるものである。かかるコンピュータは、パーソナルコンピュータ、サーバコンピュータ、タブレット端末、スマートフォンのような汎用的なコンピュータでもよいし、オンボードコンピュータのように組み込み型のコンピュータでもよい。あるいは、図２に示す構成の全部または一部を、ＡＳＩＣやＦＰＧＡなどで構成してもよい。あるいは、図２に示す構成の全部または一部を、クラウドコンピューティングや分散コンピューティングにより実現してもよい。

［物体の位置姿勢の認識処理］
　図３のフローチャートを用いて、認識装置３０が物体２の位置姿勢を認識する認識処理を説明する。図３のフローチャートの各処理は、制御部３０１がプログラムを実行することによって実現できる。

　ステップＳ１００１において、制御部３０１は、画像取得部３０２を制御して、撮像センサ１０から左右画像（左レンズを介して被写体を撮像した画像と右レンズを介して被写体を撮像した画像）を取得する。また、制御部３０１は、情報取得部３０３を制御して、記憶装置４０から複数のテンプレートおよび物体２の３次元モデルを取得する。ここで、複数のテンプレートはそれぞれ、物体２の互いに異なる姿勢に対応するテンプレート（多視点テンプレート）である。なお、制御部３０１は、撮像センサ１０から撮像パラメータを取得してもよい。

　ステップＳ１００２において、制御部３０１は、過去（過去のフレーム）における物体２の位置姿勢の認識結果が記憶装置４０に記憶されているか否かを判定する。なお、過去（過去のフレーム）における物体２の位置姿勢の認識結果は、認識装置３０の不図示の記憶部に記憶されていてもよく、制御部３０１は、過去における物体２の位置姿勢の認識結果が記憶部に記憶されているか否かを判定してもよい。過去における物体２の位置姿勢の認識結果が記憶されていると判定された場合にはステップＳ１００６に進み、そうでない場合にはステップＳ１００３に進む。

　ステップＳ１００３において、制御部３０１は、左右画像の重複する全ての範囲について距離を計測するように撮像センサ１０を制御して、計測画像を取得する（３次元計測をする）。具体的には、撮像センサ１０は、左右画像の２つの間の対応する画素を特定して、対応する画素の位置の差を算出する。そして、撮像センサ１０は、対応する画素の位置の差と、左右のレンズの位置の差とに基づき、三角測量の技術を用いて、被写体までの距離を計測する。これによって、撮像センサ１０は、例えば、点群データを有するような距離画像である計測画像を取得できる。

　ステップＳ１００４において、制御部３０１は、特徴量算出部３０４を制御して、計測画像から特徴量を取得する。

　ステップＳ１００５において、制御部３０１は、照合部３０７を制御して、ステップＳ１００１において取得した複数のテンプレートを用いて、物体２の大まかな位置姿勢を推定（認識）する。具体的には、まず、照合部３０７は、複数のテンプレートが示す特徴量と計測画像が示す特徴量とを比較することによって、照合を行う。例えば、照合部３０７は、計測画像の或る範囲とテンプレートとにおける各画素間での特徴量の差分Ｄの総合計Ｓｕｍ（Ｄ）の逆数１／Ｓｕｍ（Ｄ）を合致度として取得する。なお、これに限らず、任意の照合方法によって、合致度の取得が行われてよい。そして、照合部３０７は、合致度が最も高くなるテンプレートに対応する姿勢と、ロボット６０の姿勢（撮像センサ１０の位置姿勢）とに基づき、物体２の姿勢を推定する。また、照合部３０７は、計測画像において当該テンプレートと最も合致する位置と、ロボット６０の姿勢とに基づき、物体２の位置を推定する。

　ステップＳ１００６において、制御部３０１は、過去における物体２の位置姿勢の情報（過去情報）を用いた認識処理を実行する。過去情報を用いた認識処理の詳細については、図４のフローチャートを用いて後述する。

　ステップＳ１００７において、制御部３０１は、ステップＳ１００６における認識処理によって物体２の位置姿勢を認識できたか否かを判定する。物体２の位置姿勢を認識できたと判定された場合にはステップＳ１００９に進み、そうでない場合にはステップＳ１００３に進む。

　ステップＳ１００８において、制御部３０１は、認識部３０８を制御して、物体２の３次元モデルと、計測画像との比較をして、計測画像における物体２の位置姿勢を詳細に認識する。具体的には、認識部３０８は、照合部３０７が推定した位置姿勢を初期値として、物体２の３次元モデルを計測画像に合わせ込むことにより、計測画像における物体２の位置姿勢を認識する。認識部３０８は、例えば、ＩＣＰアルゴリズムを用いて、３次元モデルと計測画像が示す物体２の各点の比較（マッチング）を行うことによって、物体２の３次元モデルを計測画像に合わせ込むことができる。

　ステップＳ１００９において、制御部３０１は、結果出力部３０９を制御して、物体２の位置姿勢の情報を、記憶装置４０に出力する。これによって、物体２の位置姿勢の情報が記憶装置４０に記憶される。

（過去情報を用いた認識処理；ステップＳ１００６）
　図４のフローチャートを用いて、ステップＳ１００６において行われる過去情報を用いた認識処理について説明する。図４のフローチャートの各処理は、制御部３０１がプログラムを実行することによって実現できる。

　ステップＳ２００１において、制御部３０１は、推定部３０６を制御して、過去における物体２の位置姿勢の認識結果に基づき、計測画像の過去の２つのフレーム間の物体２の位置姿勢の変化量（移動量および回転量）を算出する。具体的には、推定部３０６は、２つのフレーム間の物体２の位置姿勢の差分を変化量として算出すればよい。なお、推定部３０６は、３以上の時点の物体の位置姿勢の認識結果に基づき、フレーム間の物体２の位置姿勢の変化量を算出してもよい。

　ステップＳ２００２において、制御部３０１は、推定部３０６を制御して、計測画像の過去の２つのフレーム間の物体２の変化量に基づき、前回フレームから現在フレームの間での物体２の変化量（推定変化量；移動量および回転量）を推定する。具体的には、推定部３０６は、例えば、過去の２つのフレーム間が複数フレーム分離れている場合には、計測画像の過去の２つのフレーム間の物体２の変化量を、当該過去の２つのフレーム間のフレーム数で除算することによって、推定変化量を算出（推定）する。

　ステップＳ２００３において、制御部３０１は、推定部３０６を制御して、前回フレームの物体２の位置姿勢と、前回フレームから現在フレームの間での物体２の推定変化量とに基づき、現在フレームの物体２の位置姿勢を推定する。推定部３０６は、前回フレームの物体２の位置姿勢から、推定変化量の分だけ動いたと推定して、現在フレームの物体２の位置姿勢を推定する。このように、ステップＳ２００３では、推定部３０６は、ステップＳ１００５におけるテンプレートを用いた照合による物体２の位置姿勢の推定の代わりに、過去の物体２の位置姿勢に基づき、現在の物体２の位置姿勢を推定する。

　ステップＳ２００４において、制御部３０１は、推定部３０６を制御して、推定した現在の物体の位置姿勢に基づき、現在フレームにおいて物体２が存在する可能性がある範囲（推定範囲）を推定する。例えば、推定部３０６は、ステップ２００３において推定した物体２の位置を中心とする所定の大きさ（例えば、物体２の１．５倍や２倍の大きさ）の範囲を推定領域として決定する。そして、制御部３０１は、範囲設定部３０５を制御して、推定領域を、撮像センサ１０が計測する範囲である計測範囲に設定する。

　ステップＳ２００５において、制御部３０１は、撮像センサ１０を制御して、ステップＳ２００４において設定した計測範囲（推定範囲）のみを計測するように制御して、計測画像を取得する。これによれば、物体２が存在する可能性がある計測範囲のみが計測されるため、撮像センサ１０による計測が効率化できる。

　具体的には、撮像センサ１０は、左右画像のうちの計測範囲に対応する範囲においてのみ、対応する画素の位置の差を算出することによって、物体２の存在する可能性がある計測範囲のみを計測する。このため、左右画像のうち画素の位置の差の算出処理の対象となる画素の数を減らすことができる。また、撮像センサ１０からの計測すべき距離が限定されるため、一方の画像の画素に対する他方の画像の画素の対応位置を探索する範囲が絞られる。つまり、左右画像間の対応する画素の算出に係る処理数も減少する。このように、撮像センサ１０は、物体２が存在する可能性がある位置を含む所定の範囲についてのみ、計測を実行することができる。つまり、撮像センサ１０では、物体２が存在する位置から遠い位置について計測することが不要になる。

　ステップＳ２００６において、制御部３０１は、認識部３０８を制御して、物体２の３次元モデルと、計測画像との比較をして、物体２の位置姿勢を詳細に認識する。具体的には、認識部３０８は、照合部３０７が推定した位置姿勢を初期値として、物体２の３次元モデルを計測画像に合わせ込むことにより、計測画像における物体２の位置姿勢を認識する。認識部３０８は、ＩＣＰアルゴリズムを用いて、物体２の３次元モデルと計測画像が示す物体２の各点の比較（マッチング）を行うことによって、物体２の３次元モデルを計測画像に合わせ込むことができる。

　なお、ステップＳ２００１～Ｓ２００６の処理のうちいずれかが完了できなかった所定の場合には、図４のフローチャートの処理が終了し、物体２の位置姿勢を認識できなかったとして、ステップＳ１００７においてステップＳ１００３に進む。所定の場合とは、例えば、過去の１つのフレームの物体２の位置姿勢のみが記憶されており、ステップＳ２００１において過去フレーム間の物体２の変化量が算出できない場合であり得る。また、所定の場合とは、計測範囲（推定範囲）内に物体２が存在しなかったことに起因して、ステップＳ２００６において物体２の３次元モデルを計測画像に合わせ込むことができなかった場合であり得る。

　本実施形態によれば、過去における物体２の位置姿勢に基づき、大まかな物体２の位置姿勢を推定できるため、テンプレートと計測画像との照合処理をなくすことができる。従って、物体２の位置姿勢を認識する場合において、認識処理を効率化することができる。

＜実施形態２＞
　実施形態２では、ロボット６０の位置姿勢に基づき、現在における物体２の位置姿勢を推定する認識システム１を説明する。実施形態２に係る認識システム１の構成は、実施形態１に係る認識システム１に係る構成と同様であるため、説明を省略する。実施形態２では、図２に示すステップＳ１００６の処理（過去情報を用いた認識処理）のみが、実施形態１と異なるので、図５に示すフローチャートを用いて過去情報を用いた認識処理を説明する。

　ここで、認識システム１における座標系について説明する。認識システム１において、図６に示すように、ベース座標系、オブジェクト座標系、ツール座標系、カメラ座標系の４つの座標系が存在する。ベース座標系は、ロボット６０の土台６２を中心とする座標系であり、固定された座標系である。オブジェクト座標系は、物体２を中心とする座標系である。ツール座標系は、ロボット６０の把持部６１（または把持物体３）を中心とする座標系である。カメラ座標系は、撮像センサ１０を中心とする座標系である。ここで、実施形態１に係る「物体２の位置姿勢」とは、カメラ座標系の物体２の位置姿勢であってもよいし、ベース座標系の物体２の位置姿勢であってもよい。

　図５は、実施形態２に係る過去情報を用いた認識処理を示すフローチャートである。図５において、ステップＳ２００４～Ｓ２００６の処理は、実施形態１に係るステップＳ２００４～Ｓ２００６の処理と同様である。このため、ステップＳ２００４～Ｓ２００６についての説明は省略する。

　ステップＳ３００１において、制御部３０１は、推定部３０６を用いて、カメラ座標系の過去フレームの物体２の位置姿勢と、ベース座標系の過去フレームのロボット６０（把持部６１）の位置姿勢とに基づき、カメラ座標系の過去フレームの物体２の位置姿勢をベース座標系に変換する。具体的には、制御部３０１は、下記の式１によって、過去フレームのベース座標系の物体２における位置（並進）Ｔ’_{ｏｂｊ－ｂａｓｅ}と回転（姿勢）Ｒ’_{ｏｂｊ－ｂａｓｅ}を取得する。

　式１において、位置Ｔ’_{ｏｂｊ－ｃａｍ}と回転Ｒ’_{ｏｂｊ－ｃａｍ}は、それぞれカメラ座標系の過去フレームの物体２の位置（座標）と回転（姿勢）である。なお、位置Ｔ’_{ｏｂｊ－ｃａｍ}と回転Ｒ’_{ｏｂｊ－ｃａｍ}は、認識装置３０による過去の物体２の位置姿勢の認識結果である。位置Ｔ’_{ｔｏｏｌ－ｂａｓｅ}と回転Ｒ’_{ｔｏｏｌ－ｂａｓｅ}は、それぞれベース座標系の過去フレームの把持部６１（ロボット６０）の位置と回転である。位置Ｔ_{ｃａｍ－ｔｏｏｌ}と回転Ｒ_{ｃａｍ－ｔｏｏｌ}は、それぞれツール座標系の撮像センサ１０の位置と回転である。なお、撮像センサ１０と把持部６１との相対的な位置関係は一定であるため、位置Ｔ_{ｃａｍ－ｔｏｏｌ}と回転Ｒ_{ｃａｍ－ｔｏｏｌ}は、過去フレームと現在フレームとを問わず、一定の値である。そのため、位置Ｔ_{ｃａｍ－ｔｏｏｌ}と回転Ｒ_{ｃａｍ－ｔｏｏｌ}は、事前に計測して取得することができる。

　ここで、オブジェクト座標系からベース座標系への変換ベクトルをｌとし、オブジェクト座標系からカメラ座標系への変換ベクトルをｍとする。そして、ツール座標系からカメラ座標系への変換ベクトルをｎとし、ベース座標系からツール座標系への変換ベクトルをｏとする。すると、ｌ＝ｍ＋ｎ^－１＋ｏ^－１という式が成り立つ。これを、位置および回転によって表すと、式２のように表すことができる。これによって、式１の計算式が算出できる。

　ステップＳ３００２において、制御部３０１は、推定部３０６を用いて、ステップＳ３００１にて算出した過去フレームのベース座標系の物体２の位置姿勢と、ツール座標系の現在フレームのロボット６０（土台６２）の位置姿勢とに基づき、カメラ座標系の現在フレームの物体２の位置姿勢を推定する。具体的には、制御部３０１は、下記の式３によって、過去フレームのカメラ座標系の物体２における位置Ｔ_{ｏｂｊ－ｃａｍ}と回転（姿勢）Ｒ’_{ｏｂｊ－ｃａｍ}を取得する。

　位置Ｔ_{ｂａｓｅ－ｔｏｏｌ}と回転Ｒ_{ｂａｓｅ－ｔｏｏｌ}は、それぞれツール座標系の現在フレームの土台６２（ロボット６０）の位置（座標）および回転（姿勢）である。位置Ｔ_{ｔｏｏｌ－ｃａｍ}と回転R_{ｔｏｏｌ－ｃａｍ}は、それぞれカメラ座標系の把持部６１の位置および回転である。なお、撮像センサ１０と把持部６１との相対的な位置関係は一定であるため、位置Ｔ_{ｔｏｏｌ－ｃａｍ}と回転R_{ｔｏｏｌ－ｃａｍ}は、過去フレームと現在フレームとを問わず、一定の値である。そのため、位置Ｔ_{ｔｏｏｌ－ｃａｍ}と回転R_{ｔｏｏｌ－ｃａｍ}は、事前に計測して取得することができる。

　このように、ステップＳ３００１およびＳ３００２によって、推定部３０６は、現在フレームの物体２の位置姿勢を推定することができる。ここで、実施形態１では、推定部３０６は、現在フレームの物体の位置姿勢を推定するために、過去の複数のフレーム（２つの時点）の物体２の位置姿勢を把握している必要がある。一方、実施形態２によれば、推定部３０６は、過去の１つのフレームにおいて物体２やロボット６０の位置姿勢が把握できていれば、現在フレームの物体２の位置姿勢を推定することができる。つまり、推定部３０６は、１つの時点での計測によって、現在フレームの物体２の位置姿勢を推定することができる。

　従って、実施形態２では、認識システム１は、実施形態１よりも効率的に、物体２の位置姿勢を認識する場合において、認識処理を実行することができる。

　なお、実施形態１および実施形態２では、認識システム１は、図１に示すように、移動する撮像センサ１０によって計測した計測画像に基づき、物体２の位置姿勢を推定して、推定結果に応じて物体２の位置姿勢を認識する。しかし、図７に示すように、撮像センサ１０の位置が固定されており、物体２の位置姿勢が時刻経過に応じて変化する場合にも、各実施形態に係る認識システム１を適用することができる。つまり、このような場合にも、認識システム１は、実施形態１または実施形態２に説明した方法（推定方法：認識方法）によって、過去の物体２の位置姿勢に基づき、現在の物体２の位置姿勢を推定することができる。

　また、各実施形態に係る認識システム１（推定部３０６）は、過去の物体２の姿勢位置に加えて、物体２に対する設定（移動速度や加速度、動作軌跡の設定情報）をさらに用いて、現在の物体２の位置姿勢を推定してもよい。さらに、認識システム１（推定部３０６）は、ロボット６０の姿勢変化の設定（関節のエンコーダ値や加速度、動作軌跡の設定情報）に基づき、現在の物体２の位置姿勢を推定してもよい。

　なお、実施形態に記載された事項のみによって特許請求の範囲の記載の解釈が限定されるものではない。特許請求の範囲の記載の解釈には、出願時の技術常識を考慮した、発明の課題が解決できることを当業者が認識できるように記載された範囲も含む。

　（付記１）
　所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識装置（３０）であって、
　過去において前記認識装置（３０）が認識した前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定手段（３０６）と、
　前記推定手段（３０６）が推定した位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識手段（３０８）と、
を有することを特徴とする認識装置（３０）。

　（付記２）
　所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識方法であって、
　過去において認識された前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定ステップ（Ｓ２００１，Ｓ２００２，Ｓ２００３）と、
　前記推定ステップにおいて推定された位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識ステップ（Ｓ２００６）と、
を有することを特徴とする認識方法。

１：認識システム、２：物体、３：把持物体、
１０：撮像センサ、２０：テンプレート生成装置、３０：認識装置、
４０：記憶装置、５０：ロボット制御装置、６０：ロボット、
６１：把持部、６２：土台、
２０１：制御部、２０２：情報取得部、２０３：投影画像生成部、
２０４：特徴量算出部、２０５：テンプレート生成部、２０６：情報出力部、
３０１：制御部、３０２：画像取得部、３０３：情報取得部、
３０４：特徴量算出部、３０５：範囲設定部、３０６：推定部、
３０７：照合部、３０８：認識部、３０９：結果出力部

Claims (9)

1. 　所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識装置であって、
   　過去において前記認識装置が認識した前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定手段と、
   　前記推定手段が推定した位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識手段と、
   を有することを特徴とする認識装置。
2. 　前記計測範囲を設定する設定手段をさらに有し、
   　前記推定手段は、さらに、前記推定手段が推定した位置または位置姿勢に基づき前記所定の物体が存在する範囲である推定範囲を推定し、
   　前記設定手段は、前記推定範囲を前記計測範囲として設定する、
   　ことを特徴とする請求項１に記載の認識装置。
3. 　前記推定手段は、過去の少なくとも２つの時点間における前記所定の物体の位置姿勢の変化に基づき、現在における前記所定の物体の位置姿勢を推定する、
   　ことを特徴とする請求項１または２に記載の認識装置。
4. 　前記計測画像は、撮像センサによって計測された結果を表し、
   　前記撮像センサの位置姿勢は、ロボットによって制御され、
   　前記推定手段は、過去における前記所定の物体の位置姿勢と現在におけるロボットの位置姿勢とに基づき、現在における前記所定の物体の位置姿勢を推定する、
   　ことを特徴とする請求項１から３のいずれか１項に記載の認識装置。
5. 　前記計測画像は、被写体までの距離を各画素が表す距離画像である、
   　ことを特徴とする請求項１から４のいずれか１項に記載の認識装置。
6. 　前記過去における前記所定の物体の位置姿勢の前記認識手段による認識結果が存在しない場合には、
   　　前記推定手段は、現在における前記所定の物体の位置姿勢を推定せず、
   　　前記認識手段は、前記所定の物体の特徴量を示す複数のテンプレートのそれぞれと前記計測画像との照合結果に応じて前記所定の物体の位置姿勢を認識する、
   　ことを特徴とする請求項１から５のいずれか１項に記載の認識装置。
7. 　請求項１から６のいずれか１項に記載の認識装置と、
   　前記計測範囲を計測する撮像センサと、
   　前記認識装置が認識した結果に基づき、前記所定の物体の位置姿勢を制御するロボットと、
   を有することを特徴とするロボット制御システム。
8. 　所定の物体を含む計測範囲を計測した結果を表す計測画像と前記所定の物体の３次元モデルとを比較することによって、前記計測画像における前記所定の物体の位置姿勢を認識する認識方法であって、
   　過去において認識された前記所定の物体の位置姿勢に基づき、現在における前記所定の物体の位置姿勢を推定する推定ステップと、
   　前記推定ステップにおいて推定された位置姿勢を前記所定の物体の前記３次元モデルの位置姿勢の初期値として、前記３次元モデルを前記計測画像における前記所定の物体に合わせ込んだ結果に応じて、前記計測画像における前記所定の物体の位置姿勢を認識する認識ステップと、
   を有することを特徴とする認識方法。
9. 　請求項８に記載の認識方法の各ステップをコンピュータに実行させるためのプログラム。

Images