(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022183640
(43)【公開日】2022-12-13
(54)【発明の名称】３Ｄ物体位置検出装置及び３Ｄ物体位置検出方法
(51)【国際特許分類】
G06T 7/70 20170101AFI20221206BHJP
G06T 7/00 20170101ALI20221206BHJP
G01B 11/245 20060101ALI20221206BHJP
【ＦＩ】
G06T7/70 Z
G06T7/00 300D
G06T7/00 350C
G06T7/00 C
G01B11/245 H
【審査請求】未請求
【請求項の数】11
【出願形態】ＯＬ
(21)【出願番号】P 2021091075
(22)【出願日】2021-05-31
(71)【出願人】
【識別番号】390010308
【氏名又は名称】東芝デベロップメントエンジニアリング株式会社
(74)【代理人】
【識別番号】100108855
【弁理士】
【氏名又は名称】蔵田 昌俊
(74)【代理人】
【識別番号】100103034
【弁理士】
【氏名又は名称】野河 信久
(74)【代理人】
【識別番号】100179062
【弁理士】
【氏名又は名称】井上 正
(74)【代理人】
【識別番号】100075672
【弁理士】
【氏名又は名称】峰 隆司
(74)【代理人】
【識別番号】100153051
【弁理士】
【氏名又は名称】河野 直樹
(74)【代理人】
【識別番号】100162570
【弁理士】
【氏名又は名称】金子 早苗
(72)【発明者】
【氏名】石過 壮
【テーマコード（参考）】
2F065
5L096
【Ｆターム（参考）】
2F065AA04
2F065AA37
2F065BB05
2F065DD03
2F065FF01
2F065FF05
2F065FF09
2F065JJ03
2F065JJ05
2F065JJ26
2F065QQ38
5L096AA06
5L096AA09
5L096BA05
5L096CA05
5L096DA02
5L096EA13
5L096EA15
5L096EA16
5L096FA60
5L096FA66
5L096FA67
5L096FA69
5L096GA40
5L096GA51
5L096HA11
5L096JA09
(57)【要約】
【課題】物体の位置及び姿勢を短時間に精度良く検出する。
【解決手段】一実施形態に係る３Ｄ物体位置検出装置は、画像取得部、マッチング部、及び算出部を備える。画像取得部は第１及び第２のカメラで物体を撮影した第１及び第２の画像を取得する。マッチング部は、第１の画像中の物体に対応するモデルの姿勢とモデルの基準点に対応する第１の画像中の点の位置とを探索するために、モデル及び第１の画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において第２の画像中の物体に対応するモデルの姿勢とモデルの基準点に対応する第２の画像中の点の位置とを探索するために、モデル及び第２の画像に対して２Ｄ－３Ｄマッチングを行う。算出部は、第１及び第２の画像のそれぞれに対する２Ｄ－３Ｄマッチングの結果に基づいて物体の位置及び姿勢を算出する。
【選択図】図２
【解決手段】一実施形態に係る３Ｄ物体位置検出装置は、画像取得部、マッチング部、及び算出部を備える。画像取得部は第１及び第２のカメラで物体を撮影した第１及び第２の画像を取得する。マッチング部は、第１の画像中の物体に対応するモデルの姿勢とモデルの基準点に対応する第１の画像中の点の位置とを探索するために、モデル及び第１の画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において第２の画像中の物体に対応するモデルの姿勢とモデルの基準点に対応する第２の画像中の点の位置とを探索するために、モデル及び第２の画像に対して２Ｄ－３Ｄマッチングを行う。算出部は、第１及び第２の画像のそれぞれに対する２Ｄ－３Ｄマッチングの結果に基づいて物体の位置及び姿勢を算出する。
【選択図】図２
【特許請求の範囲】
【請求項1】
第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得する画像取得部と、
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行い、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うマッチング部と、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と、前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、前記物体の位置及び姿勢を算出する算出部と、
を備える３Ｄ物体位置検出装置。
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行い、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うマッチング部と、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と、前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、前記物体の位置及び姿勢を算出する算出部と、
を備える３Ｄ物体位置検出装置。
【請求項2】
前記マッチング部は、
前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、
を含む処理をさらに行う、
請求項１に記載の３Ｄ物体位置検出装置。
前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、
を含む処理をさらに行う、
請求項１に記載の３Ｄ物体位置検出装置。
【請求項3】
前記マッチング部は、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて算出される前記物体の姿勢と前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて算出される前記物体の姿勢との差が所定の条件を満たすまで前記処理を繰り返し行う、
請求項２に記載の３Ｄ物体位置検出装置。
請求項２に記載の３Ｄ物体位置検出装置。
【請求項4】
第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得する画像取得部と、
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、を含む第１の処理と、
前記第１の処理の結果に基づいて決定される第１の探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行う第２の処理と、
前記第１の処理の結果に基づいて決定される第２の探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行う第３の処理と、
を行うマッチング部と、
前記第２の処理の結果と前記第３の処理の結果とに基づいて、前記物体の位置及び姿勢を算出する算出部と、
を備え、
前記マッチング部は、前記第２の処理及び前記第３の処理を同時並行的に実行する、
３Ｄ物体位置検出装置。
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、を含む第１の処理と、
前記第１の処理の結果に基づいて決定される第１の探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行う第２の処理と、
前記第１の処理の結果に基づいて決定される第２の探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行う第３の処理と、
を行うマッチング部と、
前記第２の処理の結果と前記第３の処理の結果とに基づいて、前記物体の位置及び姿勢を算出する算出部と、
を備え、
前記マッチング部は、前記第２の処理及び前記第３の処理を同時並行的に実行する、
３Ｄ物体位置検出装置。
【請求項5】
前記マッチング部は、前記第２の処理の結果に基づいて算出される前記物体の姿勢と前記モデル及び前記第３の処理の結果に基づいて算出される前記物体の姿勢との差が所定の条件を満たすまで前記第２の処理及び前記第３の処理を繰り返し行い、
前記第２の処理及び前記第３の処理が繰り返し行われるときには、前記第１の探索範囲は、直前に行われた前記第３の処理の結果に基づいて決定され、前記第２の探索範囲は、直前に行われた前記第２の処理の結果に基づいて決定される、
請求項４に記載の３Ｄ物体位置検出装置。
前記第２の処理及び前記第３の処理が繰り返し行われるときには、前記第１の探索範囲は、直前に行われた前記第３の処理の結果に基づいて決定され、前記第２の探索範囲は、直前に行われた前記第２の処理の結果に基づいて決定される、
請求項４に記載の３Ｄ物体位置検出装置。
【請求項6】
前記マッチング部は、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うときに、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングにより得られる前記モデルの姿勢に対して１つの軸周りの回転を前記モデルに施す、
請求項１乃至５のいずれか１項に記載の３Ｄ物体位置検出装置。
請求項１乃至５のいずれか１項に記載の３Ｄ物体位置検出装置。
【請求項7】
前記マッチング部は、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と前記１つの軸周りに前記モデルに施す回転の角度とに基づいて決定される前記第２の画像中の位置を基準として、前記第２の画像中の物体の検出を行う、
請求項６に記載の３Ｄ物体位置検出装置。
請求項６に記載の３Ｄ物体位置検出装置。
【請求項8】
前記マッチング部は、ディープラーニングを用いて前記第１の画像及び前記第２の画像のそれぞれにおける前記物体の位置を推定することにより、前記物体までの距離を推定し、前記距離の推定結果を使用して前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングを行う、
請求項１乃至７のいずれか１項に記載の３Ｄ物体位置検出装置。
請求項１乃至７のいずれか１項に記載の３Ｄ物体位置検出装置。
【請求項9】
前記ディープラーニングでの精度及び処理時間並びに２Ｄ－３Ｄマッチングでの精度及び処理時間は、前記物体の位置及び姿勢の検出精度並びに前記物体の位置及び姿勢を算出する処理時間が所定の条件を満たすように設定される、
請求項８に記載の３Ｄ物体位置検出装置。
請求項８に記載の３Ｄ物体位置検出装置。
【請求項10】
３Ｄ物体位置検出装置により実行される３Ｄ物体位置検出方法であって、
第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得することと、
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と、前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、前記物体の位置及び姿勢を算出することと、
を備える３Ｄ物体位置検出方法。
第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得することと、
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、
前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と、前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、前記物体の位置及び姿勢を算出することと、
を備える３Ｄ物体位置検出方法。
【請求項11】
第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得することと、
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、を含む第１の処理を行うことと、
前記第１の処理の結果に基づいて決定される第１の探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行う第２の処理を行うことと、
前記第１の処理の結果に基づいて決定される第２の探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行う第３の処理を行うことと、
前記第２の処理の結果と前記第３の処理の結果とに基づいて、前記物体の位置及び姿勢を算出することと、
を備え、
前記第２の処理及び前記第３の処理は同時並行的に実行される、
３Ｄ物体位置検出方法。
前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行うことと、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行うことと、を含む第１の処理を行うことと、
前記第１の処理の結果に基づいて決定される第１の探索範囲において、前記第１の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行う第２の処理を行うことと、
前記第１の処理の結果に基づいて決定される第２の探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行う第３の処理を行うことと、
前記第２の処理の結果と前記第３の処理の結果とに基づいて、前記物体の位置及び姿勢を算出することと、
を備え、
前記第２の処理及び前記第３の処理は同時並行的に実行される、
３Ｄ物体位置検出方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明の実施形態は、３Ｄ物体位置検出装置及び３Ｄ物体位置検出方法に関する。
【背景技術】
【0002】
ピッキングやパッキングなどのアクションを行うロボットが広く利用されている。ロボットが物体に対してアクションを行うためには、物体とロボットとの間の相対的な位置及び姿勢を測定する必要がある。
【0003】
特許文献１は、２次元見え方モデルを介して２台のカメラにより得られる２次元画像間の対応付けを行うことにより、物体の３次元位置を算出する方法を開示している。引用文献２は、２台のカメラにより得られた２次元画像間の対応付けを行い、三角測量の原理によって３次元画像情報を得て、３次元画像情報から物体の平面を抽出し、３次元画像情報の画像を平面の法線方向からの画像に変換し、法線方向画像及びモデル画像に対して２Ｄ－２Ｄマッチングを行うことにより、物体の位置及び姿勢を検出する方法を開示している。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開平８－１３６２２０号公報
【特許文献2】特開２０１０－１２７８１９号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
特許文献１に開示される方法では、３次元的な特徴点が少ない物体については精度の良い検出ができない。特許文献２に開示される方法では、平面部がない物体については位置及び姿勢の検出ができない。
【0006】
３次元的な特徴点が少ない物体や平坦部がない物体であっても、物体の位置及び姿勢を精度良く検出できる技術が求められている。
【0007】
また、特許文献１に記述されているように、スリット光などを用いて点群データを作成する場合、精度の良い点群データを取得するには相当の時間がかかる。モデル作成では多少時間がかかってもよいが、シーンからのデータ作成の時間は短くしなければならない。
【課題を解決するための手段】
【0008】
一実施形態に係る３Ｄ物体位置検出装置は、画像取得部、マッチング部、及び算出部を備える。画像取得部は、第１のカメラで物体を撮影した第１の画像及び第２のカメラで前記物体を撮影した第２の画像を取得する。マッチング部は、前記第１の画像中の前記物体に対応する前記物体の３Ｄ形状を表すモデルの姿勢と、前記モデルに施される回転の基準点に対応する前記第１の画像中の点の位置と、を探索するために、前記モデル及び前記第１の画像に対して２Ｄ－３Ｄマッチングを行い、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、前記第２の画像中の前記物体に対応する前記モデルの姿勢と、前記基準点に対応する前記第２の画像中の点の位置と、を探索するために、前記モデル及び前記第２の画像に対して２Ｄ－３Ｄマッチングを行う。算出部は、前記モデル及び前記第１の画像に対する２Ｄ－３Ｄマッチングの結果と、前記モデル及び前記第２の画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、前記物体の位置及び姿勢を算出する。
【発明の効果】
【0009】
本発明によれば、物体の位置及び姿勢を短時間に精度良く検出する技術が提供される。
【図面の簡単な説明】
【0010】
【図1】実施形態に係る３Ｄ物体位置検出装置が適用されるシステムの一例であるロボットシステムを示すブロック図。
【図2】実施形態に係る３Ｄ物体位置検出装置を示すブロック図。
【図4】同じ形状で大きさが異なる２つのワークがある事例を示す図。
【図5】参照画像及びカメラ画像を重ね合わせた状態を示す図。
【図6】実施形態に係る物体の位置及び姿勢を検出する方法を説明する図。
【図7】実施形態に係る物体の位置及び姿勢を検出する方法を説明する図。
【図8】実施形態に係る物体の位置及び姿勢を検出する方法を説明する図。
【図11】１台のカメラを用いた場合に物体の位置を一意に決定できないことを説明する図。
【発明を実施するための形態】
【0011】
以下、図面を参照しながら実施形態を説明する。なお、重複する説明を避けるために、全図を通じて同様の構成要素に同様の参照符号を付している。
【0012】
［構成］
図１は、実施形態に係る３Ｄ（３次元）物体位置検出装置が適用され得るロボットシステム１０を概略的に示している。図１に示すロボットシステム１０は、例えば、ある領域から別の領域にワークを移動させる。ここで、ワークはロボットシステム１０により処理される物体の一例である。ロボットシステム１０は、ロボット１１、ステレオカメラ１２、及びコントローラ１３を備える。
図１は、実施形態に係る３Ｄ（３次元）物体位置検出装置が適用され得るロボットシステム１０を概略的に示している。図１に示すロボットシステム１０は、例えば、ある領域から別の領域にワークを移動させる。ここで、ワークはロボットシステム１０により処理される物体の一例である。ロボットシステム１０は、ロボット１１、ステレオカメラ１２、及びコントローラ１３を備える。
【0013】
ロボット１１は、アームとアームの先端部に設けられたエンドエフェクタとを備える。アームは、例えば多関節アームであり、エンドエフェクタの位置及び姿勢を変更させる。エンドエフェクタは、例えば、ワークを把持するように構成されたグリッパであり得る。
【0014】
ステレオカメラ１２は１対のカメラ１２Ｒ、１２Ｌを備える。各カメラ１２Ｒ、１２Ｌは２Ｄ（２次元）画像を生成する。本実施形態では、２Ｄ画像はカラー画像である。なお、２Ｄ画像はモノクロ画像であってもよい。例えば、カメラ１２Ｒ、１２Ｌは、それらの光軸が互いに平行となるように配置される。代替として、カメラ１２Ｒ、１２Ｌは、それらの光軸が互いに交差するように配置されてもよい。ステレオカメラ１２は、ロボット１１の周囲環境に設けられてもよく、或いは、ロボット１１に取り付けられてもよい。
【0015】
コントローラ１３は、プロセッサ１４、メモリ１５、及び入出力インタフェース１６を備える。プロセッサ１４は、ＣＰＵ（Central Processing Unit）などの汎用プロセッサを含む。メモリ１５は、制御プログラム、モデル、及び各カメラ１２Ｒ、１２Ｌの内部パラメータなどのデータを記憶する。制御プログラムは、プロセッサ１４により実行されたときに、ロボット１１及びステレオカメラ１２の制御をプロセッサ１４に行わせる。制御プログラムは、図２に示す３Ｄ物体位置検出装置２０に関して後述する処理をプロセッサ１４に行わせる画像処理プログラムを含む。
【0016】
プロセッサ１４は、ステレオカメラ１２でワークを含むシーンを撮像してステレオ画像を得る。ステレオ画像は、カメラ１２Ｒにより得られる画像及びカメラ１２Ｌにより得られる画像を含む。以降では、各カメラ１２Ｒ、１２Ｌにより得られる画像をカメラ画像と称することもある。また、カメラ１２Ｒにより得られる画像を右カメラ画像と称し、カメラ１２Ｌにより得られる画像を左カメラ画像と称することもある。プロセッサ１４は、ステレオ画像に基づいてワークの位置及び姿勢を検出する。ステレオ画像に基づいてワークの位置及び姿勢を検出する方法については後述する。プロセッサ１４は、ワークの位置及び姿勢の検出結果に基づいてロボット１１を操作する。
【0017】
入出力インタフェース１６は、ロボット１１及びステレオカメラ１２に対するインタフェースである。プロセッサ１４は、入出力インタフェース１６を介してロボット１１及びステレオカメラ１２と通信する。
【0018】
制御プログラムは、コンピュータで読み取り可能な記憶媒体に記憶された状態でコントローラ１３に提供されてよい。この場合、例えば、コントローラ１３は、記憶媒体からデータを読み出すドライブを備え、記憶媒体からプログラムを取得する。記憶媒体の例は、磁気ディスク、光ディスク（ＣＤ－ＲＯＭ、ＣＤ－Ｒ、ＤＶＤ－ＲＯＭ、ＤＶＤ－Ｒなど）、光磁気ディスク（ＭＯなど）、半導体メモリを含む。また、プログラムをネットワーク上のサーバに格納し、コントローラ１３がサーバからプログラムをダウンロードするようにしてもよい。
【0019】
なお、コントローラ１３は、汎用プロセッサに代えて又は追加して、ＡＳＩＣ（Application Specific Integrated Circuit）又はＦＰＧＡ（Field Programmable Gate Array）などの専用プロセッサを含んでいてもよい。
【0020】
図２は、実施形態に係る３Ｄ物体位置検出装置２０の構成例を概略的に示している。３Ｄ物体位置検出装置２０は、図１に示したプロセッサ１４により実施される。図２に示すように、３Ｄ物体位置検出装置２０は、画像取得部２１、モデル記憶部２２、マッチング部２３、及び算出部２４を備える。３Ｄ物体位置検出装置２０は、ステレオカメラ１２と別個の装置として示されているが、ステレオカメラ１２を備えてもよい。
【0021】
画像取得部２１はステレオカメラ１２からステレオ画像を取得する。ステレオ画像は、カメラ１２Ｒにより得られる右カメラ画像及びカメラ１２Ｌにより得られる左カメラ画像を含む。
【0022】
モデル記憶部２２は、あらかじめ用意される、ワークの３Ｄ形状を表すモデル（３Ｄモデル）を格納する。本実施形態では、モデルは、ワークの３Ｄ形状をモデル化したデータにワークの色情報（ＲＧＢ情報）を付加することにより得られる色付き点群データである。モデルはグレースケールの点群データであってもよい。
【0023】
マッチング部２３は、モデル記憶部２２に格納されているモデル及び画像取得部２１により取得される各カメラ画像に対して２Ｄ－３Ｄマッチングを行う。２Ｄ－３Ｄマッチングは、３Ｄモデルを投影した２Ｄ画像とシーンとして撮影された２Ｄ画像とのマッチングからシーン中の対象物を検出する手法である。以降では、モデルを投影した２Ｄ画像を参照画像と称することもある。また２Ｄ画像どうしのマッチング処理はテンプレートマッチングであってもよいし、特徴量ベースのマッチングでもよい。後述のディープラーニングを用いることで、左カメラ画像及び右カメラ画像のそれぞれのワークのおおよその位置を取得することができれば、そこからワークまでのおおよその距離がわかるので、ワークの大きさの範囲を限定できる。探索の範囲が限定されていれば、回転と大きさを考慮した２Ｄ-２Ｄマッチングは短時間で高精度な検出が可能である。
【0024】
マッチング部２３は、モデルに対して回転及び平行移動を施したうえで参照画像を生成することができる。マッチング部２３は、カメラ画像中のワークに対応するモデルの姿勢と、モデルに施される回転の基準点に対応するカメラ画像中の点の位置と、を探索するために、モデル及びカメラ画像に対して２Ｄ－３Ｄマッチングを行う。カメラ画像中のワークに対応するモデルの姿勢は、参照画像中のモデルがカメラ画像中のワークとマッチするときにおけるモデルの姿勢を示す。例えば、マッチング部２３は、参照画像及びカメラ画像間のマッチングを行って参照画像中のモデルとカメラ画像中のワークとの間の一致度を測定し、一致度が所定の閾値を超える場合に参照画像中のモデルがカメラ画像中のワークとマッチすると判定する。姿勢は直交する３つの軸周りの回転角で表すことができる。基準点は例えばモデルの重心である。以降では、モデルに施される回転の基準点に対応するカメラ画像中の点をカメラ画像中の対応点と称することもある。基準点がモデルの重心である場合、カメラ画像中の対応点は、カメラ画像におけるワークの重心に対応する。
【0025】
マッチング部２３は、右カメラ画像中のワークに対応するモデルの姿勢と、右カメラ画像中の対応点の位置と、を探索するために、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行う。マッチング部２３は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、左カメラ画像中のワークに対応するモデルの姿勢と、左カメラ画像中の対応点の位置と、を探索するために、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行う。
【0026】
算出部２４は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、モデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、ワークの３次元空間内の位置及び姿勢を算出する。
【0027】
図３は、モデル記憶部２２に格納されるモデルの一例を概略的に示している。図３に示すモデルは、１つの面に星印がある立方体であるワークをモデル化した色付き点群データである。各面は隣接する面と異なる色及び明るさを有し、星印は星印がある面と異なる色及び明るさを有する。
【0028】
図４は、同じ形状で大きさの異なる２つのワーク４１、４２がある事例を概略的に示している。図４に示す事例では、小さいワーク４２が大きいワーク４１よりもステレオカメラ１２の近くに位置している。右カメラ画像４５Ｒでは、ワーク４１、４２は同じに写っており、ワーク４１とワーク４２を区別することができない。一方、左カメラ画像４５Ｌでは、ワーク４１とワーク４２はそれぞれ別の位置に写るので区別できる。
【0029】
図５は、参照画像中のモデル５３がカメラ画像中のワーク５１とマッチしているときの参照画像及びカメラ画像を重ね合わせた状態を概略的に示している。図５において、カメラ画像中のワーク５１が実線で示され、参照画像中のモデル５３が破線で示されている。矢印は、参照画像中のモデル５３の重心５４とカメラ画像中の対応点５２との間のずれを示す。
【0030】
図６から図８を参照して、ワークの位置及び姿勢を検出する方法について説明する。説明のために、図６に示すように、各カメラ１２Ｒ、１２Ｌに対してカメラ座標系（ＸｃＹｃＺｃ座標系）を設定し、カメラ１２Ｒのレンズとカメラ１２Ｌのレンズとの中間地点を原点Ｏとし、Ｘ軸、Ｙ軸、Ｚ軸がそれぞれＸｃ軸、Ｙｃ軸、Ｚｃ軸と平行になるようにワールド座標系（ＸＹＺ座標系）を設定する。カメラ間隔（カメラ１２Ｌ、１２Ｒ間の距離）をＴとする。この場合、ワールド座標系では、カメラ１２Ｒの位置は（Ｔ／２，０，０）であり、カメラ１２Ｌの位置は（－Ｔ／２，０，０）である。ワークの位置を（Ｘｓ，Ｙｓ，Ｚｓ）とし、ワークの回転角を（αｓ，βｓ，γｓ）とする。αｓはＸ軸周りの回転角（ピッチ方向の回転角）を表し、βｓはＹ軸周りの回転角（ヨー方向の回転角）を表し、γｓはＺ軸周りの回転角（ロール方向の回転角）を表す。
【0031】
さらに、図７に示すように、モデル７１に対して、カメラ座標系に対応するＸｍＹｍＺｍ座標系を設定する。ＸｍＹｍＺｍ座標系の原点に各カメラ１２Ｒ、１２Ｌと同等のカメラがあるものとして、参照画像７５が生成される。図７には、回転を施していない状態のモデル７１及び当該モデル７１について得られる参照画像７５が示されている。マッチング部２３は、モデル７１に対して回転及び平行移動を施し、回転及び平行移動を施した状態のモデル７１を２Ｄ画像平面に投影することにより、参照画像７５を生成する。マッチング部２３は、モデル７１の重心７３を中心として、モデルに対してＸｍ軸周りの回転、Ｙｍ軸周りの回転、Ｚｍ軸周りの回転を適用することができる。平行移動は、モデル７１の重心７３が参照画像７５の中心に位置するように、Ｚｍ軸に沿って行われてよい。Ｚｍ軸に沿った平行移動を施すことにより、参照画像７５上でのモデル７１の大きさが調整される。
【0032】
マッチング部２３は、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行い、参照画像中のモデルが右カメラ画像中のワークとマッチするときにおけるモデルの回転角（αｒ，βｒ，γｒ）と、参照画像中のモデルと右カメラ画像中のワークとの間のずれを表すシフト量（ｘｒ，ｙｒ）と、を取得する。さらに、マッチング部２３は、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行い、参照画像中のモデルが左カメラ画像中のワークとマッチするときにおけるモデルの回転角（αｌ，βｌ，γｌ）と、参照画像中のモデルと左カメラ画像中のワークとの間のずれを表すシフト量（ｘｌ，ｙｌ）と、を取得する。算出部２４は、回転角（αｒ，βｒ，γｒ）、シフト量（ｘｒ，ｙｒ）、回転角（αｌ，βｌ，γｌ）、シフト量（ｘｌ，ｙｌ）からワークの位置（Ｘｓ，Ｙｓ，Ｚｓ）及び回転角（αｓ，βｓ，γｓ）を算出する。
【0033】
以下では、説明を簡単にするために、αｓ＝γｓ＝０、Ｙｓ＝０とする。
【0034】
図６を参照すると、右カメラ画像６５Ｒは、カメラ１２Ｒでワーク６１を撮像することにより得られ、左カメラ画像６５Ｌは、カメラ１２Ｌでワーク６１を撮像することにより得られる。図６において、ｆは各カメラ１２Ｌ、１２Ｒのレンズの焦点距離を表す。
【0035】
マッチング部２３は、参照画像７５中のモデル７１が右カメラ画像６５Ｒ中のワーク６１にマッチするまで、モデル７１に対して回転及び平行移動を少しずつ行う。マッチング部２３は、参照画像７５中のモデル７１が右カメラ画像６５Ｒ中のワーク６１にマッチするときにおけるモデル７１の回転角βｒを得る。マッチング部２３は、右カメラ画像６５Ｒ中の対応点の位置を検出し、参照画像７５中のモデル７１の重心７３と右カメラ画像６５Ｒ中の対応点との間のずれを表すシフト量ｘｒを得る。
【0036】
同様に、マッチング部２３は、参照画像７５中のモデル７１が左カメラ画像６５Ｌ中のワーク６１にマッチするまで、モデル７１に対して回転及び平行移動を少しずつ行う。マッチング部２３は、参照画像７５中のモデル７１が左カメラ画像６５Ｌ中のワーク６１にマッチするときにおけるモデル７１の回転角βｌを得る。マッチング部２３は、左カメラ画像６５Ｌ中の対応点の位置を検出し、参照画像７５中のモデル７１の重心７３と左カメラ画像６５Ｌ中の対応点との間のずれを表すシフト量ｘｌを得る。
【0037】
カメラ１２Ｒ、１２Ｌの光軸が互いに平行である場合、カメラ１２Ｌから見たワーク６１の姿勢はカメラ１２Ｒから見たワーク６１の姿勢に対してＹ軸周りの回転を施したものになる。よって、マッチング部２３は、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行う際には、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングにより得られた回転角（αｒ，βｒ，γｒ）に対してＹｍ軸周りの回転をモデルに施せばよい。
【0038】
ワーク６１の位置（Ｘｓ，Ｚｓ）は下記の式で表すことができる。
【数1】
【0039】
モデル７１の回転角βｉ（ｉ＝ｒ，ｌ）は、ワーク６１の実際の回転角βｓにカメラの見込み角を加えた値になる。よって、モデル７１の回転角βｒ、βｌとワーク６１の回転角βｓとの間には、下記の関係が成り立つ。
【数2】
【0040】
よって、ワーク６１の回転角βｓは下記の式で表すことができる。
【数3】
【0041】
上記の式において下記の近似を使用している。
【数4】
【0042】
回転角（αｒ，βｒ，γｒ）及びシフト量（ｘｒ，ｙｒ）と回転角（αｌ，βｌ，γｌ）及びシフト量（ｘｌ，ｙｌ）との間には拘束条件がある。マッチング部２３は、２Ｄ－３Ｄマッチングを行う際に拘束条件を使用する。例えば、マッチング部２３は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果（回転角（αｒ，βｒ，γｒ）及びシフト量（ｘｒ，ｙｒ））に基づいて決定される探索範囲において、回転角（αｌ，βｌ，γｌ）及びシフト量（ｘｌ，ｙｌ）を探索する。αｓ＝γｓ＝０、Ｙｓ＝０である場合には、拘束条件は以下のようになる。
【0043】
上記式（３）、（４）から下記式（５）が求まる。
【数5】
【0044】
式（５）に上記の近似を適用すると、下記式（６）が求まる。
【数6】
【0045】
上記の式（６）によれば、図８に示すように、（βｌ，ｘｌ）は、傾き１／ｆ、切片（βｒ－ｘｒ／ｆ）の直線上の点である。（βｌ，ｘｌ）は上記式（６）を満たす点の１つであるので、マッチング部２３は、（βｌ，ｘｌ）を求める際には、上記式（６）及び（βｒ，ｘｒ）に基づいて決定される探索範囲において探索を行う。例えば、マッチング部２３は、式（６）を使用して回転角βｒ及びシフト量ｘｒとモデル７１に施した回転角とから算出される左カメラ画像６５Ｌ中の位置を基準として、左カメラ画像６５Ｌ中のワーク６１の検出を行う。例えば、基準となる位置を中心とする探索範囲内でワーク６１の検出を行う。
【0046】
マッチング部２３は、モデル７１及び右カメラ画像６５Ｒに対して２Ｄ－３Ｄマッチングを行い、参照画像７５中のモデル７１が右カメラ画像６５Ｒ中のワーク６１とマッチしたときにおける回転角とシフト量の組み（βｒ，ｘｒ）を特定する。ワーク６１までの距離がまだ定まらないので、（βｒ，ｘｒ）は誤差を含んでいる可能性が高い。
【0047】
さらに、マッチング部２３は、モデル７１及び左カメラ画像６５Ｌに対して２Ｄ－３Ｄマッチングを行い、参照画像７５中のモデル７１が左カメラ画像６５Ｌ中のワーク６１とマッチしたときにおける回転角とシフト量の組み（βｌ，ｘｌ）を特定する。
【0048】
マッチング部２３は、下記式（７）に従って、（βｒ，ｘｒ）及び（βｌ，ｘｌ）から誤差εを計算する。
【数7】
【0049】
誤差εは、モデル７１及び右カメラ画像６５Ｒに対する２Ｄ－３Ｄマッチングの結果に基づいて得られるワーク６１の回転角と、モデル７１及び左カメラ画像６５Ｌに対する２Ｄ－３Ｄマッチングの結果に基づいて得られるワーク６１の回転角と、の間の差を表す。
【0050】
誤差εがあらかじめ設定される閾値以下である場合、算出部２４は、下記式（８）に従ってワークの回転角βｓを得る。
【数8】
【0051】
誤差εがあらかじめ設定される閾値を超える場合、マッチング部２３は、下記式（９）を使用して（βｌ，Ｘｌ）から決定される探索範囲において探索を再度行い、（βｒ，Ｘｒ）を求める。
【数9】
【0052】
上述した操作を繰り返すことにより、誤差εは徐々に小さくなる。誤差εが閾値より小さくなるまで上述した操作を繰り返すことにより、閾値以下の精度でワーク６１の位置及び姿勢を求めることができる。さらに、探索範囲は式（６）及び式（９）を用いて決定される範囲に限定されるので、短時間で処理を行うことができる。
【0053】
実際の処理では上記式（４）の近似を用いないことで、精度の高い検出ができる。その際の処理の流れは上記と同じになる。
【0054】
回転角（αｒ，βｒ，γｒ）及びシフト量（ｘｒ，ｙｒ）の初期値を決定する際に、マッチング部２３は、ディープラーニングを用いて右カメラ画像及び左カメラ画像のそれぞれにおけるワークの位置を推定することにより、ワークまでの距離を推定し、距離の推定結果を使用してモデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングを行ってよい。例えば、マッチング部２３は、ディープラーニングを用いて右カメラ画像及び左カメラ画像におけるワークの位置ｘｒ、ｘｌを推定し、推定された位置ｘｒ、ｘｌを上記式（１）に代入することによりＺの値を求める。マッチング部２３は、求めたＺの値からワークの大きさの範囲を推定し、ワークの大きさの範囲の推定結果からモデルに施す平行移動の量の範囲を決定する。このように、ディープラーニングを用いて得られる距離の推定結果は、ワークの大きさの範囲を限定することを可能にし、よって、モデルに施す平行移動の量の範囲を限定することを可能にする。その結果、２Ｄ－３Ｄマッチングの処理時間が短縮される。ディープラーニングとしては、例えば、金崎朝子、“第４話 ３次元物体認識技術”、［online］、インターネット＜URL: https://kanezaki.github.io/media/RobotSeminar20180531_AsakoKanezaki.pdf＞又は太田英司、“セマンティック・セグメンテーションの基礎”、［online］、インターネット＜URL: https://jp.mathworks.com/content/dam/mathworks/mathworks-dot-com/company/events/webinar-cta/2459280_Basics_of_semantic_segmentation.pdf＞に開示される技術を利用することができる。
【0055】
［動作］
次に、３Ｄ物体位置検出装置２０の動作例を説明する。
次に、３Ｄ物体位置検出装置２０の動作例を説明する。
【0056】
図９は、３Ｄ物体位置検出装置２０がワークの位置及び姿勢を算出する処理手順の例を概略的に示している。
【0057】
図９のステップＳ１１において、画像取得部２１は、ステレオカメラ１２からカメラ１２Ｒにより得られた右カメラ画像及びカメラ１２Ｌにより得られた左カメラ画像を取得する。例えば、画像取得部２１は、ステレオカメラ１２を使用してワークを含むシーンを撮影し、ステレオカメラ１２から右カメラ画像及び左カメラ画像を受信する。
【0058】
ステップＳ１２において、マッチング部２３は、右カメラ画像中のワークに対応するモデルの姿勢と、右カメラ画像中の対応点の位置と、を探索するために、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行う。
【0059】
ステップＳ１３において、マッチング部２３は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、左カメラ画像中のワークに対応するモデルの姿勢と、左カメラ画像中の対応点の位置と、を探索するために、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行う。例えば、マッチング部２３は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果とモデルに施した回転角とから算出される左カメラ画像中の位置を基準として、ワーク６１の検出を行う。
【0060】
ステップＳ１４において、算出部２４は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果とモデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果とに基づいてワークの位置及び姿勢を算出する。
【0061】
【0062】
図１０のステップＳ２１において、マッチング部２３は、右カメラ画像に基づいてシフト量ｘｒ及び回転角βｒの初期値を設定する。例えば、マッチング部２３は、モデルに対して回転及び平行移動を施してモデルを２Ｄ画像平面に投影することにより参照画像を生成し、参照画像と右カメラ画像との間でマッチングを行う処理を繰り返し行い、それにより参照画像中のモデルが右カメラ画像中のワークとマッチするときにおけるシフト量ｘｒ及び回転角βｒを初期値として得る。
【0063】
マッチング部２３は、ディープラーニングを使用して右カメラ画像からシフト量ｘｒ及び回転角βｒの初期値を設定してもよい。具体的には、マッチング部２３は、ディープラーニングを用いて右カメラ画像及び左カメラ画像のそれぞれにおけるワークの位置を推定することにより、ワークまでの距離を推定し、距離の推定結果を使用してモデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行う。シフト量ｘｒ及び回転角βｒの初期値の設定にディープラーニングを使用することにより、ワークの個体差や照明の影響を受けにくくなり、シフト量ｘｒ及び回転角βｒの初期値をより適切に設定することが可能となる。その結果、後述する繰り返し処理の回数を低減することができる。
【0064】
ディープラーニングでは推論時間を実用的な時間(例えば0.2秒)にしようとすると、検出精度はワークまでの距離に対して±10%程度になる。ディープラーニングの検出結果を元に本実施形態の方法を適用することで、検出精度を±1%程度にすることが可能となる。ディープラーニングと本実施形態の方法を組み合わせることで短い時間(例えば0.5秒)で高い精度でワークの位置と姿勢を検出することが可能となる。例えば、ディープラーニングでの精度及び処理時間並びに２Ｄ－３Ｄマッチングでの精度及び処理時間は、ワークの位置及び姿勢の精度並びにワークの位置及び姿勢を算出する処理時間が所定の条件を満たすように設定される。
【0065】
ディープラーニングの位置検出で用いられるインスタンスセグメンテーションやセマンティックセグメンテーションでは推論時間はほぼ入力画像の画素数に比例する。おおよその位置と姿勢だけがわかればよいのであれば、例えば4×4のブロックに分割することで1024×1024を256×256にダウンサイジングできるので、推論時間を0.2秒程度に抑えることができる。また対象ワークの種類があらかじめわかっていれば、ディープラーニングによってワークの種類を特定できるので、複数種類(例えば5種類)のワークに対応することができる。
【0066】
ステップＳ２２において、マッチング部２３は、上記式（６）に従ってステップＳ２１において得られたシフト量ｘｒ及び回転角βｒから決定される探索範囲において、左カメラ画像に基づいてシフト量ｘｌ及び回転角βｌを求める。ディープラーニングを用いることで左カメラ画像におけるワークのおおよその位置をしることができるので、ｘｌの探索範囲を限定することができる。ｘｒ及びｘｌのおおよその値を上記式（１）に代入することでおおよそのＺの値がわかるので、２Ｄ－３Ｄマッチングの際の大きさの範囲を限定することができる。
【0067】
ステップＳ２３において、マッチング部２３は、ステップＳ２１において得られたシフト量ｘｒ及び回転角βｒ及びステップＳ２２において得られたシフト量ｘｌ及び回転角βｌから上記式（７）に示す誤差εを求め、誤差εが閾値以下であるか否かを判定する。
【0068】
誤差εが閾値を超える場合（ステップＳ２３；Ｎｏ）、フローはステップＳ２４に進む。ステップＳ２４において、マッチング部２３は、上記式（９）に従ってステップＳ２２において得られたシフト量ｘｌ及び回転角βｌから決定される探索範囲において、右カメラ画像に基づいてシフト量ｘｒ及び回転角βｒを求める。その後、フローはステップＳ２２に戻る。ステップＳ２２において、マッチング部２３は、上記式（６）に従ってステップＳ２４において求められたシフト量ｘｒ及び回転角βｒから決定される探索範囲において、左カメラ画像に基づいてシフト量ｘｌ及び回転角βｌを求める。ステップＳ２３において、マッチング部２３は、ステップＳ２４において得られたシフト量ｘｒ及び回転角βｒ及びステップＳ２２において得られたシフト量ｘｌ及び回転角βｌから上記式（７）に示す誤差εを求め、誤差εが閾値以下であるか否かを判定する。
【0069】
誤差εが閾値以下である場合（ステップＳ２３；Ｙｅｓ）、フローはステップＳ２５に進む。ステップＳ２５において、算出部２４は、シフト量ｘｒ及び回転角βｒ並びにシフト量ｘｌ及び回転角βｌに基づいてワークの位置（Ｘｓ，Ｚｓ）及び回転角βｓを求める。例えば、算出部２４は、上記式（８）に従ってワークの回転角βｓを算出し、上記式（１）、（２）に従ってワークの位置（Ｘｓ，Ｚｓ）を算出する。
【0070】
［効果］
３Ｄ物体位置検出装置２０は、右カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、モデルの基準点に対応する右カメラ画像中の点の位置と、を探索するために、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、左カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、基準点に対応する左カメラ画像中の点の位置と、を探索するために、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、モデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、ワークの位置及び姿勢を算出する。これにより、位置及び姿勢を精度良く検出することが可能となる。本実施形態に係る３Ｄ物体位置検出装置２０は、３次元的な特徴点が少ないワークや平坦部がないワークについても適用可能である。
３Ｄ物体位置検出装置２０は、右カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、モデルの基準点に対応する右カメラ画像中の点の位置と、を探索するために、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、左カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、基準点に対応する左カメラ画像中の点の位置と、を探索するために、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行い、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、モデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果と、に基づいて、ワークの位置及び姿勢を算出する。これにより、位置及び姿勢を精度良く検出することが可能となる。本実施形態に係る３Ｄ物体位置検出装置２０は、３次元的な特徴点が少ないワークや平坦部がないワークについても適用可能である。
【0071】
例えば、図１１に示すように、単一のカメラ１０１で得られた１枚のカメラ画像１０２に対して２Ｄ－３Ｄマッチングを行うと、ワークの姿勢は一意に決定できるが、同じ形状で大きさが異なるワーク１０３、１０４が異なる距離にいる場合に、それらを区別できず、ワークの位置を一意に決定することができない。これに対して、本実施形態では、２台のカメラ１２Ｌ、１２Ｒを使用することにより、ワークの位置及び姿勢を一意に決定することができる。
【0072】
さらに、モデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングにおいて、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において探索を行うことにより、処理時間を短縮することが可能となる。
【0073】
３Ｄ物体位置検出装置２０は、モデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、右カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、基準点に対応する右カメラ画像中の点の位置と、を探索するために、モデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行うことと、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて決定される探索範囲において、左カメラ画像中のワークの姿勢にマッチするモデルの姿勢と、基準点に対応する左カメラ画像中の点の位置と、を探索するために、モデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行うことと、を含む処理をさらに行う。３Ｄ物体位置検出装置２０は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて算出されるワークの姿勢とモデル及び左カメラ画像に対する２Ｄ－３Ｄマッチングの結果に基づいて算出されるワークの姿勢との差が所定の条件を満たすまで上記処理を繰り返す。これにより、所望する精度でワークの位置及び姿勢を検出することが可能となる。
【0074】
３Ｄ物体位置検出装置２０は、左カメラ画像中のワークに対応するモデルの姿勢を探索する際に、右カメラ画像に関して決定されたモデルの姿勢に対してＹｍ軸周りの回転（ヨー方向の回転）のみをモデルに施す。３Ｄ物体位置検出装置２０は、モデル及び右カメラ画像に対する２Ｄ－３Ｄマッチングの結果とモデルに施すＹｍ軸周りの回転の角度とに基づいて決定される左カメラ画像中の位置を基準として、左カメラ画像中のワークの検出を行う。このように、モデル及び一方のカメラ画像に対する２Ｄ－３Ｄマッチングの結果を使用して、他方のカメラ画像に対する２Ｄ－３Ｄマッチングにおける探索範囲を絞り込むことにより、処理時間を短縮することが可能となる。
【0075】
【0076】
図１２のステップＳ３１において、マッチング部２３は、シフト量ｘｒ及び回転角βｒの初期値並びにシフト量ｘｌ及び回転角βｌの初期値を設定する。例えば、マッチング部２３は、ディープラーニングを用いて右カメラ画像及び左カメラ画像のそれぞれにおけるワークの位置を推定することにより、ワークまでの距離を推定する。マッチング部２３は、距離の推定結果を使用してモデル及び右カメラ画像に対して２Ｄ－３Ｄマッチングを行い、それによりシフト量ｘｒ及び回転角βｒの初期値を設定する。さらに、マッチング部２３は、距離の推定結果を使用してモデル及び左カメラ画像に対して２Ｄ－３Ｄマッチングを行い、それによりシフト量ｘｌ及び回転角βｌの初期値を設定する。
【0077】
ステップＳ３２において、マッチング部２３は、下記式（１０）に従ってステップＳ３１において得られたシフト量ｘｌ及び回転角βｌから決定される探索範囲において、右カメラ画像に基づいてシフト量ｘｒ及び回転角βｒを求める。
【数10】
【0078】
ステップＳ３３において、マッチング部２３は、上記式（６）に従ってステップＳ３１において得られたシフト量ｘｒ及び回転角βｒから決定される探索範囲において、左カメラ画像に基づいてシフト量ｘｌ及び回転角βｌを求める。
【0079】
ステップＳ３２の処理及びステップＳ３３の処理は同時並行的に実行される。ステップＳ３２の処理及びステップＳ３３の処理が同時並行的に実行されるとは、ステップＳ３２の処理及びステップＳ３３の処理が少なくとも部分的に時間的に重なって実行されることを指す。
【0080】
ステップＳ３４において、マッチング部２３は、ステップＳ３２において得られたシフト量ｘｒ及び回転角βｒ及びステップＳ３３において得られたシフト量ｘｌ及び回転角βｌから上記式（７）に示す誤差εを求め、誤差εが閾値以下であるか否かを判定する。
【0081】
誤差εが閾値を超える場合（ステップＳ３４；Ｎｏ）、マッチング部２３は、上記式（９）に従ってステップＳ３３において得られたシフト量ｘｌ及び回転角βｌから探索範囲を決定し（ステップＳ３５）、決定した探索範囲において、右カメラ画像に基づいてシフト量ｘｒ及び回転角βｒを新たに求める（ステップＳ３２）。
【0082】
さらに、マッチング部２３は、下記式（１１）に従ってステップＳ３２において得られたシフト量ｘｒ及び回転角βｒから探索範囲を決定し（ステップＳ３６）、決定した探索範囲において、左カメラ画像に基づいてシフト量ｘｌ及び回転角βｌを新たに求める（ステップＳ３３）。
【数11】
【0083】
誤差εが閾値以下である場合（ステップＳ３４；Ｙｅｓ）、フローはステップＳ３７に進む。ステップＳ３７において、算出部２４は、ステップＳ３２において得られたシフト量ｘｒ及び回転角βｒ並びにステップＳ３３において得られたシフト量ｘｌ及び回転角βｌに基づいてワークの位置（Ｘｓ，Ｚｓ）及び回転角βｓを求める。例えば、算出部２４は、上記式（８）に従ってワークの回転角βｓを算出し、上記式（１）、（２）に従ってワークの位置（Ｘｓ，Ｚｓ）を算出する。
【0084】
図１２に示すフローは、右カメラ画像から求まる初期値（βｒ，ｘｒ）から（βｌ，ｘｌ）を求めると同時に、左カメラ画像から求まる初期値（βｌ，ｘｌ）から（βｒ，ｘｒ）を求め、それぞれの結果に基づき次の探索範囲を決定するというループを含む。このようにすることで、処理時間を短縮することができる。閾値の設定によっては、ループは１回で終わることもあり、その場合は１回分の探索時間しかかからない。
【0085】
上述した実施形態では、２Ｄ－３Ｄマッチングの実行時にモデルに回転及び平行移動を施しながらモデルを投影した参照画像を次々に生成している。他の実施形態では、モデルに回転及び平行移動を施してモデルを投影した参照画像をあらかじめ生成しておき、参照画像をモデル記憶部２２に格納しておいてもよい。
【0086】
モデルは、点の位置と点間の接続関係とにより物体の３Ｄ形状をモデル化したものであってもよい。本実施形態のように、カメラ画像をカラー画像とし、モデルを色付き点群データとすると、２Ｄ－３Ｄマッチングにおいてモデルの姿勢及びカメラ画像中の対応点の位置の検出精度を向上することができる。
【0087】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【0088】
１０…ロボットシステム、１１…ロボット、１２…ステレオカメラ、１２Ｌ…カメラ、１２Ｒ…カメラ、１３…コントローラ、１４…プロセッサ、１５…メモリ、１６…入出力インタフェース、２０…３Ｄ物体位置検出装置、２１…画像取得部、２２…モデル記憶部、２３…マッチング部、２４…算出部。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
