(57)【要約】【課題】対象物の形状が複雑な形状であっても、精度よく対象物の位置及び姿勢を算出することができるロボットを提供すること。【解決手段】対象物の三次元点群であるシーン点群が分割されたシーン分割領域のうち前記シーン点群を構成する点を所定数以上含む前記シーン分割領域であるシーンキューブ毎のシーンキューブ特徴量を算出し、算出した前記シーンキューブ特徴量と、前記対象物のモデルの三次元点群であるモデル点群が分割されたモデル分割領域のうち前記モデル点群を構成する点を前記所定数以上含む前記モデル分割領域であるモデルキューブ毎のモデルキューブ特徴量とに基づいて、前記モデルキューブに対応する前記シーンキューブを特定し、特定した当該シーンキューブに基づいて前記対象物の位置及び姿勢を算出する、ロボット制御装置。【選択図】図３
2017202549.tif 000002
【特許請求の範囲】
【請求項１】
対象物の三次元点群であるシーン点群が分割されたシーン分割領域のうち前記シーン点群を構成する点を所定数以上含む前記シーン分割領域であるシーンキューブ毎のシーンキューブ特徴量を算出し、
算出した前記シーンキューブ特徴量と、前記対象物のモデルの三次元点群であるモデル点群が分割されたモデル分割領域のうち前記モデル点群を構成する点を前記所定数以上含む前記モデル分割領域であるモデルキューブ毎のモデルキューブ特徴量とに基づいて、前記モデルキューブに対応する前記シーンキューブを特定し、特定した当該シーンキューブに基づいて前記対象物の位置及び姿勢を算出する、
ロボット制御装置。
【請求項２】
複数の前記シーンキューブそれぞれの相対的な位置関係に基づいて前記シーンキューブ特徴量を算出し、複数の前記モデルキューブの相対的な位置関係に基づいて前記モデルキューブ特徴量を算出する、
請求項１に記載のロボット制御装置。
【請求項３】
算出した前記対象物の位置及び姿勢に基づいてロボットを制御する、
請求項１又は２に記載のロボット制御装置。
【請求項４】
前記シーンキューブは、前記シーン点群のうちの一部分である部分点群を構成する点を前記所定数以上含む前記シーン分割領域であり、
前記シーン点群の中から曲率により面を構成する点群をシーンセグメントとして検出し、検出した１以上の前記シーンセグメントに基づいて前記シーン点群の中から前記モデル点群に対応する一部分を前記部分点群として検出する、
請求項１から３のうちいずれか一項に記載のロボット制御装置。
【請求項５】
検出した前記１以上の前記シーンセグメントに基づく特徴量であるシーンセグメント特徴量を算出し、前記モデル点群の中から曲率によって検出された面を構成する点群であるモデルセグメントに対応する前記シーンセグメントとして尤もらしい前記シーンセグメントである対象シーンセグメントを、算出した前記シーンセグメント特徴量に基づいて前記１以上の前記シーンセグメントの中から特定し、特定した前記対象シーンセグメントに基づいて前記シーン点群の中から前記モデル点群に対応する一部分を前記部分点群として検出する、
請求項４に記載のロボット制御装置。
【請求項６】
請求項１から５のうちいずれか一項に記載のロボット制御装置に制御され、
所定の作業を行う、
ロボット。
【請求項７】
請求項１から５のうちいずれか一項に記載のロボット制御装置と、
前記ロボット制御装置に制御されるロボットと、
を備えるロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボット制御装置、ロボット、及びロボットシステムに関する。
【背景技術】
【０００２】
対象物の位置及び姿勢を算出し、算出した当該位置及び姿勢に基づいて所定の作業をロボットに行わせる技術の研究や開発が行われている。
【０００３】
これに関し、対象物を含む二次元画像から対象物のエッジを検出し、検出したエッジと対象物の三次元幾何モデルの辺との対応関係に基づいて対象物の三次元位置及び姿勢を算出する情報処理システムが知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１４−４６４３３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、このような情報処理システムでは、対象物の形状が複雑な形状である場合、検出した対象物のエッジと対象物の三次元幾何モデルの辺とを対応付けることが困難な場合があった。その結果、当該情報処理システムでは、対象物の三次元位置及び姿勢を精度よく算出することができない場合があった。
【課題を解決するための手段】
【０００６】
上記課題の少なくとも一つを解決するために本発明の一態様は、対象物の三次元点群であるシーン点群が分割されたシーン分割領域のうち前記シーン点群を構成する点を所定数以上含む前記シーン分割領域であるシーンキューブ毎のシーンキューブ特徴量を算出し、算出した前記シーンキューブ特徴量と、前記対象物のモデルの三次元点群であるモデル点群が分割されたモデル分割領域のうち前記モデル点群を構成する点を前記所定数以上含む前記モデル分割領域であるモデルキューブ毎のモデルキューブ特徴量とに基づいて、前記モデルキューブに対応する前記シーンキューブを特定し、特定した当該シーンキューブに基づいて前記対象物の位置及び姿勢を算出する、ロボット制御装置である。
この構成により、ロボット制御装置は、シーンキューブ毎のシーンキューブ特徴量を算出し、算出したシーンキューブ特徴量と、モデルキューブ毎のモデルキューブ特徴量とに基づいて、モデルキューブに対応するシーンキューブを特定し、特定した当該シーンキューブに基づいて対象物の位置及び姿勢を算出する。これにより、ロボット制御装置は、対象物の形状が複雑な形状であっても、精度よく対象物の位置及び姿勢を算出することができる。
【０００７】
また、本発明の他の態様は、ロボット制御装置において、複数の前記シーンキューブそれぞれの相対的な位置関係に基づいて前記シーンキューブ特徴量を算出し、複数の前記モデルキューブの相対的な位置関係に基づいて前記モデルキューブ特徴量を算出する、構成が用いられてもよい。
この構成により、ロボット制御装置は、複数のシーンキューブそれぞれの相対的な位置関係に基づいてシーンキューブ特徴量を算出し、複数のモデルキューブの相対的な位置関係に基づいてモデルキューブ特徴量を算出する。これにより、ロボット制御装置は、複数のシーンキューブそれぞれの相対的な位置関係に基づいて算出したシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【０００８】
また、本発明の他の態様は、ロボット制御装置において、算出した前記対象物の位置及び姿勢に基づいてロボットを制御する、構成が用いられてもよい。
この構成により、ロボット制御装置は、算出した対象物の位置及び姿勢に基づいてロボットを制御する。これにより、ロボット制御装置は、算出した対象物の位置及び姿勢に基づいてロボットに精度の高い作業を行わせることができる。
【０００９】
また、本発明の他の態様は、ロボット制御装置において、前記シーンキューブは、前記シーン点群のうちの一部分である部分点群を構成する点を前記所定数以上含む前記シーン分割領域であり、前記シーン点群の中から曲率により面を構成する点群をシーンセグメントとして検出し、検出した１以上の前記シーンセグメントに基づいて前記シーン点群の中から前記モデル点群に対応する一部分を前記部分点群として検出する、構成が用いられてもよい。
この構成により、ロボット制御装置は、シーン点群の中から曲率により面を構成する点群をシーンセグメントとして検出し、検出した１以上のシーンセグメントに基づいてシーン点群の中からモデル点群に対応する一部分を部分点群として検出する。これにより、ロボット制御装置は、検出した部分点群に基づくシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【００１０】
また、本発明の他の態様は、ロボット制御装置において、検出した前記１以上の前記シーンセグメントに基づく特徴量であるシーンセグメント特徴量を算出し、前記モデル点群の中から曲率によって検出された面を構成する点群であるモデルセグメントに対応する前記シーンセグメントとして尤もらしい前記シーンセグメントである対象シーンセグメントを、算出した前記シーンセグメント特徴量に基づいて前記１以上の前記シーンセグメントの中から特定し、特定した前記対象シーンセグメントに基づいて前記シーン点群の中から前記モデル点群に対応する一部分を前記部分点群として検出する、構成が用いられてもよい。
この構成により、ロボット制御装置は、検出した１以上のシーンセグメントに基づく特徴量であるシーンセグメント特徴量を算出し、モデル点群の中から曲率によって検出された面を構成する点群であるモデルセグメントに対応するシーンセグメントとして尤もらしいシーンセグメントである対象シーンセグメントを、算出したシーンセグメント特徴量に基づいて１以上のシーンセグメントの中から特定し、特定した対象シーンセグメントに基づいてシーン点群の中からモデル点群に対応する一部分を部分点群として検出する。これにより、ロボット制御装置は、対象シーンセグメントに基づくシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【００１１】
また、本発明の他の態様は、上記に記載のロボット制御装置に制御され、所定の作業を行う、ロボットである。
この構成により、ロボットは、上記に記載のロボット制御装置に制御され、所定の作業を行う。これにより、ロボットは、対象物の形状が複雑な形状であっても、所定の作業を精度よく行うことができる。
【００１２】
また、本発明の他の態様は、上記に記載のロボット制御装置と、前記ロボット制御装置に制御されるロボットと、を備えるロボットシステムである。
この構成により、ロボットシステムは、シーンキューブ毎のシーンキューブ特徴量を算出し、算出したシーンキューブ特徴量と、モデルキューブ毎のモデルキューブ特徴量とに基づいて、モデルキューブに対応するシーンキューブを特定し、特定した当該シーンキューブに基づいて対象物の位置及び姿勢を算出する。これにより、ロボットシステムは、対象物の形状が複雑な形状であっても、精度よく対象物の位置及び姿勢を算出することができる。
【００１３】
以上により、ロボット制御装置、及びロボットシステムは、シーンキューブ毎のシーンキューブ特徴量を算出し、算出したシーンキューブ特徴量と、モデルキューブ毎のモデルキューブ特徴量とに基づいて、モデルキューブに対応するシーンキューブを特定し、特定した当該シーンキューブに基づいて対象物の位置及び姿勢を算出する。これにより、ロボット制御装置、及びロボットシステムは、対象物の形状が複雑な形状であっても、精度よく対象物の位置及び姿勢を算出することができる。
また、ロボットは、上記に記載のロボット制御装置に制御され、所定の作業を行う。これにより、ロボットは、対象物の形状が複雑な形状であっても、所定の作業を精度よく行うことができる。
【図面の簡単な説明】
【００１４】
【図１】実施形態に係るロボットシステム１の構成の一例を示す図である。
【図２】ロボット制御装置３０のハードウェア構成の一例を示す図である。
【図３】ロボット制御装置３０の機能構成の一例を示す図である。
【図４】ロボット２０に所定の作業を行わせる際にロボット制御装置３０が行う処理の流れの一例を示すフローチャートである。
【図５】対象物Ｖの一例を示す図である。
【図６】シーン点群の一例を示す図である。
【図７】モデルセグメントペアに対応するシーンセグメントペアと、モデルセグメントペアに対応しないシーンセグメントペアとのそれぞれを例示する図である。
【図８】ステップＳ１８０において対象点群検出部３７２が検出した対象点群の一例を示す図である。
【図９】点群ＰＰを含む領域において平面的に分布しているシーンキューブを例示する図である。
【図１０】対象シーンキューブの方向とロボット座標系ＲＣにおけるＺ軸の正方向とを一致させた場合における図９に示したシーンキューブを例示する図である。
【図１１】座標変換した各点に基づいて新たに検出されたシーンキューブを例示する図である。
【発明を実施するための形態】
【００１５】
＜実施形態＞
以下、本発明の実施形態について、図面を参照して説明する。
【００１６】
＜ロボットシステムの構成＞
まず、ロボットシステム１の構成について説明する。
図１は、実施形態に係るロボットシステム１の構成の一例を示す図である。ロボットシステム１は、撮像部１０と、ロボット２０と、ロボット制御装置３０を備える。
【００１７】
撮像部１０は、例えば、集光された光を電気信号に変換する撮像素子であるＣＣＤ（Charge  Coupled  Device）やＣＭＯＳ（Complementary  Metal  Oxide  Semiconductor）等を備えたステレオカメラである。なお、撮像部１０は、ステレオカメラに代えて、ライトフィールドカメラ等の三次元画像を撮像可能な他のカメラであってもよい。撮像部１０は、図１に示した作業台ＴＢの上面にばら積みされたＮ個の対象物Ｏである対象物Ｏ１〜対象物ＯＮを含む範囲を撮像範囲として撮像可能な位置に設置される。ここで、Ｎは、１以上の整数である。
【００１８】
作業台ＴＢは、例えば、テーブル等の台である。なお、作業台ＴＢは、テーブルに代えて、Ｎ個の対象物Ｏをばら積みすることが可能な台であれば、他の台であってもよい。対象物Ｏは、例えば、製品に組み付けるプレート、ネジ、ボルト等の産業用の部品や部材である。以下では、一例として、対象物Ｏがネジの場合について説明する。なお、対象物Ｏは、産業用の部品や部材に代えて、日用品や生体等の他の物体であってもよい。
【００１９】
撮像部１０は、ケーブルによってロボット制御装置３０と通信可能に接続されている。なお、ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ（Universal  Serial  Bus）等の規格によって行われる。また、撮像部１０は、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によってロボット制御装置３０と接続される構成であってもよい。
【００２０】
ロボット２０は、アームＡと、アームＡを支持する支持台Ｂを備える単腕ロボットである。単腕ロボットは、この一例におけるアームＡのような１本のアーム（腕）を備えるロボットである。なお、ロボット２０は、単腕ロボットに代えて、複腕ロボットであってもよい。複腕ロボットは、２本以上のアーム（例えば、２本以上のアームＡ）を備えるロボットである。なお、複腕ロボットのうち、２本のアームを備えるロボットは、双腕ロボットとも称される。すなわち、ロボット２０は、２本のアームを備える双腕ロボットであってもよく、３本以上のアーム（例えば、３本以上のアームＡ）を備える複腕ロボットであってもよい。また、ロボット２０は、スカラロボットや、円筒型ロボット等の他のロボットであってもよい。
【００２１】
アームＡは、エンドエフェクターＥと、マニピュレーターＭを備える。
エンドエフェクターＥは、この一例において、物体を把持可能な指部を備えるエンドエフェクターである。なお、エンドエフェクターＥは、当該指部を備えるエンドエフェクターに代えて、空気の吸引や磁力、治具等によって物体を持ち上げることが可能なエンドエフェクターや、他のエンドエフェクターであってもよい。
【００２２】
エンドエフェクターＥは、ケーブルによってロボット制御装置３０と通信可能に接続されている。これにより、エンドエフェクターＥは、ロボット制御装置３０から取得される制御信号に基づく動作を行う。なお、ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。また、エンドエフェクターＥは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によってロボット制御装置３０と接続される構成であってもよい。
【００２３】
マニピュレーターＭは、６つの関節を備える。また、当該６つの関節はそれぞれ、図示しないアクチュエーターを備える。すなわち、マニピュレーターＭを備えるアームＡは、６軸垂直多関節型のアームである。アームＡは、支持台Ｂと、エンドエフェクターＥと、マニピュレーターＭと、マニピュレーターＭが備える６つの関節それぞれのアクチュエーターとによる連携した動作によって６軸の自由度の動作を行う。なお、アームＡは、５軸以下の自由度で動作する構成であってもよく、７軸以上の自由度で動作する構成であってもよい。
【００２４】
マニピュレーターＭが備える６つの（関節に備えられた）アクチュエーターはそれぞれ、ケーブルによってロボット制御装置３０と通信可能に接続されている。これにより、当該アクチュエーターは、ロボット制御装置３０から取得される制御信号に基づいて、マニピュレーターＭを動作させる。なお、ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。また、マニピュレーターＭが備える６つのアクチュエーターのうちの一部又は全部は、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によってロボット制御装置３０と接続される構成であってもよい。
【００２５】
ロボット制御装置３０は、この一例において、ロボットコントローラーである。ロボット制御装置３０は、予め入力された動作プログラムに基づいて制御信号を生成する。ロボット制御装置３０は、生成した制御信号をロボット２０に送信し、ロボット２０に所定の作業を行わせる。以下では、説明の便宜上、ロボット制御装置３０による当該制御信号の生成及び送信についての説明を省略し、ロボット制御装置３０がロボット２０に行わせる動作、及びロボット２０を動作させる際にロボット制御装置３０が行う処理について説明する。
【００２６】
所定の作業は、この一例において、ロボット２０がアームＡを動作させ、作業台ＴＢの上面にばら積みされた対象物Ｏを１つずつ把持し、把持した対象物Ｏを図示しない給材領域に配置する作業である。なお、所定の作業は、これに代えて、他の作業であってもよい。
【００２７】
＜ロボットを動作させる際にロボット制御装置が行う処理の概要＞
以下、ロボット２０に所定の作業を行わせる際にロボット制御装置３０が行う処理の概要について説明する。
【００２８】
ロボット制御装置３０は、撮像部１０に前述の撮像領域をステレオ撮像させる。ロボット制御装置３０は、撮像部１０がステレオ撮像した撮像画像を撮像部１０から取得する。ロボット制御装置３０は、撮像部１０から取得した撮像画像に基づいて、作業台ＴＢの上面にばら積みされたＮ個の対象物Ｏの三次元点群であるシーン点群を生成する。
【００２９】
より具体的には、ロボット制御装置３０は、当該撮像画像に基づいて、当該撮像画像に含まれる物体の表面の各点のロボット座標系ＲＣにおける三次元位置（三次元座標）を検出する。ロボット制御装置３０は、検出した当該三次元位置に基づいて当該表面の三次元形状を表す点群である三次元点群をシーン点群として生成する。すなわち、シーン点群を構成する各点は、ロボット座標系ＲＣにおける三次元位置を示す。ここで、当該物体には、撮像範囲に含まれる作業台ＴＢの上面の少なくとも一部と、Ｎ個の対象物Ｏとが含まれる。なお、ロボット制御装置３０が撮像画像からシーン点群を生成する方法は、既知の方法であってもよく、これから開発される方法であってもよいため説明を省略する。また、ロボット制御装置３０は、ＴｏＦ（Time  of  Flight）や三角測量法等の従来の手法に基づいて距離画像を生成する他の装置から、シーン点群を取得する構成であってもよい。
【００３０】
ロボット制御装置３０は、生成したシーン点群に基づいて、１以上のシーンキューブ毎に、シーンキューブの特徴量であるシーンキューブ特徴量を算出する。シーンキューブは、当該シーン点群を含む領域が分割された分割領域であるシーン分割領域のうち当該シーン点群を構成する点を所定数以上含むシーン分割領域のことである。当該領域は、ロボット制御装置３０がロボット制御装置３０の記憶領域内に生成した仮想的な領域である。所定数は、この一例において、３である。なお、所定数は、３以上の数であれば他の数であってもよい。
【００３１】
ロボット制御装置３０は、算出したシーンキューブ特徴量と、モデルキューブ特徴量とに基づいて、モデルキューブに対応するシーンキューブである対応キューブを特定する。モデルキューブ特徴量は、モデルキューブ毎の特徴量のことである。モデルキューブは、対象物Ｏのモデルの三次元点群であるモデル点群を含む領域が分割された分割領域であるモデル分割領域のうちモデル点群を構成する点を所定数以上含むモデル分割領域のことである。当該領域は、ロボット制御装置３０がロボット制御装置３０の記憶領域内に生成した仮想的な領域である。当該モデルは、この一例において、対象物ＯのＣＡＤ（Computer  Aided  Design）モデルに基づいて生成されたＣＧ（Computer1  Graphics）のことである。モデル点群は、当該モデルの表面の各点の当該領域における三次元位置（三次元座標）に基づいて生成された点群であって当該表面の三次元形状を表す三次元点群である。なお、モデル点群は、過去に対象物Ｏがステレオ撮像された撮像画像に基づいて生成された三次元点群であってもよい。ロボット制御装置３０は、特定した対応キューブに基づいて対象物Ｏの位置及び姿勢を算出する。これにより、ロボット制御装置３０は、対象物Ｏの形状が複雑な形状であっても、精度よく対象物Ｏの位置及び姿勢を算出することができる。
【００３２】
ここで、対象物Ｏの位置及び姿勢は、対象物Ｏの重心の三次元位置及び三次元姿勢によって表される。より具体的には、対象物Ｏの位置は、この一例において、対象物Ｏの重心に設定された三次元局所座標系の原点のロボット座標系ＲＣにおける位置（当該三次元位置）によって表される。また、対象物Ｏの姿勢は、この一例において、当該三次元局所座標系における各座標軸のロボット座標系ＲＣにおける方向（当該三次元姿勢）によって表される。
【００３３】
また、この一例において、ロボット制御装置３０は、シーン点群と、シーンキューブと、シーンキューブ特徴量とを算出する方法と同様の方法によって、対象物Ｏのモデルに基づいて、モデル点群と、モデルキューブと、モデルキューブ特徴量とを予め算出する。そして、ロボット制御装置３０は、算出したモデル点群と、モデルキューブと、モデルキューブ特徴量とを予め記憶する。なお、ロボット制御装置３０は、モデル点群と、モデルキューブと、モデルキューブ特徴量とを他の装置から取得する構成であってもよい。この場合、当該装置は、当該方法によってモデル点群と、モデルキューブと、モデルキューブ特徴量とを検出する。
【００３４】
ロボット制御装置３０は、算出した対象物Ｏの位置及び姿勢に基づいて、ロボット２０に所定の作業を行わせ、対象物Ｏを図示しない給材領域に配置させる。
以下では、ロボット２０に所定の作業を行わせる際にロボット制御装置３０が行う処理について詳しく説明する。
【００３５】
＜ロボット制御装置のハードウェア構成＞
以下、図２を参照し、ロボット制御装置３０のハードウェア構成について説明する。図２は、ロボット制御装置３０のハードウェア構成の一例を示す図である。
【００３６】
ロボット制御装置３０は、例えば、ＣＰＵ（Central  Processing  Unit）３１と、記憶部３２と、入力受付部３３と、通信部３４と、表示部３５を備える。これらの構成要素は、バスＢｕｓを介して相互に通信可能に接続されている。また、ロボット制御装置３０は、通信部３４を介してロボット２０と通信を行う。
【００３７】
ＣＰＵ３１は、記憶部３２に格納された各種プログラムを実行する。
記憶部３２は、例えば、ＨＤＤ（Hard  Disk  Drive）やＳＳＤ（Solid  State  Drive）、ＥＥＰＲＯＭ（Electrically  Erasable  Programmable  Read−Only  Memory）、ＲＯＭ（Read−Only  Memory）、ＲＡＭ（Random  Access  Memory）等を含む。なお、記憶部３２は、ロボット制御装置３０に内蔵されるものに代えて、ＵＳＢ等のデジタル入出力ポート等によって接続された外付け型の記憶装置であってもよい。記憶部３２は、ロボット制御装置３０が処理する各種情報や画像、動作プログラムを含む各種のプログラム等を格納する。
【００３８】
入力受付部３３は、例えば、表示部３５と一体に構成されたタッチパネルである。なお、入力受付部３３は、キーボードやマウス、タッチパッド、その他の入力装置であってもよい。
通信部３４は、例えば、ＵＳＢ等のデジタル入出力ポートやイーサネット（登録商標）ポート等を含んで構成される。
表示部３５は、例えば、液晶ディスプレイパネル、あるいは、有機ＥＬ（ElectroLuminescence）ディスプレイパネルである。
【００３９】
＜ロボット制御装置の機能構成＞
以下、図３を参照し、ロボット制御装置３０の機能構成について説明する。図３は、ロボット制御装置３０の機能構成の一例を示す図である。
【００４０】
ロボット制御装置３０は、記憶部３２と、制御部３６を備える。
【００４１】
制御部３６は、ロボット制御装置３０の全体を制御する。制御部３６は、撮像制御部３６１と、画像取得部３６４と、点群生成部３６６と、対象点群検出部３７２と、キューブ検出部３７４と、キューブ特徴量算出部３７６と、第１位置姿勢算出部３８０と、第２位置姿勢算出部３８２と、ロボット制御部３８４を備える。制御部３６が備えるこれらの機能部は、例えば、ＣＰＵ３１が、記憶部３２に記憶された各種プログラムを実行することにより実現される。また、当該機能部のうちの一部又は全部は、ＬＳＩ（Large  Scale  Integration）やＡＳＩＣ（Application  Specific  Integrated  Circuit）等のハードウェア機能部であってもよい。
【００４２】
撮像制御部３６１は、撮像部１０が撮像可能な範囲である撮像範囲を撮像部１０にステレオ撮像させる。
画像取得部３６４は、撮像部１０が撮像した撮像画像（三次元画像）を撮像部１０から取得する。
点群生成部３６６は、画像取得部３６４が取得した撮像画像に基づいてシーン点群を生成する。また、点群生成部３６６は、記憶部３２に予め記憶された対象物Ｏのモデルに基づいてモデル点群を生成する。
【００４３】
対象点群検出部３７２は、シーン点群から対象点群を検出する。対象点群は、シーン点群の一部分である部分点群のうちキューブ検出部３７４が前述のシーンキューブを検出する対象とする部分点群のことである。
【００４４】
キューブ検出部３７４は、対象点群検出部３７２が検出した対象点群に基づいて前述のシーンキューブを検出する。また、キューブ検出部３７４は、モデル点群に基づいて前述のモデルキューブを検出する。
キューブ特徴量算出部３７６は、キューブ検出部３７４が検出したシーンキューブに基づいて、シーンキューブ毎にシーンキューブ特徴量を算出する。また、キューブ特徴量算出部３７６は、キューブ検出部３７４が検出したモデルキューブに基づいて、モデルキューブ毎にモデルキューブ特徴量を算出する。
【００４５】
第１位置姿勢算出部３８０は、キューブ特徴量算出部３７６が算出したシーンキューブ特徴量及びモデルキューブ特徴量に基づいて、キューブ検出部３７４が検出したモデルキューブに対応するシーンキューブを対応キューブとして特定する。第１位置姿勢算出部３８０は、特定した対応キューブと、当該モデルキューブとに基づいて、対象物Ｏの位置及び姿勢を算出する第１位置姿勢算出処理を行う。
【００４６】
第２位置姿勢算出部３８２は、第１位置姿勢算出部３８０が対象物Ｏの位置及び姿勢を算出した結果と、ＩＣＰ（Iterative  Closest  Point）アルゴリズムとに基づいて、対象物Ｏの位置及び姿勢を第１位置姿勢算出処理よりも高い精度で算出する第２位置姿勢算出処理を行う。
ロボット制御部３８４は、第２位置姿勢算出部３８２が算出した対象物Ｏの位置及び姿勢に基づいてロボット２０に所定の作業を行わせる。なお、ロボット制御部３８４は、第１位置姿勢算出部３８０が算出した対象物Ｏの位置及び姿勢に基づいてロボット２０に所定の作業を行わせる構成であってもよい。この場合、制御部３６は、第２位置姿勢算出部３８２を備えない構成であってもよい。
【００４７】
＜ロボットに所定の作業を行わせる際にロボット制御装置が行う処理＞
以下、図４を参照し、ロボット２０に所定の作業を行わせる際にロボット制御装置３０が行う処理について説明する。図４は、ロボット２０に所定の作業を行わせる際にロボット制御装置３０が行う処理の流れの一例を示すフローチャートである。
【００４８】
なお、以下では、当該処理のうちの対象物Ｏの位置及び姿勢を算出する処理におけるシーン点群の取り扱いを明確に示すため、作業台ＴＢの上面にばら積みされた物体がＮ個の対象物ＯではなくＮ個の対象物Ｖである場合について説明する。ここで、図５を参照し、対象物Ｖについて説明する。図５は、対象物Ｖの一例を示す図である。図５に示した例では、対象物Ｖは、リブＶＲを有するシングルリブタイプのイケールである。
【００４９】
また、以下では、一例として、所定の作業においてロボット２０がＮ個の対象物Ｖから１個の対象物Ｖを把持し、把持した対象物Ｖを図示しない給材領域に配置する場合について説明する。なお、所定の作業においてロボット２０がＮ個の対象物Ｖから複数個の対象物Ｖを１個ずつ把持し、把持した対象物Ｖを当該給材領域に配置する場合、ロボット制御装置３０は、図４に示したフローチャートの処理の一部又は全部を当該複数回繰り返す。図４に示したフローチャートの処理の一部を当該複数回繰り返す場合、例えば、２回目以降の処理では、ステップＳ１４０〜ステップＳ２６０の処理を繰り返してもよい。
【００５０】
撮像制御部３６１は、撮像部１０に撮像範囲をステレオ撮像させる（ステップＳ１１０）。次に、画像取得部３６４は、ステップＳ１１０において撮像部１０がステレオ撮像した撮像画像を撮像部１０から取得する（ステップＳ１２０）。次に、点群生成部３６６は、ステップＳ１２０において画像取得部３６４が取得した撮像画像に基づいてシーン点群を生成する（ステップＳ１３０）。
【００５１】
次に、対象点群検出部３７２は、ステップＳ１３０において点群生成部３６６が生成したシーン点群に基づいて、シーン点群から１以上のシーンセグメントを検出する（ステップＳ１４０）。シーンセグメントは、シーン点群の一部分である部分点群のうち所定条件を満たす１以上の面を構成する部分点群のことである。所定条件は、以下に示した２つの条件である条件１）、条件２）のうち少なくとも一方を満たすことである。
【００５２】
条件１）一つながりの面であって曲率が一定の面
条件２）一つながりの面であって曲率が所定値未満の面
【００５３】
なお、曲率が０以外の場合、これらの面は曲面であり、曲率が０の場合、当該面は平面である。シーンセグメントは、ステップＳ１６０〜ステップＳ１８０において対象点群検出部３７２により、シーン点群から前述の対象点群を検出するため用いられる。ここで、図６を参照し、ステップＳ１４０の処理について説明する。
【００５４】
図６は、シーン点群の一例を示す図である。図６において、対象物Ｖの表面の三次元形状を示す三次元点群を明確に示すため、シーン点群を構成する点のうちのロボット座標系ＲＣにおけるＺ軸方向の位置である高さが作業台ＴＢの上面以下の点を省略している。図６に示した例では、図６に示したシーン点群には、前述の所定条件を満たす面を構成する部分点群が５つ含まれている。対象点群検出部３７２は、これらの部分点群を、シーンセグメントＳ１〜シーンセグメントＳ５の５つのシーンセグメントとして検出する。
【００５５】
ステップＳ１４０において対象点群検出部３７２が１以上のシーンセグメントを検出した後、対象点群検出部３７２は、当該シーンセグメントに基づいてシーンセグメント特徴量を算出する（ステップＳ１５０）。シーンセグメント特徴量は、１以上のシーンセグメントに基づいて算出される特徴量のことである。ここで、ステップＳ１５０の処理について説明する。
【００５６】
以下では、一例として、対象点群検出部３７２が、ステップＳ１４０において検出した１以上のシーンセグメントの中から選択された２つのシーンセグメントの組み合わせであるシーンセグメントペア毎にシーンセグメント特徴量を算出する場合について説明する。なお、この場合、ステップＳ１４０において検出されたシーンセグメントの数は、２以上でなければならないため、以下では、ステップＳ１４０において２以上のシーンセグメントが検出された場合について説明する。
【００５７】
対象点群検出部３７２は、ステップＳ１４０において検出した２以上のシーンセグメントのそれぞれを示すシーンセグメント情報を生成する。シーンセグメント情報は、この一例において、各シーンセグメントを示すインデックス数である。対象点群検出部３７２は、例えば、当該２以上のシーンセグメントの中から２つのシーンセグメントの組み合わせであるシーンセグメントペアを１つずつ選択する。シーンセグメントペアの数は、シーンセグメントの数がＷの場合、ＷＣ２個である。対象点群検出部３７２は、選択したシーンセグメントペアと、以下に示した式（１）とに基づいて、当該シーンセグメントペアのシーンセグメント特徴量（当該シーンセグメントペアに基づくシーンセグメント特徴量）を算出する。
【００５８】
【数１】
2017202549.tif 000003
【００５９】
ここで、ｉは、対象点群検出部３７２が選択したシーンセグメントペアに含まれるシーンセグメントのうちの１つ目のシーンセグメントを示すシーンセグメント情報である。ｊは、対象点群検出部３７２が選択したシーンセグメントペアに含まれるシーンセグメントのうちの２つ目のシーンセグメントを示すシーンセグメント情報である。Ｆｉｊは、ｉが示すシーンセグメントである第１シーンセグメントと、ｊが示すシーンセグメントである第２シーンセグメントとのシーンセグメントペアに基づくシーンセグメント特徴量を表すベクトルである。ａｉは、第１シーンセグメントの面積である。ａｊは、第２シーンセグメントの面積である。ｃｉ１は、第１シーンセグメントの主曲率のうちの小さい方の主曲率である。ｃｉ２は、第１シーンセグメントの主曲率のうちの大きい方の主曲率である。ｃｊ１は、第２シーンセグメントの主曲率のうちの小さい方の主曲率である。ｃｊ２は、第２シーンセグメントの主曲率のうちの大きい方の主曲率である。ｄｉｊは、第１シーンセグメントを構成する点と第２シーンセグメントを構成する点との間の距離のうちの最も短い距離であるセグメント間距離である。
【００６０】
対象点群検出部３７２は、ステップＳ１５０において、ＷＣ２個のシーンセグメントペアの中から選択したシーンセグメントペアに基づいて、第１シーンセグメントの面積と、第２シーンセグメントの面積と、第１シーンセグメントの主曲率と、第２シーンセグメントの主曲率と、セグメント間距離とを算出する。そして、対象点群検出部３７２は、算出したこれらの値に基づいて、当該シーンセグメントペアのシーンセグメント特徴量を算出する。対象点群検出部３７２は、このようなシーンセグメント特徴量の算出処理を、ＷＣ２個のシーンセグメントペア毎に行うことにより、ＷＣ２個のシーンセグメントペア毎のシーンセグメント特徴量を算出する。なお、上記の式（１）がベクトルであることからも自明なように、Ｆｉｊ≠Ｆｊｉである。
【００６１】
なお、対象点群検出部３７２は、ステップＳ１４０において検出した１以上のシーンセグメントのそれぞれ毎にシーンセグメント特徴量を算出する構成であってもよい。この場合、ステップＳ１４０において検出されたシーンセグメントの数は、１以上でよい。また、当該場合、シーンセグメント特徴量を表すベクトルには、各シーンセグメントの面積及び主曲率が含まれる。
【００６２】
また、対象点群検出部３７２は、ステップＳ１４０において検出した複数のシーンセグメントの中から選択された３以上のシーンセグメントの組み合わせ毎にシーンセグメント特徴量を算出する構成であってもよい。この場合、ステップＳ１４０において検出されたシーンセグメントの数は、当該３以上でなければならない。また、当該場合、シーンセグメント特徴量を表すベクトルには、３以上のシーンセグメントの組み合わせに含まれる各シーンセグメントの面積及び主曲率と、当該各シーンセグメント間のセグメント間距離とが含まれる。
【００６３】
ステップＳ１５０においてシーンセグメントペア毎のシーンセグメント特徴量を算出した後、対象点群検出部３７２は、記憶部３２に予め記憶された第１ランダムフォレストと、当該シーンセグメント特徴量とに基づいて、シーン点群の中から対象点群の候補を検出する対象点群候補検出処理を行う（ステップＳ１６０）。ここで、ステップＳ１６０の処理について説明する。
【００６４】
まず、第１ランダムフォレストについて説明する。
第１ランダムフォレストは、モデルセグメントペアの特徴量であるモデルセグメント特徴量（モデルセグメントペアに基づくモデルセグメント特徴量）に基づいて、あるシーンセグメント特徴量のシーンセグメントペアに対応するモデルセグメントペアとして尤もらしいモデルセグメントペア情報を出力する関数である。第１ランダムフォレストには、複数のモデルセグメントペア情報のそれぞれに対してモデルセグメント特徴量が対応付けられた情報である第１対応情報がユーザーにより予め記憶される（学習させられる）。
【００６５】
モデルセグメントペアは、モデル点群から検出された２以上のモデルセグメントの中から選択された２つのモデルセグメントの組み合わせである。モデルセグメントは、モデル点群に基づいてモデル点群から検出される面のうちの前述の所定条件を満たす面を構成する点群のことである。モデルセグメントペア情報は、モデルセグメントペアを示す情報である。モデルセグメントペアの数は、モデル点群から検出されたモデルセグメントの数がＬ個の場合、ＬＣ２個である。あるモデルセグメントペアのモデルセグメント特徴量は、当該モデルセグメントペアと上記の式（１）とに基づいて算出された特徴量のことである。
【００６６】
なお、モデルセグメントは、対象点群検出部３７２によって予め検出される。対象点群検出部３７２は、モデル点群に基づいて、モデル点群から前述の所定条件を満たす２以上の面を構成する点群のそれぞれをモデルセグメントとして検出する。対象点群検出部３７２がモデル点群に基づいてモデルセグメントを検出する方法は、対象点群検出部３７２がシーン点群に基づいてシーンセグメントを検出する方法と同様の方法であるため説明を省略する。また、モデルセグメント特徴量は、対象点群検出部３７２によって予め算出される。対象点群検出部３７２は、検出したモデルセグメントに基づいてモデルセグメント特徴量を算出する。対象点群検出部３７２がモデルセグメントに基づいてモデルセグメント特徴量を算出する方法は、対象点群検出部３７２がシーンセグメントに基づいてシーンセグメント特徴量を算出する方法と同様の方法であるため説明を省略する。
【００６７】
第１ランダムフォレストは、あるシーンセグメントペアに基づいて算出されたシーンセグメント特徴量が対象点群検出部３７２により入力されると、第１対応情報に基づいて、当該シーンセグメントペアに対応することが最も尤もらしいモデルセグメントペアを示すモデルセグメントペア情報と、当該モデルセグメントペアが当該シーンセグメントペアに対応することの尤もらしさを示す尤度とを対象点群検出部３７２に出力する。
【００６８】
このような第１ランダムフォレストによって、対象点群検出部３７２は、シーン点群から対象点群の候補を検出する。具体的には、対象点群検出部３７２は、あるシーンセグメント特徴量を第１ランダムフォレストに入力し、第１ランダムフォレストから出力される前述の尤度とモデルセグメントペア情報とを第１ランダムフォレストから取得する。対象点群検出部３７２は、当該尤度が所定閾値未満である場合、当該シーンセグメントペアに対応するモデルセグメントペアがないと判定する。すなわち、当該場合、対象点群検出部３７２は、当該シーンセグメントペア（に含まれる２つのシーンセグメント）が対象点群の候補ではないと判定する。一方、対象点群検出部３７２は、第１ランダムフォレストから出力された尤度が所定閾値以上である場合、第１ランダムフォレストに入力したシーンセグメントペア（に含まれる２つのシーンセグメント）を対象点群の候補として検出（特定）する。また、対象点群検出部３７２は、検出した当該候補に、当該シーンセグメントペアを示すシーンセグメントペア情報と、第１ランダムフォレストから当該尤度とともに出力されたモデルセグメントペア情報と、当該尤度とを対応付ける。
【００６９】
ここで、モデルセグメントペアに含まれる２個のモデルセグメントは、元々１つの対象物Ｖのモデルの三次元形状を表すモデル点群から検出されたモデルセグメントである。このため、あるモデルセグメントペアに対応するシーンセグメントペアに含まれる２つのシーンセグメントは、シーン点群に含まれるモデル点群に対応する１以上の部分点群のいずれかから検出された可能性が高い。これは、当該シーンセグメントペアに基づいて算出されるシーンセグメント特徴量が、当該モデルセグメントペアのモデルセグメント特徴量とほぼ一致する（又は差分が所定値未満となる）ためである。このため、対象点群検出部３７２は、第１ランダムフォレストから出力された尤度が所定閾値以上である場合、第１ランダムフォレストに入力したシーンセグメントペアを対象点群の候補として検出（特定）する。
【００７０】
対象点群検出部３７２は、ステップＳ１５０において算出したシーンセグメント特徴量のそれぞれに対して、このような対象点群候補検出処理を行う。図７は、モデルセグメントペアに対応するシーンセグメントペアと、モデルセグメントペアに対応しないシーンセグメントペアとのそれぞれを例示する図である。図７に示した例では、対象点群検出部３７２は、シーンセグメントＳ２とシーンセグメントＳ５とのシーンセグメントペアＣＣ１がシーン点群のうちのモデル点群に対応する部分点群から検出された可能性が高いと第１ランダムフォレストによって判定され、シーンセグメントペアＣＣ１を対象点群の候補として検出する。また、当該例では、対象点群検出部３７２は、シーンセグメントＳ３とシーンセグメントＳ４とのシーンセグメントペアＣＣ２がシーン点群のうちのモデル点群に対応する部分点群から検出された可能性が高いと第１ランダムフォレストによって判定され、シーンセグメントペアＣＣ２を対象点群の候補として検出する。一方、当該例では、対象点群検出部３７２は、シーンセグメントＳ４とシーンセグメントＳ５とのシーンセグメントペアＮＣ１がシーン点群のうちのモデル点群に対応する部分点群から検出された可能性が低いと第１ランダムフォレストによって判定され、シーンセグメントペアＮＣ１を対象点群の候補として検出しない。また、当該例では、対象点群検出部３７２は、シーンセグメントＳ２とシーンセグメントＳ４とのシーンセグメントペアＮＣ２がシーン点群のうちのモデル点群に対応する部分点群から検出された可能性が低いと第１ランダムフォレストによって判定され、シーンセグメントペアＮＣ２を対象点群の候補として検出しない。
【００７１】
ステップＳ１６０において対象点群候補検出処理を実行した後、対象点群検出部３７２は、ステップＳ１６０において対象点群の候補が検出されたか否かを判定する（ステップＳ１７０）。ステップＳ１６０において対象点群の候補が検出されていないと対象点群検出部３７２が判定した場合（ステップＳ１７０−ＮＯ）、ロボット制御部３８４は、例えば、作業台ＴＢの部位のうちの予め決められた部位をエンドエフェクターＥにより把持し、作業台ＴＢを振動させる（ステップＳ２６０）。これにより、ロボット制御部３８４は、作業台ＴＢの上面にばら積みされたＮ個の対象物Ｖの配置を変えることができる。そして、撮像制御部３６１は、ステップＳ１１０に遷移し、再び撮像部１０に撮像範囲をステレオ撮像させる。一方、ステップＳ１６０において対象点群の候補が検出されていると判定した場合（ステップＳ１７０−ＹＥＳ）、対象点群検出部３７２は、ステップＳ１６０において検出した１以上の対象点群の候補のそれぞれに対応付けられた尤度のうち第２所定条件を満たす尤度に対応付けられた対象点群の候補を対象点群として特定する（ステップＳ１８０）。第２所定条件は、例えば、最も高い尤度であることであるが、これに代えて、他の条件であってもよい。
【００７２】
このように、対象点群検出部３７２は、ステップＳ１６０〜ステップＳ１８０の処理によって、１以上のシーンセグメントペアに基づくシーンセグメント特徴量を算出し、モデルセグメントペアに対応するシーンセグメントペアとして尤もらしいシーンセグメントペア（すなわち、この一例における対象点群の候補）を、算出したシーンセグメント特徴量に基づいて１以上のシーンセグメントペアの中から特定し、特定したシーンセグメントペアに基づいてシーン点群の中からモデル点群に対応する一部分を部分点群（すなわち、この一例における対象点群）として検出する。これにより、ロボット制御装置３０は、特定したシーンセグメントペアに基づくシーンキューブ特徴量に基づいて、精度よく対象物Ｖの位置及び姿勢を算出することができる。なお、対象点群の候補は、対象シーンセグメントの一例である。
【００７３】
次に、キューブ検出部３７４は、ステップＳ１８０において対象点群検出部３７２が検出した対象点群に基づいてシーンキューブを検出する（ステップＳ１９０）。ここで、図８を参照し、ステップＳ１９０の処理について説明する。図８は、ステップＳ１８０において対象点群検出部３７２が検出した対象点群の一例を示す図である。図８に示した点群ＰＰは、ステップＳ１８０において対象点群検出部３７２が検出した対象点群の一例である。キューブ検出部３７４は、点群ＰＰを含む領域を複数の立方体形状の分割領域に分割する。当該領域は、記憶部３２の記憶領域内に生成された仮想的な領域である。当該領域における位置は、ロボット座標系ＲＣにおける位置（座標）によって表される。図８に示した点線は、当該分割領域のうちの一部の分割領域の境界を示している。
【００７４】
点群ＰＰを含む領域を前述の分割領域に分割する際、キューブ検出部３７４は、ロボット座標系ＲＣにおけるＸ軸、Ｙ軸、Ｚ軸それぞれにおいて、点群ＰＰを含む範囲を算出する。当該範囲は、例えば、最小値と最大値によって表される範囲である。キューブ検出部３７４は、算出したＸ軸の範囲とＹ軸の範囲とＺ軸の範囲とのうちの最も狭い範囲を特定する。キューブ検出部３７４は、特定した当該範囲を第２所定数の分割範囲に等分割する。第２所定数は、例えば、２０である。なお、第２所定数は、これに代えて、２０より小さな数であってもよく、２０より大きな数であってもよい。キューブ検出部３７４は、当該分割範囲の長さを１辺の長さとする立方体形状の分割領域によってロボット座標系ＲＣにおける点群ＰＰを含む領域を分割する。この際、キューブ検出部３７４は、当該最も狭い範囲の両端のうちの一端を基準位置として分割領域を各座標軸の方向に隙間なく並べる。また、キューブ検出部３７４は、点群ＰＰの全体を覆い尽くすように分割領域を並べる。なお、当該領域を分割する方法は、これに限られず、他の方法であってもよい。また、分割領域の形状は、立方体形状に代えて、他の形状であってもよい。ただし、分割領域の形状は、ステップＳ２１０の処理が簡単になるため、立方体形状であることが望ましい。
【００７５】
キューブ検出部３７４は、ロボット座標系ＲＣにおける点群ＰＰを含む領域を分割領域に分割した後、各分割領域内に含まれる点であって点群ＰＰを構成する点の数を算出する。そして、キューブ検出部３７４は、各分割領域のうちの当該点が前述の所定数（この一例において、３）以上含まれる分割領域をシーンキューブとして検出する。当該所定数は、ＣＰＵ３１の処理速度に応じてユーザーにより決定される数である。当該処理速度が速いほど、当該所定数を小さくしてもステップＳ１９０〜ステップＳ２４０の処理に要する時間が増大しないため、ユーザーは、当該所定数を小さな数に決定することができる。
【００７６】
ステップＳ１９０においてキューブ検出部３７４が１以上のシーンキューブを検出した後、キューブ特徴量算出部３７６は、１以上のシーンキューブに基づいて、１以上のシーンキューブ毎にシーンキューブ特徴量を算出する（ステップＳ２１０）。ここで、図９〜図１１を参照し、ステップＳ２１０の処理について説明する。
【００７７】
キューブ特徴量算出部３７６は、ステップＳ１９０においてキューブ検出部３７４が検出した１以上のシーンキューブからシーンキューブ特徴量を算出する対象となるシーンキューブを対象シーンキューブとして１つずつ選択する。キューブ特徴量算出部３７６は、キューブ検出部３７４が検出した１以上のシーンキューブにおける相対的な位置関係であって、選択した対象シーンキューブと他のシーンキューブとの相対的な位置関係に基づいてシーンキューブ特徴量を算出する。
【００７８】
具体的には、キューブ特徴量算出部３７６は、対象シーンキューブから他のシーンキューブのそれぞれまでの位置関係を表すテンソルをシーンキューブ特徴量として算出する。以下では、一例として、各シーンキューブが点群ＰＰを含む領域において平面的に分布している場合について説明する。各シーンキューブが当該平面的に分布している場合、各シーンキューブの図心は、ある１つの平面に含まれる。
【００７９】
図９は、点群ＰＰを含む領域において平面的に分布しているシーンキューブを例示する図である。図９に示した例では、６個のシーンキューブ、すなわちシーンキューブＣ１〜シーンキューブＣ６のそれぞれが当該領域において平面的に分布している。なお、図９に示した例では、４個の分割領域ＵがシーンキューブＣ１〜シーンキューブＣ６とともに描かれている。分割領域Ｕは、ステップＳ１９０において所定数未満の点を含んでいること等の何らかの理由によってシーンキューブとして検出されなかった分割領域である。以下では、一例として、シーンキューブＣ２が対象シーンキューブである場合について説明する。また、以下では、一例として、ロボット制御装置３０が行う処理において分割領域Ｕを用いない場合について説明するが、ロボット制御装置３０は、分割領域Ｕを何らかの処理に用いる構成であってもよい。
【００８０】
キューブ特徴量算出部３７６は、対象シーンキューブに含まれる所定数以上の点によって構成される面の法線方向を対象シーンキューブの方向として算出する。図９に示した例では、当該法線方向は、矢印Ａ１が示す方向である。キューブ特徴量算出部３７６は、キューブ検出部３７４が検出した１以上のシーンキューブのそれぞれに含まれる各点の位置を座標変換し、算出した当該法線方向とロボット座標系ＲＣにおけるＺ軸の正方向とを図１０に示したように一致させる。図１０は、対象シーンキューブの方向とロボット座標系ＲＣにおけるＺ軸の正方向とを一致させた場合における図９に示したシーンキューブを例示する図である。
【００８１】
キューブ特徴量算出部３７６は、座標変換した各点に基づいて、キューブ検出部３７４に当該各点を含む領域を前述の分割領域に分割させ、１以上のシーンキューブを新たに検出させる。すなわち、ここでのシーンキューブを検出する対象は、当該座標変換した各点である。シーンキューブの検出方法は、前述したシーンキューブの検出方法と同様の検出方法であるため説明を省略する。キューブ特徴量算出部３７６は、例えば、新たに検出されたシーンキューブのうち選択した対象シーンキューブに含まれていた所定数以上の点を最も多く含むシーンキューブを新たな対象シーンキューブとして特定する。図１１は、座標変換した各点に基づいて新たに検出されたシーンキューブを例示する図である。図１１に示した例では、キューブ検出部３７４は、座標変換した各点に基づいて７個のシーンキューブであるシーンキューブＣ１１〜シーンキューブＣ１７のそれぞれを検出している。以下では、一例として、新たな対象シーンキューブとして特定されたシーンキューブがシーンキューブＣ１２である場合について説明する。なお、図１１に示した分割領域Ｕは、図９及び図１０に示した分割領域Ｕと同様に、座標変換した各点に基づいて新たにシーンキューブを検出した際、所定数未満の点を含んでいること等の何らかの理由でシーンキューブとして検出されなかった分割領域である。
【００８２】
キューブ特徴量算出部３７６は、シーンキューブ特徴量を表すテンソル（この一例において、行列）のＰ行Ｑ列目の成分として、ロボット座標系ＲＣにおけるＸ軸方向に沿って分割領域Ｐ個分だけ対象シーンキューブから離れたシーンキューブであって、ロボット座標系ＲＣにおけるＺ軸方向に沿って分割領域（Ｑ−１）個分だけ対象シーンキューブから離れたシーンキューブの数を算出する。ここで、Ｐは１以上の整数である。なお、Ｐの最大値は、ロボット座標系ＲＣにおけるＸ軸方向に沿って対象シーンキューブから最も離れたシーンキューブまでの分割領域の数（対象シーンキューブを０個として数えた場合の数）である。また、Ｑは１以上の整数である。なお、Ｑの最大値は、ロボット座標系ＲＣにおけるＺ軸方向に沿って対象シーンキューブから最も離れたシーンキューブまでの分割領域の数（対象シーンキューブを０個として数えた場合の数）である。
【００８３】
例えば、シーンキューブ特徴量を表すテンソルの１行１列目の成分は、ロボット座標系ＲＣにおけるＸ軸方向に沿って分割領域１個分だけ対象シーンキューブから離れたシーンキューブであって、ロボット座標系ＲＣにおけるＺ軸方向に沿って分割領域０個分だけ対象シーンキューブから離れたシーンキューブの数である。すなわち、図１１に示した例では、当該シーンキューブがシーンキューブＣ１１とシーンキューブＣ１３の２個であるため、当該成分は、２である。また、シーンキューブ特徴量を表すテンソルの２行１列目の成分は、ロボット座標系ＲＣにおけるＸ軸方向に沿って分割領域２個分だけ対象シーンキューブから離れたシーンキューブであって、ロボット座標系ＲＣにおけるＺ軸方向に沿って分割領域０個分だけ対象シーンキューブから離れたシーンキューブの数である。すなわち、図１１に示した例では、当該シーンキューブがシーンキューブＣ１４の１個のみであるため、当該成分は、１である。また、シーンキューブ特徴量を表すテンソルの６行４列目の成分は、ロボット座標系ＲＣにおけるＸ軸方向に沿って分割領域６個分だけ対象シーンキューブから離れたシーンキューブであって、ロボット座標系ＲＣにおけるＺ軸方向に沿って分割領域３個分だけ対象シーンキューブから離れたシーンキューブの数である。すなわち、図１１に示した例では、当該シーンキューブがシーンキューブＣ１５の１個のみであるため、当該成分は、１である。また、シーンキューブ特徴量を表すテンソルの５行４列目の成分は、ロボット座標系ＲＣにおけるＸ軸方向に沿って分割領域５個分だけ対象シーンキューブから離れたシーンキューブであって、ロボット座標系ＲＣにおけるＺ軸方向に沿って分割領域３個分だけ対象シーンキューブから離れたシーンキューブの数である。すなわち、図１１に示した例では、当該シーンキューブが０個であるため、当該成分は、０である。
【００８４】
このようにしてキューブ特徴量算出部３７６は、シーンキューブ特徴量を表すテンソルの各成分を算出することにより、ステップＳ１９０においてキューブ検出部３７４が検出した１以上のシーンキューブ毎のシーンキューブ特徴量を算出する。以下に示す式（２）は、図１１に示した例においてキューブ特徴量算出部３７６が算出した当該テンソルＴｃである。
【００８５】
【数２】
2017202549.tif 000004
【００８６】
なお、各シーンキューブが点群ＰＰを含む領域において立体的に分布している場合、シーンキューブ特徴量は、対象シーンキューブと当該領域内に位置する他のシーンキューブのそれぞれまでの位置関係を表すテンソルによって表される。この場合、当該テンソルは、３階のテンソルである。各シーンキューブが当該立体的に分布している場合、各シーンキューブの図心は、ある１つの平面に含めることができない。
【００８７】
ステップＳ２１０においてキューブ特徴量算出部３７６がシーンキューブ毎のシーンキューブ特徴量を算出した後、第１位置姿勢算出部３８０は、第２ランダムフォレストと、当該シーンキューブ特徴量とに基づいて、ステップＳ１９０において検出されたシーンキューブの中からモデルキューブに対応するシーンキューブを対応キューブとして特定する（ステップＳ２２０）。第２ランダムフォレストは、記憶部３２に予め記憶されている。ここで、ステップＳ２２０の処理について説明する。
【００８８】
まず、第２ランダムフォレストについて説明する。
第２ランダムフォレストは、モデルキューブの特徴量であるモデルキューブ特徴量に基づいて、あるシーンキューブ特徴量のシーンキューブに対応するモデルキューブとして尤もらしいモデルキューブ情報を出力する関数である。第２ランダムフォレストには、複数のモデルキューブのそれぞれを示すモデルキューブ情報のそれぞれに対してモデルキューブ特徴量が対応付けられた情報である第２対応情報がユーザーにより予め記憶される（学習させられる）。
【００８９】
なお、モデルキューブは、キューブ検出部３７４により予め検出される。キューブ検出部３７４は、モデル点群に基づいて１以上のモデルキューブを検出する。キューブ検出部３７４がモデル点群に基づいて１以上のモデルキューブを検出する方法は、キューブ検出部３７４がシーン点群に基づいて１以上のシーンキューブを検出する方法と同様の方法であるため説明を省略する。また、モデルキューブ特徴量は、キューブ特徴量算出部３７６により予め算出される。キューブ特徴量算出部３７６は、キューブ検出部３７４が検出したモデルキューブに基づいてモデルキューブ毎のモデルキューブ特徴量を算出する。キューブ特徴量算出部３７６がキューブ検出部３７４が検出したモデルキューブに基づいてモデルキューブ毎のモデルキューブ特徴量を算出する方法は、キューブ検出部３７４が検出したシーンキューブに基づいてシーンキューブ毎のシーンキューブ特徴量を算出する方法と同様の方法であるため説明を省略する。
【００９０】
第２ランダムフォレストは、あるシーンキューブのシーンキューブ特徴量が第１位置姿勢算出部３８０により入力されると、第２対応情報に基づいて、当該シーンキューブに対応することが最も尤もらしいモデルキューブを示すモデルキューブ情報と、当該モデルキューブが当該シーンキューブに対応することの尤もらしさを示す尤度とを第１位置姿勢算出部３８０に出力する。
【００９１】
このような第２ランダムフォレストによって、第１位置姿勢算出部３８０は、キューブ検出部３７４が検出したシーンキューブからモデルキューブに対応するシーンキューブを対応キューブとして特定する。具体的には、第１位置姿勢算出部３８０は、あるシーンキューブ特徴量を第２ランダムフォレストに入力し、第２ランダムフォレストから出力される前述の尤度とモデルキューブ情報とを第２ランダムフォレストから取得する。第１位置姿勢算出部３８０は、当該尤度が所定閾値未満である場合、当該シーンキューブに対応するモデルキューブがないと判定する。一方、第１位置姿勢算出部３８０は、第２ランダムフォレストから出力された尤度が所定閾値以上である場合、第２ランダムフォレストに入力したシーンキューブ特徴量のシーンキューブを対応キューブとして特定する。また、第１位置姿勢算出部３８０は、特定した対応キューブに、第２ランダムフォレストから当該尤度とともに出力されたモデルキューブ情報を対応付ける。
【００９２】
ステップＳ２２０において１以上の対応キューブを特定した後、第１位置姿勢算出部３８０は、各対応キューブと、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブとに基づいて、第１位置姿勢算出処理を行う（ステップＳ２３０）。ここで、ステップＳ２３０の処理について説明する。
【００９３】
第１位置姿勢算出部３８０は、各対応キューブの位置と、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブの位置とが一致するようにシーン点群に対するモデル点群の位置合わせを行う。そして、第１位置姿勢算出部３８０は、例えば、各対応キューブの位置と、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブの位置とが一致している度合いが最も高い状態におけるモデル点群の重心のロボット座標系ＲＣにおける位置及び姿勢を対象物Ｖの位置及び姿勢として算出する。この場合、当該度合いは、各対応キューブの位置と、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブの位置との差分によって表してもよく、当該差分に基づく統計量等の他の量によって表してもよい。当該度合いが当該差分である場合、当該度合いが最も高い状態は、当該差分が最も小さい状態のことである。なお、第１位置姿勢算出部３８０は、各対応キューブの位置と、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブの位置とに基づいて、すでに知られている他の位置合わせの方法によって対象物Ｖの位置及び姿勢を算出する構成であってもよい。
【００９４】
ステップＳ２３０において第１位置姿勢算出処理が行われた後、第２位置姿勢算出部３８２は、第２位置姿勢算出処理を行う（ステップＳ２４０）。ここで、ステップＳ２４０の処理について説明する。第２位置姿勢算出部３８２は、各対応キューブの位置と、各対応キューブに対応付けられたモデルキューブ情報が示すモデルキューブの位置とが一致している度合いが最も高い状態におけるシーン点群及びモデル点群に基づいて、シーン点群の位置に対するモデル点群の位置をＩＣＰアルゴリズムによって移動させ、当該状態におけるシーン点群とモデル点群との位置合わせを行う。そして、第２位置姿勢算出部３８２は、当該位置合わせ後のモデル点群の重心のロボット座標系ＲＣにおける位置及び姿勢を、対象物Ｖの位置及び姿勢として算出する。
【００９５】
ステップＳ２４０において第２位置姿勢算出処理が行われた後、ロボット制御部３８４は、ステップＳ２４０において算出された対象物Ｖの位置及び姿勢に基づいて、対象物Ｖをロボット２０に把持させる。そして、ロボット制御部３８４は、ロボット２０を動作させ、ロボット２０に把持された対象物Ｖを図示しない給材領域に配置させ（ステップＳ２５０）、処理を終了する。
【００９６】
なお、図４のフローチャートについての説明では、ロボット制御装置３０が対象物Ｖの位置及び姿勢を算出する処理について説明したが、対象物Ｖが対象物Ｏであっても、当該処理の内容は変化しない。すなわち、当該フローチャートの処理によって、ロボット制御装置３０は、対象物Ｏの位置及び姿勢を算出することができる。そして、ロボット制御装置３０は、ロボット２０を動作させ、対象物Ｏについての所定の作業をロボット２０に行わせることができる。
【００９７】
また、点群生成部３６６は、画像取得部３６４が撮像部１０から取得した撮像画像の一部に基づいてシーン点群を生成する構成であってもよい。この場合、点群生成部３６６は、ユーザーから受け付けた操作、又は所定の特定条件に基づいて、当該一部を当該撮像画像上から特定する。
【００９８】
また、点群生成部３６６は、生成したシーン点群を構成する点の数が所定の上限値よりも多い場合、当該点の一部を間引く（又はサンプリング）する構成であってもよい。例えば、点群生成部３６６は、ロボット座標系ＲＣの各座標軸方向において、当該点を１つ飛ばしで間引く。
【００９９】
また、図４に示したフローチャートでは、平面の組み合わせによって形成された形状を有する対象物の一例である対象物Ｖについて説明したが、これに代えて、図１に示した対象物Ｏやブッシュのような曲面と平面との組み合わせによって形成された形状を有する対象物に対して当該フローチャートの処理を適用する構成であってもよい。
【０１００】
以上のように、ロボット制御装置３０は、シーンキューブ毎のシーンキューブ特徴量を算出し、算出したシーンキューブ特徴量と、モデルキューブ毎のモデルキューブ特徴量とに基づいて、モデルキューブに対応するシーンキューブを特定し、特定した当該シーンキューブに基づいて対象物（この一例において、対象物Ｏ又は対象物Ｖ）の位置及び姿勢を算出する。これにより、ロボット制御装置３０は、対象物の形状が複雑な形状であっても、精度よく対象物の位置及び姿勢を算出することができる。
【０１０１】
また、ロボット制御装置３０は、複数のシーンキューブそれぞれの相対的な位置関係に基づいてシーンキューブ特徴量を算出し、複数のモデルキューブの相対的な位置関係に基づいてモデルキューブ特徴量を算出する。これにより、ロボット制御装置３０は、複数のシーンキューブそれぞれの相対的な位置関係に基づいて算出したシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【０１０２】
また、ロボット制御装置３０は、算出した対象物の位置及び姿勢に基づいてロボットを制御する。これにより、ロボット制御装置３０は、算出した対象物の位置及び姿勢に基づいてロボット２０に精度の高い作業を行わせることができる。
【０１０３】
また、ロボット制御装置３０は、シーン点群の中から曲率により面を構成する点群をシーンセグメントとして検出し、検出した１以上のシーンセグメントに基づいてシーン点群の中からモデル点群に対応する一部分を部分点群（この一例において、対象点群）として検出する。これにより、ロボット制御装置３０は、検出した部分点群に基づくシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【０１０４】
また、ロボット制御装置３０は、検出した１以上のシーンセグメントに基づく特徴量であるシーンセグメント特徴量を算出し、モデル点群の中から曲率によって検出された面を構成する点群であるモデルセグメントに対応するシーンセグメントとして尤もらしいシーンセグメントである対象シーンセグメント（この一例において、対象点群）を、算出したシーンセグメント特徴量に基づいて１以上のシーンセグメントの中から特定し、特定した対象シーンセグメントに基づいてシーン点群の中からモデル点群に対応する一部分を部分点群として検出する。これにより、ロボット制御装置３０は、対象シーンセグメントに基づくシーンキューブ特徴量に基づいて、精度よく対象物の位置及び姿勢を算出することができる。
【０１０５】
以上、この発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない限り、変更、置換、削除等されてもよい。
【０１０６】
また、以上に説明した装置（例えば、ロボット制御装置３０）における任意の構成部の機能を実現するためのプログラムを、コンピューター読み取り可能な記録媒体に記録し、そのプログラムをコンピューターシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳ（Operating  System）や周辺機器等のハードウェアを含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ（Compact  Disk）−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピューターシステム内部の揮発性メモリー（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。
【０１０７】
また、上記のプログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【符号の説明】
【０１０８】
１…ロボットシステム、１０…撮像部、２０…ロボット、３０…ロボット制御装置、３１…ＣＰＵ、３２…記憶部、３３…入力受付部、３４…通信部、３５…表示部、３６…制御部、３６１…撮像制御部、３６４…画像取得部、３６６…点群生成部、３７２…対象点群検出部、３７４…キューブ検出部、３７６…キューブ特徴量算出部、３８０…第１位置姿勢算出部、３８２…第２位置姿勢算出部、３８４…ロボット制御部
【図１】
2017202549.tif 000005
【図２】
2017202549.tif 000006
【図３】
2017202549.tif 000007
【図４】
2017202549.tif 000008
【図５】
2017202549.tif 000009
【図６】
2017202549.tif 000010
【図７】
2017202549.tif 000011
【図８】
2017202549.tif 000012
【図９】
2017202549.tif 000013
【図１０】
2017202549.tif 000014
【図１１】
2017202549.tif 000015