(57)【特許請求の範囲】
【請求項１】
固定部と内部自由度を有する可変部とを含み長手軸を有する３次元モデルを示す３次元モデル情報を保持するモデル保持部と、
認識対象物の３次元形状を示す認識対象情報を取得する認識対象情報取得部と、
前記認識対象情報と前記３次元モデル情報とに基づき、前記長手軸に対して対称な２つの候補姿勢をつくり、該２つの候補姿勢いずれに関しても、前記認識対象物と、前記可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして前記認識対象物の位置および姿勢を特定する認識対象物特定部と、
を備えることを特徴とする形状認識装置。
【請求項２】
前記認識対象物の前記可変部の候補となる部位が延長されている場合に、該可変部の候補となる部位の固定部側端部の部位を切り出す端部切出部をさらに備えることを特徴とする請求項１の形状認識装置。
【請求項３】
前記端部切出部は、前記可変部の任意の点を中心とする、該可変部の延長方向に垂直な断面の任意の２つの点間の直線距離の最大値以上の半径を有する円によって２つに分断される部位を切り出すことを特徴とする請求項２に記載の形状認識装置。
【請求項４】
前記認識対象物特定部は、前記認識対象物における前記３次元モデルの固定部にマッチングする部位を特定し、その特定した部位の位置および姿勢を用いて、前記認識対象物と、前記可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして前記認識対象物の位置および姿勢を特定することを特徴とする請求項１から３のいずれか１項に記載の形状認識装置。
【請求項５】
固定部と内部自由度を有する可変部とを含み長手軸を有する３次元モデルを示す３次元モデル情報を予め保持し、
認識対象物の３次元形状を示す認識対象情報を取得し、
前記認識対象情報と前記３次元モデル情報とに基づき、前記長手軸に対して対称な２つの候補姿勢をつくり、該２つの候補姿勢いずれに関しても、前記認識対象物と、前記可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして前記認識対象物の位置および姿勢を特定することを特徴とする形状認識方法。
【請求項６】
コンピュータを、
認識対象物の３次元形状を示す認識対象情報を取得する認識対象情報取得部と、
前記認識対象情報と、固定部と内部自由度を有する可変部とを含むみ長手軸を有する３次元モデルを示す３次元モデル情報とに基づき、前記長手軸に対して対称な２つの候補姿勢をつくり、該２つの候補姿勢いずれに関しても、前記認識対象物と、前記可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして前記認識対象物の位置および姿勢を特定する認識対象物特定部と、
して機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、任意の認識対象物の位置および姿勢を認識可能な形状認識装置、形状認識方法、および、そのプログラムに関する。
【背景技術】
【０００２】
ファクトリーオートメーションにおけるアセンブリ処理では、ケーブルを用いて複数の電子機器同士の接続作業も実行される。このようなケーブルの接続作業を、産業用のロボットを用いて行うことを試みる場合、ロボットは、ケーブル終端のコネクタやその付近のケーブルを自律的に認識し、そのコネクタの位置や姿勢を特定して把持（ピッキング）しなければならない。
【０００３】
しかし、現行の技術では、ケーブルを除く、単純な形状や特別な着色を施したコネクタ単体しか、その形状を認識することができない。また、コネクタが単純な形状や特別な着色で構成されていたとしても、ケーブルが不規則に収容され、コネクタの一部をケーブルが覆ってしまっていたり、環境光により着色した色を正しく認識できない場合、コネクタを的確に特定することができなかった。さらに、通常、ケーブルには様々な模様が示されており、その模様（テクスチャ）をケーブルやコネクタのエッジと誤認識することもあった。このようにコネクタを的確に認識できないと、コネクタが未接続の状態となったり、誤った接続位置にコネクタを誤接続してしまうおそれがある。
【０００４】
コネクタの未接続や誤接続を防止する技術としては、例えば、コネクタ付きケーブルのコネクタ部分にＲＦ−ＩＤタグを内蔵させ、対応するジャック部品に読み取り回路を配置することで、コネクタ付きケーブルとジャック部品との接続管理を行う技術が開示されている（例えば、特許文献１）。また、コネクタ付きケーブルのうち、ケーブルの任意の点に設けられた加圧スイッチを押止することで、コネクタまたはケーブルの所定の部分が発光する技術も知られている（例えば、特許文献２）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００４−３４９１８４号公報
【特許文献２】特開２００６−２２８４８２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、特許文献１の技術は、コネクタの接続関係を事後的に確認する技術であり、特許文献２の技術は、任意のケーブルを人が認識する技術であり、コネクタやケーブルの形状認識には応用できず、また、両技術は、コネクタやケーブルの加工を前提とし、既存のコネクタやケーブルには適用できなかった。
【０００７】
また、従来の技術では、コネクタを単体で認識せざるを得ず、コネクタから延長されるケーブルは物理的に切り離せないので形状認識の弊害となることもあり、コネクタの認識精度を高めることができなかった。
【０００８】
本発明は、このような課題に鑑み、コネクタ等の固定部にケーブル等の可変部を加えた３次元モデルを通じて認識対象物を的確に抽出可能な形状認識装置、形状認識方法、および、そのプログラムを提供することを目的としている。
【課題を解決するための手段】
【０００９】
上記課題を解決するために、本発明の形状認識装置は、固定部と内部自由度を有する可変部とを含み長手軸を有する３次元モデルを示す３次元モデル情報を保持するモデル保持部と、認識対象物の３次元形状を示す認識対象情報を取得する認識対象情報取得部と、認識対象情報と３次元モデル情報とに基づき、長手軸に対して対称な２つの候補姿勢をつくり、２つの候補姿勢いずれに関しても、認識対象物と、可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして認識対象物の位置および姿勢を特定する認識対象物特定部とを備えることを特徴とする。
【００１０】
形状認識装置は、認識対象物の可変部の候補となる部位が延長されている場合に、可変部の候補となる部位の固定部側端部の部位を切り出す端部切出部をさらに備えてもよい。
【００１１】
端部切出部は、可変部の任意の点を中心とする、可変部の延長方向に垂直な断面の任意の２つの点間の直線距離の最大値以上の半径を有する円によって２つに分断される部位を切り出してもよい。
【００１２】
認識対象物特定部は、認識対象物における３次元モデルの固定部にマッチングする部位を特定し、その特定した部位の位置および姿勢を用いて、認識対象物と、可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして認識対象物の位置および姿勢を特定してもよい。
【００１３】
上記課題を解決するために、本発明の形状認識方法は、固定部と内部自由度を有する可変部とを含み長手軸を有する３次元モデルを示す３次元モデル情報を予め保持し、認識対象物の３次元形状を示す認識対象情報を取得し、認識対象情報と３次元モデル情報とに基づき、長手軸に対して対称な２つの候補姿勢をつくり、２つの候補姿勢いずれに関しても、認識対象物と、可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして認識対象物の位置および姿勢を特定することを特徴とする。
【００１４】
また、上記課題を解決するために、コンピュータを、認識対象物の３次元形状を示す認識対象情報を取得する認識対象情報取得部と、認識対象情報と、固定部と内部自由度を有する可変部とを含み長手軸を有する３次元モデルを示す３次元モデル情報とに基づき、長手軸に対して対称な２つの候補姿勢をつくり、２つの候補姿勢いずれに関しても、認識対象物と、可変部の内部自由度を変化させた３次元モデルとをパターンマッチングして認識対象物の位置および姿勢を特定する認識対象物特定部として機能させるためのプログラムが提供される。
【発明の効果】
【００１５】
本発明によれば、コネクタ等の固定部にケーブル等の可変部を加えた３次元モデルを通じて認識対象物を的確に抽出することが可能となる。
【図面の簡単な説明】
【００１６】
【図１】ピッキングシステムの概略的な接続関係を示した説明図である。
【図２】認識対象物のパターンマッチングの概要を説明するための説明図である。
【図３】形状認識装置の概略的な構成を示した機能ブロック図である
【図４】固定部と可変部とを一体形成した３次元モデルを例示した説明図である。
【図５】端部切出部の動作を説明するための説明図である。
【図６】端部切出部の具体的な切り出し動作の一例を説明するための説明図である。
【図７】形状認識方法の処理の流れを示したフローチャートである。
【発明を実施するための形態】
【００１７】
以下に添付図面を参照しながら、本発明の好適な実施形態について詳細に説明する。かかる実施形態に示す寸法、材料、その他具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、本発明を限定するものではない。なお、本明細書及び図面において、実質的に同一の機能、構成を有する要素については、同一の符号を付することにより重複説明を省略し、また本発明に直接関係のない要素は図示を省略する。
【００１８】
（ピッキングシステム１００）
図１は、ピッキングシステム１００の概略的な接続関係を示した説明図である。ピッキングシステム１００は、３次元形状測定装置１１０と、形状認識装置１２０と、ロボット制御装置１３０と、ロボット１４０とを含んで構成される。本実施形態が対象とする認識対象物１１２は、モデル自体の変化を伴わない固定部のみからなるものに加え、固定部と内部自由度を有する可変部とが一体形成されたものに及ぶ。かかる内部自由度は、通常のパターンマッチングで用いられる固定部の外観そのもののスピン等ではなく、固定部に対する内部での相対的な変化を言う。ここでは、その理解を容易にするため、コネクタ（固定部）とコネクタから延長されるケーブル（可変部）とが一体形成された認識対象物１１２を用いて説明するが、認識対象物１１２がコネクタやケーブルに限定されないのは言うまでもない。このようなピッキングシステム１００における形状認識では、認識対象物１１２は球のような完全に全方向に対し対称なものを除き、一般的に形状の偏りにより、長手軸（長手方向）を有する。長手軸とは形状の広がりが大きい方向に沿った軸の事を言う。
【００１９】
３次元形状測定装置１１０は、例えば、レーザースキャナ方式の３次元距離センサで構成され、可変部としてのケーブルが不規則に折り曲げられた（とぐろを巻いた）状態で乱雑に収容されている１または複数の認識対象物（ワーク：ここではコネクタおよびケーブル）１１２の測定面（表面）における複数の検出点の３次元座標を検出する。
【００２０】
具体的に、３次元形状測定装置１１０としての３次元距離センサは、測定面上の複数の検出点の、３次元形状測定装置１１０からの距離を示すＺ軸座標と、検出方向に垂直な面（ＸＹ平面）上におけるＸ、Ｙ軸座標とを合わせて３次元座標とし、複数の検出点全ての３次元座標を示す情報である認識対象情報を生成する。こうして生成された認識対象情報は、形状認識装置１２０に送信される。ここでは、認識対象情報として、認識対象物上の複数の検出点における３次元座標を送信しているが、かかる場合に限られず、認識対象物の３次元形状を表す様々な情報を送信することができる。また、３次元形状測定装置１１０として、３次元距離センサを挙げているが、３次元形状が把握可能な既存の様々なセンサを採用することが可能である。
【００２１】
形状認識装置１２０は、３次元形状測定装置１１０から受信した認識対象情報に基づいて認識対象物１１２を抽出し、パターンマッチング等により、その認識対象物１１２が３次元モデル１２２で示される所望する認識対象物１１２であることを認識し、その認識対象物１１２の位置や姿勢を特定する。
【００２２】
ロボット制御装置１３０は、そのようにして認識された１の認識対象物１１２の位置や姿勢を示す情報を形状認識装置１２０から受信して、ロボット１４０に認識対象物１１２をピッキングさせる制御指令を発する。ロボット１４０は、その制御指令を受けて、不規則に収容された複数の認識対象物１１２から、指示された１の認識対象物１１２、例えば、コネクタやその付近のケーブルを、吸着式バンド１４２を通じてピッキングする。
【００２３】
従来のピッキングシステムでは、コネクタを単体で認識せざるを得ず、コネクタから延長されるケーブルは物理的に切り離せないので形状認識の弊害となることもあり、コネクタの認識精度を高めることができなかった。本実施形態では、コネクタにおけるケーブルとの接続位置やケーブルの形状も、コネクタを特徴付ける要素として用いることで、コネクタの位置や姿勢を高精度で特定する。
【００２４】
ただし、ケーブル等の可変部はフレキシブルに変化するので、延長されるケーブル全てに関してパターンマッチングを実行しようとすると、計算量が膨大になってしまい、結局マッチング不能となる問題が生じ得る。ここでは、その可変部の変化範囲（自由度）を制限することで、可変部の変化範囲をパターンマッチングのパラメータに取り込むことも目的としている。
【００２５】
図２は、認識対象物１１２のパターンマッチングの概要を説明するための説明図である。特に図２（ａ）は、認識対象物１１２がコネクタ単体であった場合、図２（ｂ）は、認識対象物１１２としてコネクタとケーブルの一部を用いた場合、図２（ｃ）は、図２（ｂ）の場合に、さらにケーブルを変化範囲内で変化させてパターンマッチングを行った場合を示している。
【００２６】
例えば、対称性があり長手軸を有する円柱形状のコネクタのみをイメージした認識対象物１１２と３次元モデル１２２とがあるとする。ただし、図２では認識対象物１１２および３次元モデル１２２が側面視で表されているので形状が矩形となっている。ここでは、理解を容易にするため、平行移動変換は既に行われており、認識対象物１１２であるコネクタの長手軸が、３次元モデル１２２の長手軸を図２（ａ）中時計回りに２７０°回転した状態（図２（ａ）における回転角度０°の状態）となっており、長手軸回りのスピン角度は考慮しないものとする。
【００２７】
パターンマッチングでは、まず、３次元モデル１２２を基準として、認識対象物１１２と３次元モデル１２２との一致度が高くなる方向（図２（ａ）中一致度の軌跡に沿った矢印で示す。）に、認識対象物１１２を回転する。そして、一致度が極値に落ち着くと、その点をパターンマッチングの収束点としている。
【００２８】
図２（ａ）では、認識対象物１１２が９０°の姿勢になった場合と、２７０°の姿勢になった場合の２つの姿勢で一致度が高い収束点（極値）を得ることができる。そのうち、２７０°の姿勢が正しい姿勢であり、９０°の姿勢は誤った姿勢である。ここでは、一致度を評価することで正しい姿勢を抽出することができる。
【００２９】
図２（ｂ）では、コネクタにケーブルの一部を加えた３次元モデル１２２と、それに相当する認識対象物１１２とのパターンマッチングを示している。ここでは、パターンマッチングの要素として、コネクタにおけるケーブルとの接続位置やケーブルの形状が含まれているので、より厳密にパターンマッチングが遂行され、極値となる２つの姿勢のうち少なくとも誤った姿勢（９０°）の一致度が低くなる。また、正しい姿勢（２７０°）においては、認識対象物１１２のケーブルの姿勢によっては一致度が高まり、誤った姿勢（９０°）との差分が大きくなるので、図２（ａ）と比べて、認識対象物１１２の位置や姿勢をより特定し易くなる。長手軸を有する認識対象物では、長手軸方向に、順方向、逆方向に揃った場合に一致度が高くなる。順方向、逆方向、どちらかが見つかった時に、それと逆向きの候補姿勢をつくり、２つの最終マッチング結果を比較すると、誤った姿勢を得るのを避ける事ができる。
【００３０】
さらに、図２（ｃ）では、コネクタにケーブルの一部を加えた３次元モデル１２２と、それに相当する認識対象物１１２とをパターンマッチングするのみならず、３次元モデル１２２のケーブル１１４部分を既知の折り曲げ範囲（変化範囲）で変化させ、認識対象物１１２に合わせ込んでいる。パターンマッチングの際、正しい姿勢（２７０°）以外では、ケーブルを折り曲げても一致度はあまり高まらないが、正しい姿勢（２７０°）に近づくと、ケーブルの折り曲げ変化によって一致度が顕著に向上する。こうして、正しい姿勢（２７０°）と誤った姿勢（９０°）との差分がさらに大きくなるので、認識対象物１１２の位置や姿勢をさらに特定し易くなる。
【００３１】
本実施形態の形状認識装置１２０は、３次元モデル１２２として固定部と可変部とを一体的に形成し、パターンマッチングすることで認識対象物１１２を的確に特定することが可能となる。以下では、形状認識装置１２０の具体的な構成を示し、その後、具体的な処理の流れ（形状認識方法）を詳述する。
【００３２】
（形状認識装置１２０）
図３は、形状認識装置１２０の概略的な構成を示した機能ブロック図である。形状認識装置１２０は、モデル保持部２１０と、データバッファ２１２と、中央制御部２１４とを含んで構成される。
【００３３】
モデル保持部２１０は、ＲＯＭ、不揮発性ＲＡＭ、フラッシュメモリ、ＨＤＤ(Hard Disk Drive)等で構成され、認識対象物１１２に対応する３次元モデル１２２を示す３次元モデル情報を保持する。本実施形態において、モデル保持部２１０は、固定部のみを有する３次元モデルを示す３次元モデル情報に加え、固定部と可変部とを一体形成した３次元モデル１２２を示す３次元モデル情報を保持する。
【００３４】
図４は、固定部と可変部とを一体形成した３次元モデル１２２を例示した説明図である。例えば、図４（ａ）では、フレキシブルな要素を有さないコネクタ２５０を固定部とし、矢印の変化範囲で円弧状に折り曲げ可能なケーブル２５２を可変部としている。また、図４（ｂ）の如く、コネクタ２５０には、コネクタ２５０をプラグとした場合の接続先であるレセプタクルへのロック機構２５４が設けられていることもあり、コネクタ２５０をレセプタクルに接続後、そのロック機構２５４を矢印の方向に回転させて脱落を防止している。かかるロック機構２５４も固定部に対して所定の回転範囲（変化範囲）を有する可変部として扱うことができる。
【００３５】
固定部に対する可変部の他の例としては、コネクタ（固定部）の他のラッチ機構（可変部）やスライドバー（可変部）、コネクタ（固定部）の蓋（可変部）の有無、その蓋の紛失防止用チェーン等の拘束部材（可変部）、電子機器（固定部）におけるボリューム、スイッチ、レセプタクルのロック機構（可変部）等、様々な認識対象物１１２を想定することができる。
【００３６】
データバッファ２１２は、ＳＲＡＭ、ＤＲＡＭ等で構成され、３次元形状測定装置１１０から受信した認識対象情報を一時的に保持する。
【００３７】
中央制御部２１４は、中央処理装置（ＣＰＵ）や信号処理装置（ＤＳＰ：Digital Signal Processor）、プログラム等が格納されたＲＯＭやメモリ、ワークエリアとしてのＲＡＭ等を含む半導体集積回路により、形状認識装置１２０全体を管理および制御する。また、本実施形態において、中央制御部２１４は、認識対象情報取得部２３０と、端部切出部２３２と、認識対象物特定部２３４としても機能する。
【００３８】
認識対象情報取得部２３０は、３次元形状測定装置１１０から任意の認識対象物１１２の３次元形状を示す認識対象情報を取得し、データバッファ２１２に保持させる。
【００３９】
端部切出部２３２は、認識対象物１１２の可変部の候補となる部位が延長されている場合に、可変部の候補となる部位の固定部側端部の部位（ラベル）を可変部として切り出す。ここで、ラベルは、連続かつスムーズに繋がる曲面を示す。本来、一連のラベルを特定した場合、そのラベル全てを可変部とすべきところ、ここでは、端部切出部２３２が、敢えて、その一連のラベルを分割し、固定部側端部の部位を示すラベルのみを可変部としている。
【００４０】
図５は、端部切出部２３２の動作を説明するための説明図である。ここでも、固定部としてコネクタ２５０を、可変部としてケーブル２５２を例に挙げている。ケーブル２５２の終端にはコネクタ２５０が接続されており、ケーブル２５２同士の重なりや潜り込みが少ない場合、図５（ａ）のように、一連のケーブル２５２としてみなされる部位（ラベル）が長く判断されることがある。しかし、ケーブル２５２のラベルが大きいと、可変部としての変化範囲も大きくなり計算が膨大となる。そこで、本実施形態では、認識対象物１１２と、図５（ｂ）に示す、固定部と可変部とを一体形成した３次元モデル１２２とパターンマッチングを実行するために、端部切出部２３２は、必要な可変部の部位、ここでは、図５（ｃ）のように、ケーブル２５２のコネクタ２５０（固定部）側端部の部位のみを切り出し、他を排除する。
【００４１】
こうすることで、変化範囲の大きいケーブルのラベルを、パターンマッチングの前段で候補から除外できる。また、３次元モデル１２２としても同等の大きさの可変部２５２を準備することで、パターンマッチングをより的確に行うことが可能となる。かかる構成により、計算処理の高速化やロバスト化を図ることができる。
【００４２】
可変部の候補となる部位を切り出す一例として、端部切出部２３２は、可変部の延長方向に垂直な断面の任意の２つの点間の直線距離の最大値に基づいて決定される円によって２つに分断される部位（３つに分断されない部位）を端部とみなして切り出す。
【００４３】
図６は、端部切出部２３２の具体的な切り出し動作の一例を説明するための説明図である。端部切出部２３２は、まず、図６（ａ）に示す切出位置を決定するための指標となる円２５６を設定する。かかる円２５６の半径は、可変部の延長方向に垂直な断面の任意の２つの点間の直線距離の最大値以上とする。ここで、可変部の延長方向に垂直な断面は、例えば、可変部が円柱形状で示される場合、その延長方向に垂直な断面（円）であり、任意の２つの点間の直線距離の最大値は、可変部が円柱形状で示される場合、その面（円）の直径となる。即ち、端部切出部２３２は、半径が、可変部の短手方向の幅以上となる円２５６を設定している。
【００４４】
そして、端部切出部２３２は、図６（ａ）のように、可変部としてのケーブル２５２を示すラベルの任意の点２５８を中心として円２５６を形成する。図６（ａ）の例では、円２５６によってケーブル２５２のラベルが（１）、（２）、（３）の３つに分断される。端部切出部２３２は、このようにケーブル２５２のラベルを３つに分断する円２５６の中心の点２５８をケーブルの端部とは判定しない。
【００４５】
端部切出部２３２は、任意の点２５８をシフトして、例えば、図６（ｂ）に示す円２５６を形成する。図６（ｂ）の例では、円２５６によってケーブル２５２のラベルが（１）、（２）の２つにしか分断されない。端部切出部２３２は、このようにケーブル２５２のラベルが３つに分断されない円２５６の中心の点２５８をケーブルの端部と判定する。ここでは、説明の便宜上、ケーブル２５２のあらゆる点２５８で端部であるか否か判定しているように記載したが、実際は、計算負荷軽減のため、固定部であるコネクタ２５０に近い部位から円２５６を順次形成し、分断される領域が２つから３つに変化した位置に基づいて切り出し範囲を決定している。しかし、端部の切り出し方法は、かかる場合に限られず、様々な既存の方法を採用することができる。
【００４６】
認識対象物特定部２３４は、認識対象情報と３次元モデル情報とに基づき、認識対象物１１２と、可変部の内部自由度を変化させた３次元モデル１２２とをパターンマッチングして認識対象物１１２の位置および姿勢を特定する。
【００４７】
ここで、認識対象物特定部２３４は、認識対象物１１２と３次元モデル１２２とのパターンマッチング（軸特定）において、いずれか一方を基準とし、他方の位置や姿勢を変更することとなるが、いずれを基準とするかは限定されない。本実施形態では、３次元モデル１２２の方が認識対象物１１２より情報量が多いので、計算負荷を軽減すべく、３次元モデル１２２を基準として認識対象物１１２の位置や姿勢を変更することとする。
【００４８】
尚、かかる軸特定には、ＩＣＰ（Iterative Closest Points）やＰＣＡ（Principal Component Analysis）等、様々な手法を用いることができるが、ここでは、その一例としてＩＣＰを用いる。ＩＣＰは、６自由度ある３次元モデル１２２の位置や姿勢を、認識対象物１１２と３次元モデル１２２とを特定するためのそれぞれの対応点間の距離の合計（ノルム）が閾値以下になるまで共役勾配法等を利用して反復的に更新する手法である。ＰＣＡは、入力されたデータが空間内においてどのように長手軸を持つかを調べる手法（主軸成分解析）である。
【００４９】
ＩＣＰは、例えば、ＰＣＡ等と異なり、認識対象物１１２と３次元モデル１２２との点群同士のマッチングを目的としているので、認識対象物１１２が重なること等による認識対象物１１２の部分的な欠落に強いため、ロバスト性に優れ、比較的高精度に認識対象物１１２の位置や姿勢を特定することが可能となる。
【００５０】
また、上述したように、認識対象物特定部２３４は、認識対象物１１２の可変部の内部自由度を、想定される変化範囲内で変化させつつ３次元モデル１２２とのパターンマッチングを遂行する。かかる内部自由度は、図４を用いて説明したように、折り曲げや可変部のみの回転等を含む。さらにケーブル等可変部の剛性モデルを加味することも可能である。このような可変部の変化範囲は、変化パラメータとして３次元モデル１２２と共にモデル保持部２１０に保持される。認識対象物特定部２３４は、３次元モデル１２２の後述する３次元点群データの各対応点の位置を変化させることで変化パラメータを収束させている。
【００５１】
このように、３次元モデル１２２の可変部の内部自由度を、変化範囲内で変化させつつ認識対象物１１２とのパターンマッチングを行うことにより、可変部の変化も加味することができるので、一致度を格段に高めることができる。また、規則性を有する等、変化範囲を限定できる部位のみを可変部として用いることで、計算負荷の増大を抑制することができ、適切な計算負荷の下、認識対象物１１２の的確な位置および姿勢を特定することが可能となる。
【００５２】
また、このようなマッチングによって得られたケーブル２５２等の折り曲げ角度は、ケーブル２５２等の延長方向の推定にも利用することが可能である。例えば、可変部の３次元モデル１２２としてフレキシブルな弾性管（例えばゴムホース）を登録しておき、さらに、そのモデルの節点と、その節点での接線の角度等をパラメータとして設定しておく。そうすると、その角度を調整することで３次元モデル１２２の折り曲げ度合いを変化させることができるので、弾性管の大まかな方向のみならず、要所での角度もマッチングさせて、弾性管のうねり姿勢等の認識も可能となる。
【００５３】
また、認識対象物特定部２３４は、認識対象物１１２が長手軸を有する場合、まず、その長手軸を特定し、その長手軸に沿って対称的（互いに逆向き）に配置した２つの姿勢候補を生成し、その２つの候補姿勢のいずれに関しても３次元モデル１２２とのパターンマッチングを行って、認識対象物１１２の位置や姿勢を特定するとしてもよい。
【００５４】
図２を用いて説明したように、長手軸を有する、コネクタや、コネクタとケーブルが一体形成された認識対象物１１２は、パターンマッチングにおいて２つ以上の極値を有する場合が多い。この場合に、認識対象物１１２と３次元モデル１２２との長手軸が合っていると、それが誤った姿勢であってもパターンマッチングの一致度が高くなる。
【００５５】
そこで、認識対象物特定部２３４は、パターンマッチングを２段階に分け、１段階目において、認識対象物１１２と３次元モデル１２２とのパターンマッチングの結果が収束し得る軸のみを特定し、特定した軸を基準とした２つの姿勢候補を生成し、２段階目において、その姿勢候補を用いてパターンマッチングを厳密に行う。具体的に、認識対象物特定部２３４は、認識対象物１１２を０°からＺ軸を中心に時計回りに回転し、その一致度が１つ目の極値（９０°）になる前の所定の閾値に達した時点で、一旦パターンマッチングを停止する。そして、認識対象物特定部２３４は、停止したときの認識対象物１１２を２段階目の姿勢候補とすると共に、さらに１８０°回転した認識対象物１１２も姿勢候補とする。続いて、認識対象物１１２の２つの姿勢候補に基づいて、厳密なパターンマッチングを並行して行う。
【００５６】
このような２つの姿勢候補は、図２の例では、それぞれ９０°と２７０°において極値を示すが、そのうち一致度が高い２７０°の姿勢が認識対象物１１２の真の姿勢であると判断される。ここでは、１つの長手軸に対して２つの姿勢候補があるので、そのいずれもパターンマッチングを行うことで、誤った姿勢にのみパターンマッチングが収束するのを回避している。
【００５７】
また、その２つの姿勢の関係、例えば、軸に沿って対称的（互いに逆向き）な関係にあることは把握されているので、軸の特定に関しては、いずれか一方の姿勢の軸を用いればよく、そのときに厳密なパターンマッチングを要さない。即ち、１段階目では、厳密なパターンマッチングと比較して緩い閾値で一方のみの軸を抽出すればよいこととなる。
【００５８】
ここでは、少なくとも一方の姿勢候補に関して、最適な収束点（極値）が導出されるので、他方の収束点を導出するまでもなく、パターンマッチングが完了する。したがって、認識対象物１１２の位置と姿勢とが的確に導出される。また、軸を抽出するまでのパターンマッチングを２つの極値それぞれについて実行せず、いずれか一方でのみ行うので、処理負荷の軽減を図ることができる。
【００５９】
さらに、認識対象物特定部２３４は、認識対象物１１２における３次元モデル１２２の固定部にのみマッチングする部位を特定し、その特定した部位の位置および姿勢を用いて、認識対象物１１２と可変部の内部自由度を変化させた３次元モデル１２２とを厳密にパターンマッチングして認識対象物１１２の位置および姿勢を特定するとしてもよい。
【００６０】
例えば、可変部としてのケーブル２５２があまりにもフレキシブルな場合、上述したコネクタ２５０とケーブル２５２とを一体形成した３次元モデル１２２のパターンマッチングのみでは、収束点を得ることが難しい場合がある。その場合、固定部であるコネクタ２５０のみの３次元モデルを独立して用い、コネクタ２５０のみのパターンマッチングを遂行して、コネクタ２５０の例えば長手軸のみを推定し、その長手軸に沿った姿勢を開始姿勢としてコネクタ２５０とケーブル２５２とを一体形成した３次元モデル１２２とのパターンマッチングを行うことで、認識対象物１１２の位置および姿勢を特定する。
【００６１】
ここでは、形状が歪みにくい固定部のパターンマッチングを事前に行うことで、コネクタ２５０とケーブル２５２とを一体形成した３次元モデル１２２の大凡の姿勢を推定することができ、可変部の変化範囲を限定することが可能となるので、処理負荷を軽減すると共に、認識対象物１１２の位置および姿勢を迅速に特定することが可能となる。
【００６２】
以上、説明した形状認識装置１２０では、固定部単体では対称性によってその姿勢を特定できない場合であっても、固定部における可変部との接続位置や可変部の形状を、固定部を特徴付ける要素として用いているので、認識対象物１１２の位置や姿勢を高精度で特定することが可能となる。さらに、固定部に可変部を加えた３次元モデル１２２と、それに相当する認識対象物１１２とをパターンマッチングするのみならず、フレキシブルな可変部を既知の変化範囲で変化させることで、位置および姿勢をより高精度に特定することが可能となる。
【００６３】
さらに、本実施形態では、パターンマッチング手法として、ＩＣＰを用いているので、様々な形状を有する認識対象物１１２に対しても位置や姿勢を特定できる。ＩＣＰでは、認識対象物１１２の一部が隠されていた場合においてもロバスト性が高いので有効な結果を得ることが可能となる。したがって、可変部としてのケーブルの一部を他の物体が覆ったとしても、ケーブルの進行方向を把握でき、ケーブルがとぐろ状に重なっていたとしても各ケーブルの先を推定することができる。
【００６４】
また、コンピュータによって形状認識装置１２０として機能するプログラムや、当該プログラムを記憶した記憶媒体も提供される。さらに、当該プログラムは、記憶媒体から読み取られてコンピュータに取り込まれてもよいし、通信ネットワークを介して伝送されてコンピュータに取り込まれてもよい。
【００６５】
（形状認識方法）
また、形状認識装置１２０を用いた形状認識方法も提供される。以下、このような形状認識方法を詳細に説明する。図７は、形状認識方法の処理の流れを示したフローチャートである。特に、図７（ａ）は形状認識方法の全体的な処理の流れを、図７（ｂ）はＩＣＰに関するサブルーチンを示している。ここで、モデル保持部２１０は３次元モデル情報を予め保持している。
【００６６】
本実施形態における固定部と可変部とを一体形成した３次元モデル１２２を用いたパターンマッチングは、固定部が対称性を有し、その方向を一義的に求めることができない場合や、固定部に接続されている可変部がフレキシブルに変化し、そのフレキシブルな変化部をピッキングしたい場合等に用いると効果的である。これに対して、固定部の形状が特徴的であり、可変部を特定する必要がない場合、固定部のみによる３次元モデル１２２によってパターンマッチングを実行することもできる。
【００６７】
認識対象情報取得部２３０は、３次元形状測定装置１１０から、任意の認識対象物１１２の３次元形状を示す認識対象情報を取得する（Ｓ３００）。そして、端部切出部２３２は、認識対象物１１２の可変部のラベルの大きさが所定の閾値より大きいか否か、即ち、可変部の候補となる部位が延長されているか否かを判定し（Ｓ３０２）、可変部のラベルの大きさが所定の閾値より大きい場合（Ｓ３０２におけるＹＥＳ）、可変部の候補となる部位の固定部側端部の部位（ラベル）を切り出す（Ｓ３０４）。このとき、可変部のラベルの大きさが所定の閾値以下であれば（Ｓ３０２におけるＮＯ）、そのまま認識対象物１１２の特定ステップＳ３０６に移行する。
【００６８】
認識対象物特定部２３４は、認識対象情報と３次元モデル情報とに基づいて、ＩＣＰによるパターンマッチングを行う（Ｓ３０６）。
【００６９】
図７（ｂ）を用いて認識対象物特定部２３４によるＩＣＰの処理の流れを簡単に説明する。３次元形状測定装置１１０から取得された認識対象情報には、認識対象物１１２の測定面における配列された複数の検出点に関する３次元座標（３次元点群データ）が示されている。３次元点群データの点の数は、認識対象物１１２の大きさと、パターンマッチングの要求精度に応じて決定される。ＩＣＰエンジンは、ＩＣＰの繰り返し処理を行う前に、このような認識対象情報中の３次元点群データに対し、床や支柱、コンテナ等の明らかな背景計測点があれば、それを除外する（Ｓ３５０）。
【００７０】
ＩＣＰエンジンは、初期姿勢を用意し、それに回転や並行移動変換などの姿勢変更処理を施す（Ｓ３５２）。姿勢変更処理は認識対象物１１２に対して施すことも可能であるし、３次元モデル１２２側に対して施すことも可能である。例えば、点数の少ないほうを動かして、計算処理を少なくしてもよい。続いて、姿勢変更処理後の認識対象物１１２と３次元モデル１２２との間でそれぞれ対応する点を求め（Ｓ３５４：ペアリング）、対応点間の距離合計（ノルム）を計算する（Ｓ３５６）。このノルムが小さいほうが形状の一致度合いが高いといえる。一致度合いが高かった姿勢を取り出し（Ｓ３５８）、一致度が閾値以下（ノルムが閾値以上）の間（Ｓ３６０におけるＮＯ）、それを初期姿勢に置き換えて、上記処理を繰り返す。このように、一致度が閾値を超える（ノルムが閾値以下になる）まで（Ｓ３６０におけるＹＥＳ）共役勾配法等を利用して反復的に更新し、パターンマッチングを遂行する。
【００７１】
ノルムの取り方（メトリック）としては、例えば、Ｌ１ノルムやＬ２ノルム等があり、用途に応じて適した取り方を採用する。また、このとき、３次元モデル１２２の可変部に相当する変化パラメータを予め定められた変化範囲内で変化させつつ対応点間の距離を導出する。これは、ＩＣＰでの収束パラメータの次元を増やしたことに相当する。このように、３次元モデル１２２の可変部の内部自由度を変化させつつ認識対象物１１２とのパターンマッチングを行うことにより、可変部の変化も加味することができるので、一致度を格段に高めることができる。また、規則性を有する等、変化範囲を限定できる部位のみを可変部として用いることで、計算負荷の増大を抑制することができ、適切な計算負荷の下、認識対象物１１２の的確な位置および姿勢を特定することが可能となる。
【００７２】
こうして導出された位置および姿勢は、最終的に、形状認識装置１２０の座標系からロボット１４０の座標系に変換される。
【００７３】
このような形状認識方法においても、固定部単体では対称性によって、長手軸に対して順方向か逆方向かを特定できない場合であっても、固定部における可変部との接続位置や可変部の形状を、固定部を特徴付ける要素として用いているので、認識対象物１１２の位置や姿勢を高精度で特定することが可能となる。さらに、固定部に可変部を加えた３次元モデル１２２と、それに相当する認識対象物１１２とをパターンマッチングするのみならず、フレキシブルな可変部を既知の変化範囲で変化させることで、位置および姿勢をより高精度に特定することが可能となる。
【００７４】
以上、添付図面を参照しながら本発明の好適な実施形態について説明したが、本発明はかかる実施形態に限定されないことは言うまでもない。当業者であれば、特許請求の範囲に記載された範疇において、各種の変更例または修正例に想到し得ることは明らかであり、それらについても当然に本発明の技術的範囲に属するものと了解される。
【００７５】
なお、本明細書の形状認識方法における各工程は、必ずしもフローチャートとして記載された順序に沿って時系列に処理する必要はなく、並列的あるいはサブルーチンによる処理を含んでもよい。
【産業上の利用可能性】
【００７６】
本発明は、任意の認識対象物の位置および姿勢を認識可能な形状認識装置、形状認識方法、および、そのプログラムに利用することができる。
【符号の説明】
【００７７】
１００  …ピッキングシステム
１１２  …認識対象物
１２０  …形状認識装置
１２２  …３次元モデル
２１０  …モデル保持部
２３０  …認識対象情報取得部
２３２  …端部切出部
２３４  …認識対象物特定部
２５０  …コネクタ
２５２  …ケーブル
【図１】
0005716434.tif 000002
【図２】
0005716434.tif 000003
【図３】
0005716434.tif 000004
【図４】
0005716434.tif 000005
【図５】
0005716434.tif 000006
【図６】
0005716434.tif 000007
【図７】
0005716434.tif 000008