JP7034971B2 - 作動システム、制御装置、およびプログラム - Google Patents

Description

実施形態は、作動システム、制御装置、およびプログラムに関する。

従来、ビジュアルサーボによるフィードバック制御を実行する作動システムが知られている。

特開２０１７－９４４８２号公報

E. Malis, F. Chaumette and S. Boudet, "2D 1/2 visual servoing stability analysis with respect to camera calibration errors," Proceedings of the 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems. Innovations in Theory, Practice and Applications, 1998, vol. 2, pp. 691 - 697.

例えば、より不都合の少ない新規な構成の作動システムが得られれば、有益である。

実施形態の作動システムは、目標面検出部と、位置算出部と、方向算出部と、移動制御部と、を備えている。目標面検出部は、可動部材に設けられた深度センサによって得られた深度画像から目標物の目標面を検出する。記憶部は、第一時刻において検出された目標面内の少なくとも一点について前記深度画像における座標を記憶する。位置算出部は、検出された目標面について所定の第一位置を算出する。方向算出部は、検出された目標面について所定の第一方向を算出する。移動制御部は、可動部材に対して固定された第二位置と第一位置との位置偏差が小さくなるとともに、可動部材に対して固定された第二方向と第一方向との方向偏差が小さくなるよう、アクチュエータを制御する。前記目標面検出部は、前記第一時刻よりも後の第二時刻において、前記記憶された座標に基づいて前記深度画像の全域よりも狭い特定範囲を設定し、前記特定範囲内において前記目標面の第一領域を検出し、前記特定範囲外において前記目標面として前記第一領域と繋がる第二領域を検出する。

図１は、実施形態の作動システムの例示的な構成図である。 図２は、実施形態の作動システムの例示的なブロック図である。 図３は、実施形態の作動システムにおいて実行される処理の手順を示す例示的なフローチャートである。 図４は、実施形態の作動システムにより深度画像において複数の処理対象物の中から決定された目標物を示す模式的かつ例示的な図である。 図５は、実施形態の作動システムにおける現在位置、現在方向、目標位置、および目標方向の一例を示す模式的かつ例示的な斜視図である。 図６は、実施形態の作動システムにおける目標位置、仮目標位置、目標方向、仮目標方向、距離差分データ、姿勢差分データ、現在位置、および現在方向を示す模式的かつ例示的な斜視図である。 図７は、実施形態の作動システムの移動制御部の例示的なブロック図である。 図８は、第１実施形態の作動システムの移動制御部において実行される演算処理および制御の手順を示す例示的なフローチャートである。 図９は、実施形態の作動システムにより深度画像に対して設定された特定範囲を示す模式的かつ例示的な図である。 図１０は、実施形態の作動システムによって処理される深度画像の各画素について法線ベクトルを計算する近接領域を示す模式的かつ例示的な図である。 図１１は、実施形態の作動システムによって処理される深度画像の特定範囲内において検出された目標面の第一領域を示す模式的かつ例示的な図である。 図１２は、実施形態の作動システムによって処理される深度画像の特定範囲内において検出された目標面の第一領域、および特定範囲外において検出された目標面の第二領域を示す模式的かつ例示的な図である。 図１３は、第２実施形態の作動システムのハンドの例示的な構成図である。 図１４は、第２実施形態の作動システムの移動制御部において実行される処理の手順を示す例示的なフローチャートである。 図１５は、第２実施形態の作動システムの移動制御部において実行される目標面の法線ベクトル回りの相対的な回転処理を示す模式的かつ例示的な図である。 図１６は、第３実施形態の作動システムの学習済みのディープニューラルネットワークで構成された目標決定部の例示的なブロック図である。 図１７は、第３実施形態の作動システムにおいて入力画像に対するＤＮＮ型把持計画部による出力結果のマッピングを示す例示的な説明図である。 図１８は、第３実施形態の作動システムの目標物決定部による初期把持姿勢および目標位置の決定の手順を示す例示的なフローチャートである。 図１９は、第３実施形態の作動システムの学習済みのディープニューラルネットワークを作成する学習システムの例示的なブロック図である。 図２０Ａは、第３実施形態の作動システムの物理シミュレータ部が計算空間上に生成したバラ積み状態のバーチャルデータモデルを示す模式的かつ例示的な斜視図である。 図２０Ｂは、第３実施形態の作動システムの物理シミュレータ部が生成したバラ積み状態のバーチャルデータモデルから生成した深度画像の模式的かつ例示的な図である。 図２０Ｃは、第３実施形態の作動システムで得られる把持姿勢候補情報を示す模式的かつ例示的な図である。 図２１は、第４実施形態のピッキングシステムの例示的な構成図である。 図２２は、第４実施形態のピッキングシステムによるピッキングの手順を示す例示的なフローチャートである。

以下、作動システムの例示的な実施形態が開示される。以下に示される実施形態の構成や制御（技術的特徴）、ならびに当該構成や制御によってもたらされる作用および結果（効果）は、一例である。

また、以下の複数の実施形態には、同様の構成要素が含まれている。以下では、それら同様の構成要素には共通の符号が付与されるとともに、重複する説明が省略される場合がある。また、本明細書において、序数は、構成要素等を区別するために便宜上付与されており、優先順位や順番を示すものではない。

（第１実施形態）
［概要］
図１は、作動システム１の構成図である。図１に示されるように、作動システム１は、作動装置１００を含んでいる。作動装置１００は、多関節ロボットであって、関節１１２を介して互いに可動に連結された複数のアーム１１１を有している。複数のアーム１１１は、直列に連結されている。複数のアーム１１１の先端には、関節１１２を介してハンド１２０が設けられている。アーム１１１やハンド１２０は、可動部材の一例である。ハンド１２０は、マニピュレータや、アクセス部、処理部等と称されうる。アーム１１１やハンド１２０の位置および姿勢は、アクチュエータ１１３，１１４（図２参照）の作動によって、変化したり維持されたりしうる。アクチュエータは、例えばモータや、モータ付きポンプ、ソレノイド、ソレノイド付き電磁弁、ピストンシリンダ等であるが、これらには限定されない。また、ハンド１２０は、例えば、バキュームチャックや、グリッパ等であるが、これらには限定されない。

フィールド１１（エリア、ステージ）には、作動装置１００による処理の対象である処理対象物４００が置かれている。なお、処理対象物４００は、パレットや、コンテナのような容器内に収容されてもよいし、ベルトコンベヤや、ローラコンベヤのような搬送装置上に載置されてもよいし、テーブルや、台のような固定ベース上に載置されてもよい。また、本実施形態では、作動装置１００による処理は、一例として、処理対象物４００をハンド１２０で把持し、所定の位置へ運搬することである。ただし、作動装置１００による処理は、把持や運搬には限定されない。

作動システム１は、作動装置１００による処理をより確実にあるいはより円滑に実行できるよう、深度カメラ２００によって撮影した深度画像に基づくサーボ制御を実行する。本実施形態では、作動システム１は、ハンド１２０が処理対象物４００の処理の実行位置または初期位置（目標位置）で、処理対象物４００の処理の実行姿勢または初期姿勢（目標姿勢）となるよう、アクチュエータ１１３（図２参照）を制御する。また、作動システム１は、ハンド１２０が目標位置に到達しかつ目標姿勢となった時点で、所定の処理を実行するよう、アクチュエータ１１３，１１４を制御する。作動システム１は、ロボットハンドリングシステムや、マニピュレータシステム、対象物追従システム、サーボ作動システムとも称されうる。

深度カメラ２００は、深度画像を取得する。深度画像データは、２次元の各画素における距離データ（深度データ）である。深度カメラ２００は、深度センサの一例である。なお、深度カメラ２００は、ＲＧＢの画像を取得してもよい。この場合、深度カメラ２００は、ＲＧＢ－Ｄセンサであり、深度カメラ２００から得られるデータには、深度画像データと、２次元のＲＧＢ画像データとが含まれる。

図２は、作動システム１のブロック図である。作動システム１は、制御装置３００を含む。制御装置３００は、制御部３１０、主記憶部３２０、補助記憶部３３０等を有している。制御部３１０は、例えば、central processing unit（ＣＰＵ）やコントローラ等であり、主記憶部３２０は、例えば、read only memory（ＲＯＭ）や、random access memory（ＲＡＭ）等である。

制御部３１０による演算処理や制御は、ソフトウエアによって実行されてもよいし、ハードウエアによって実行されてもよい。また、制御部３１０による演算処理や制御には、ソフトウエアによる演算処理や制御とハードウエアによる演算処理や制御とが含まれてもよい。ソフトウエアによる演算処理や制御の場合にあっては、制御部３１０は、ＲＯＭや、ＨＤＤ、ＳＳＤ、フラッシュメモリ等に記憶されたプログラム（アプリケーション）を読み出して実行する。制御部３１０は、プログラムにしたがって動作することにより、制御部３１０に含まれる各部、すなわち、画像処理部３１１や、目標物決定部３１２、移動制御部３１３、処理制御部３１４等として、機能する。この場合、プログラムには、これら各部に対応するモジュールが含まれる。

プログラムは、それぞれインストール可能な形式または実行可能な形式のファイルでＣＤ－ＲＯＭや、ＦＤ、ＣＤ－Ｒ、ＤＶＤ、ＵＳＢメモリ等の、コンピュータで読み取り可能な記録媒体に記録されて提供されうる。また、プログラムは、通信ネットワークに接続されたコンピュータの記憶部に記憶され、ネットワーク経由でダウンロードされることによって導入されうる。また、プログラムは、ＲＯＭ等に予め組み込まれてもよい。

また、制御部３１０の全部あるいは一部がハードウエアによって構成される場合、制御部３１０には、例えば、field programmable gate array（ＦＰＧＡ）や、application specific integrated circuit（ＡＳＩＣ）等が含まれうる。

画像処理部３１１は、深度カメラ２００から深度画像データを取得する。深度カメラ２００がＲＧＢ－Ｄセンサである場合、画像処理部３１１は、深度画像データとともに２次元ＲＧＢデータを取得することができる。また、作動システム１が深度カメラ２００とは別のカメラ（不図示）を備えている場合、画像処理部３１１は、当該カメラによる深度画像データや２次元ＲＧＢ画像データを、取得することができる。

また、画像処理部３１１は、深度カメラ２００のカメラ行列を用いた座標変換により、各位置での距離データから、３次元点群データを算出する。３次元点群データは、深度カメラ２００の画素毎の３次元位置を示すデータである。
深度画像の座標をｕ_ｉ，ｖ_ｉ、深度を

、カメラ行列をＭ_ｃとすると、３次元点群データは、次の式（１）で表せる。

なお、以下では、３次元点群データも、深度画像データに含まれることとする。すなわち、深度画像データは、深度カメラ２００の各画素（深度カメラ２００の２次元座標系における位置座標）について、距離データと、３次元点群データ（３次元位置データ）とを含む。

目標物決定部３１２は、深度画像の初期画像に基づいて、少なくとも一つの処理対象物４００中から、存在位置や大きさ等についての所定の条件を満たす目標物４１０（の画像、図４参照）を決定する。ここで、深度画像の初期画像は、ハンド１２０が移動制御の開始位置に位置されている状態で、深度カメラ２００によって得られた深度画像である。

移動制御部３１３は、ハンド１２０が、目標物４１０および目標面４１１に対して所定の位置および姿勢、すなわち目標位置および目標姿勢となるよう、アクチュエータ１１３を制御する。移動制御部３１３は、深度画像によるサーボ制御を実行する。

処理制御部３１４は、ハンド１２０が目標位置に到達しかつ目標姿勢となった状態において、作動装置１００（ハンド１２０）が所定の処理、例えば目標物４１０の把持動作を実行するよう、アクチュエータ１１３，１１４を制御する。

制御部３１０における位置や、方向、姿勢に関する各種の演算は、例えば、深度カメラ２００に固定された３次元座標系での値によって実行されるが、これには限定されず、座標変換処理が施された後、フィールド１１に固定された３次元座標系のような他の３次元座標系での値によって実行されてもよい。

図３は、制御装置３００による処理の手順を示すフローチャートである。図３に示されるように、まずは、目標物決定部３１２が目標物４１０および初期目標位置を決定し（Ｓ１０）、次に、移動制御部３１３が移動制御を実行する（Ｓ１１）。Ｓ１１では、ハンド１２０の移動に伴い、各タイムステップ（例えば、フレームレート３０［ｆｐｓ］以内）において、現在位置と目標位置との位置偏差と位置閾値との比較、および現在方向と目標方向との方向偏差と方向閾値との比較が実行され、位置偏差が位置閾値以下となり、方向偏差が方向閾値以下となった時点で、移動制御が終了する。次に、処理制御部３１４が処理制御を実行する（Ｓ１２）。

なお、本実施形態では、ハンド１２０が目標物４１０および目標面４１１に対して処理（例えば把持）を開始する位置、言い換えると処理を実行する前の位置が、目標位置であり、処理を開始する際の目標位置におけるハンド１２０の姿勢、言い換えると処理を実行する前のハンド１２０の姿勢が、目標姿勢（目標方向）であり、制御部３１０は、移動制御を実行した後に処理制御を実行する。ただし、これには限定されず、ハンド１２０が目標物４１０および目標面４１１に対して処理を実行した際あるいは処理を実行した後の位置が目標位置であり、ハンド１２０が目標物４１０および目標面４１１に対して処理を実行した際あるいは処理を実行した後の姿勢が目標姿勢であってもよい。その場合、制御部３１０は、移動制御の後段で移動制御とともに処理制御を実行する。

［目標物決定部の詳細な説明］
図４は、複数の処理対象物４００の中から決定された目標物４１０を示す図である。図４に示されるように、目標物決定部３１２は、例えば、深度画像の初期画像に基づいて、複数の積み重ねられた処理対象物４００のうち、最も上に位置する処理対象物４００を、目標物４１０として決定する。目標物決定部３１２は、処理対象物４００が存在するフィールド１１に対する深度カメラ２００の相対的な位置に基づいて、複数の処理対象物４００のうち最も上に位置する処理対象物４００を検出することができる。なお、処理対象物４００が一つだけであった場合、目標物決定部３１２は、当該一つの処理対象物４００を、目標物４１０として決定する。

また、目標物決定部３１２は、例えば、決定された目標物４１０の表面のうち深度画像中で最も大きく写っている表面の重心（中心、図心）を、初期状態における暫定的な目標位置Ｐｔ（ここでは初期目標位置）として決定し、当該目標位置Ｐｔのデータ（例えば、深度画像における座標、深度値、および３次元位置）をデータ記憶部３３１に記憶する。データ記憶部３３１は、記憶部の一例である。

なお、目標物決定部３１２は、深度画像をガウシアンフィルタで畳み込み、深度カメラ２００に最も近い点を目標位置Ｐｔとして決定するとともに、当該点を含む処理対象物４００を目標物４１０として決定してもよい。また、目標物決定部３１２は、ニューラルネットワークにおいて、深度画像と、当該深度画像から決定された目標位置Ｐｔ、の多数の組み合わせに基づく学習処理を実行することにより、深度画像に対応する目標位置Ｐｔを決定してもよい。この場合、目標物決定部３１２は、学習処理によって得られた、深度画像から目標位置Ｐｔを決定する近似関数を用いて、深度画像から目標位置Ｐｔを決定することができる。また、目標物決定部３１２は、他の処理対象物４００から分離した目標物４１０の表面上で各点における法線方向が互いに閾値以内となる領域を検出し、当該領域における重心（中心、図心）を目標位置Ｐｔとして決定し、かつ当該法線方向の代表値を目標方向Ｄｔとして決定してもよい。

なお、目標物決定部３１２は、深度カメラ２００とは別のカメラ（不図示）による撮影画像や、別のセンサによる検出信号等から、目標物４１０を決定してもよい。この場合、別のカメラによる撮影画像や別のセンサによる検出信号の座標（位置）と、深度カメラ２００による深度画像の座標（位置）との相関関係は予め取得されている。

［移動制御部の詳細な説明］
図５は、目標位置Ｐｔ、目標方向Ｄｔ、現在位置Ｐｃ、および現在方向Ｄｃを示す斜視図である。目標位置Ｐｔおよび目標方向Ｄｔは、検出された目標面４１１から所定のアルゴリズムにしたがって一義的に定められる。目標位置Ｐｔは、第一位置の一例であり、目標方向Ｄｔは、第一方向の一例である。また、現在位置Ｐｃおよび現在方向Ｄｃは、深度カメラ２００の座標系において固定されており、深度カメラ２００が設けられたハンド１２０に対して固定されている。現在位置Ｐｃは、第二位置の一例であり、現在方向Ｄｃは、第二方向の一例である。

移動制御部３１３は、ハンド１２０が初期位置（不図示）から目標位置Ｐｔへ移動する途中の各タイムステップにおいて、深度画像から目標面４１１を検出し、当該検出した目標面４１１から目標位置Ｐｔおよび目標方向Ｄｔを算出して、目標位置Ｐｔと現在位置Ｐｃとの位置偏差および目標方向Ｄｔと現在方向Ｄｃとの方向偏差が小さくなるようアクチュエータ１１３に対する指令値を算出する。アクチュエータ１１３は、算出された指令値に従って動作し、これにより、現在位置Ｐｃが目標位置Ｐｔに近付くとともに現在方向Ｄｃが目標方向Ｄｔに近付くように、ハンド１２０が移動する。このような各タイムステップにおける処理が繰り返されることにより、ハンド１２０は目標位置Ｐｔに近付くとともに、ハンド１２０の姿勢は目標位置Ｐｔにおける目標姿勢に近付く。

図５の例では、目標位置Ｐｔは、各タイムステップにおいて検出された目標面４１１の重心（中心、図心）に設定されており、目標方向Ｄｔは、各タイムステップにおいて検出された目標面４１１の近似平面（不図示）の法線方向（ただし深度カメラ２００から離れる方向）、あるいは検出された目標面４１１上の各点における法線方向の例えば平均値のような代表値に設定されている。ただし、目標位置Ｐｔおよび目標方向Ｄｔは、これらには限定されず、検出された目標面４１１から所定のアルゴリズムによって算出され当該検出された目標面４１１に対して一義的に定まる他の位置や方向であってもよい。また、目標位置Ｐｔおよび目標方向Ｄｔは、検出された目標面４１１の重心のような目標面４１１を代表する位置、および目標面４１１の法線方向のような目標面４１１を代表する方向に対して、所定の相対的な位置関係を有する位置および方向であってもよい。また、目標位置Ｐｔおよび目標方向Ｄｔは、処理対象物４００の形状や、大きさのようなスペックや、ハンド１２０の構造や、大きさ、把持方式のようなスペック、ハンド１２０と深度カメラ２００との相対位置、ハンド１２０に対する深度カメラ２００による相対的な撮像方向等に応じて、適宜に設定されうるし、適宜に変更されうる。なお、代表値は平均値には限定されない。

また、現在位置Ｐｃは、ハンド１２０と目標物４１０とが所期の相対的な位置関係を有した状態において、目標位置Ｐｔと重なる位置に設定される。具体的に、現在位置Ｐｃは、ハンド１２０が目標物４１０を把持した状態で目標位置Ｐｔと重なる位置に、設定されている。図５の例では、ハンド１２０および深度カメラ２００は、ハンド１２０が目標物４１０を把持した状態において目標位置Ｐｔが深度画像の中心（座標）に位置されるよう、構成されている。よって、現在位置Ｐｃは、深度画像の中心（座標）に設定されている。ただし、現在位置Ｐｃの設定は、これには限定されず、例えば、処理制御部３１４が実行する処理制御（例えば、ハンド１２０による把持制御）の開始時点において目標位置Ｐｔと重なる位置であってもよい。

また、現在方向Ｄｃは、ハンド１２０と目標物４１０とが所期の相対的な位置関係を有した状態において、目標方向Ｄｔと重なる方向に設定される。具体的に、現在方向Ｄｃは、ハンド１２０が目標物４１０を把持し現在位置Ｐｃと目標位置Ｐｔとが一致した状態で、目標方向Ｄｔと重なる方向に、設定されている。図５の例では、ハンド１２０および深度カメラ２００は、ハンド１２０が目標物４１０を把持した状態において深度カメラ２００の光軸方向と目標面４１１の法線方向（ただし深度カメラ２００から離れる方向）とが平行となるように、構成されている。よって、現在方向Ｄｃは、深度カメラ２００の光軸方向と平行に設定されている。ただし、現在方向Ｄｃの設定は、これには限定されず、例えば、処理制御部３１４が実行する処理制御（例えば、ハンド１２０による把持制御）の開始時点において目標方向Ｄｔと重なる方向であってもよい。

図６は、目標位置Ｐｔ、仮目標位置Ｐｔ’、目標方向Ｄｔ、仮目標方向Ｄｔ’、距離差分データΔＰｔ、姿勢差分データΔＤｔ、現在位置Ｐｃ、および現在方向Ｄｃを示す斜視図である。移動制御部３１３は、以下のような演算によって、目標位置Ｐｔおよび目標方向Ｄｔを決定してもよい。すなわち、データ記憶部３３１は、距離差分データΔＰｔおよび姿勢差分データΔＤｔを記憶している。移動制御部３１３は、例えば前のタイムステップにおける目標位置Ｐｔおよび目標方向Ｄｔを、仮目標位置Ｐｔ’および仮目標方向Ｄｔ’とし、仮目標位置Ｐｔ’、距離差分データΔＰｔ、および姿勢差分データΔＤｔから目標位置Ｐｔを算出するとともに、仮目標方向Ｄｔ’および姿勢差分データΔＤｔから目標方向Ｄｔを算出することができる。

図７は、移動制御部３１３のブロック図である。図８は、移動制御部３１３による演算処理および制御の手順を示すフローチャートである。図７に示されるように、移動制御部３１３は、目標面検出部３１３ａと、位置方向算出部３１３ｂと、指令値算出部３１３ｃと、を有している。データ記憶部３３１は、例えば、補助記憶部３３０によって構成される。データ記憶部３３１は、各タイムステップで算出されたデータ（値）を記憶する。データ記憶部３３１に記憶されたデータは、後のタイムステップで使用される。

また、各タイムステップにおいて、図８のＳ１１０１からＳ１１１１が実行され、Ｓ１１１０においてＹｅｓとなった時点で図８のフローによる演算処理および制御は終了する。

移動制御部３１３は、まず、目標面検出部３１３ａとして機能し、画像処理部３１１から、現在のタイムステップでの深度画像（３次元点群データを含む）を取得するとともに、データ記憶部３３１に記憶されている前のタイムステップでの目標位置Ｐｔおよび目標方向Ｄｔを示すデータを取得する（Ｓ１１０１）。前のタイムステップは、第一時刻の一例であり、現在のタイムステップは、第二時刻の一例である。

次に、目標面検出部３１３ａは、深度画像に、目標面４１１の検出に用いる特定範囲Ａｄを設定する（Ｓ１１０２）。図９は、深度画像に対して設定された特定範囲Ａｄを示す図である。特定範囲Ａｄは、データ記憶部３３１に記憶されている初期または前のタイムステップでの目標位置Ｐｔ’の座標を中心座標Ｐｏとする深度画像の２次元座標における四角形状の領域であり、ｋ個の画素を含む領域である。特定範囲Ａｄは、深度画像の全域よりも狭い範囲であり、例えば、深度カメラ２００の２次元座標系における座標軸の方向をｕ方向およびｖ方向とした場合、ｕ方向に４１個、ｖ方向に４１個の合計１６８１（＝ｋ）個の画素を含む正方形状の領域である。なお、特定範囲Ａｄの形状は、四角形状でなくてもよい。特定範囲Ａｄの大きさや形状等のスペックは、深度画像における処理対象物４００の大きさや形状等に応じて適宜に設定されうる。特定範囲Ａｄの大きさは、深度カメラ２００と目標物４１０との距離が近いほど大きく設定されてもよい。また、特定範囲Ａｄの中心位置（基準位置）の座標は、前のタイムステップでの目標位置Ｐｔの座標と同じ、すなわち深度画像上で不動でなくてもよく、例えば、公知の画像追跡手法や画像ヤコビアンによる深度カメラ２００の移動を深度画像上での移動に変換する手法を用いることにより、深度画像上で移動してもよい。また、特定範囲Ａｄは、一定でなくてもよく、例えば、目標物４１０に応じて変更されたり、各タイムステップで変更されたりしてもよい。

次に、目標面検出部３１３ａは、前のタイムステップでの目標位置Ｐｔと同じ座標の参照位置、および当該参照位置に対して周辺座標（隣接座標）となる周辺位置における、現在のタイムステップでの３次元点群データから、現在のタイムステップにおける目標物４１０の表面の法線方向（ただし、深度カメラ２００から離れる方向）に沿う単位ベクトルを参照ベクトルｎ_ｏとして設定する（Ｓ１１０３）。目標面検出部３１３ａは、各画素の法線方向に沿った法線ベクトルを、次の式（２）により算出することができる。なお、以下では、添字ｕ_ｉ，ｖ_ｉは、演算の対象となる点の深度画像上の位置座標を示しており、ｉは、タイムステップである。

図１０は、各画素の法線ベクトルを算出する近接領域Ａｐを示す図である。図１０では、対象画素の近接領域Ａｐに含まれる隣接画素にはドットパターンが付与されている。式（２）に示されるような、ｕ方向の二つの隣接画素の三次元位置の差分値（ベクトルＶ１）と、ｖ方向の二つの隣接画素の三次元位置の差分値（ベクトルＶ２）と、の外積からは、当該二つのベクトルＶ１，Ｖ２と直交するベクトル、すなわち当該画素における目標面４１１の法線ベクトルが得られる。よって、目標面検出部３１３ａは、中心座標Ｐｏについて、式（２）を適用することにより、参照ベクトルｎ_ｏを算出することができる。ベクトルＶ１，Ｖ２は、近接領域Ａｐに沿う二つのベクトルの一例である。なお、位置偏差または方向偏差が所定の判定閾値以下であった場合には、参照ベクトルｎ_ｏを、データ記憶部３３１に記憶されている前のタイムステップでの目標方向Ｄｔに沿った単位ベクトルとしてもよい。この場合、式（２）による参照ベクトルｎ_ｏの演算を省略することができる。

次に、目標面検出部３１３ａは、特定範囲Ａｄ内で次の式（３）を満たす点を、目標面４１１の第一領域Ａ１として検出する（Ｓ１１０４）。

式（３）の左辺は各位置における法線ベクトルと参照ベクトルとの差の大きさを示し、式（３）の右辺は、目標面４１１として検出する際の条件となる閾値を示す。式（３）により、参照ベクトルｎｏに対する角度差が閾値よりも小さい法線ベクトルを有する点が、目標面４１１の第一領域Ａ１を構成する点として検出される。図１１は、図１０の例について検出された第一領域Ａ１を示す図である。特定範囲Ａｄは、先行検出範囲とも称されうる。

次に、目標面検出部３１３ａは、第一領域Ａ１に含まれるｍ個の点の３次元点群データから、目標面４１１に略沿う参照平面の方程式を算出する（Ｓ１１０５）。参照平面の方程式は、次の式（４）で表せる。

目標面検出部３１３ａは、以下の式（５）で表される関数が最小になる自明でない解ｎ_ｘ，ｎ_ｙ，ｎ_ｚ，ｄを算出する。ここに、［ｎ_ｘ，ｎ_ｙ，ｎ_ｚ］（参照平面の法線ベクトル）の大きさは１であり、ｎ_ｚ＞０とする。

このため、次の式（６）で表される行列Ａを特異値分解により分解し、得られた最も小さい固有値に関する固有ベクトルが、目標面４１１の第一領域Ａ１に対する参照平面の法線ベクトルと、ｄである。

得られた固有ベクトルで表される参照平面は、第一領域Ａ１の近似平面の一例である。なお、平面の算出精度を高めるため、式（５）で表される関数に替えて、次の式（７）で表される１ノルムの関数を用いてもよい。

この場合、反復再重み付き最小二乗法による近似解が得られる。

次に、目標面検出部３１３ａは、式（６）の特異値分解によって得られたベクトル［ｎ_ｘ，ｎ_ｙ，ｎ_ｚ］の単位ベクトルを、新たな参照ベクトルｎ_ｍとする、言い換えると、目標面検出部３１３ａは、参照ベクトルｎ_ｏを参照ベクトルｎ_ｍに更新する（Ｓ１１０６）。

次に、目標面検出部３１３ａは、特定範囲Ａｄ外において目標面４１１の第二領域Ａ２を検出する（Ｓ１１０７）。図１２は、図１１の例について検出された第二領域Ａ２を示す図である。具体的に、目標面検出部３１３ａは、領域拡張法により、第一領域Ａ１と繋がる第二領域Ａ２を検出することができる。この場合、目標面検出部３１３ａは、目標面４１１（開始時点では第一領域Ａ１）の端部の画素であるとともに特定範囲Ａｄの端部であるエッジａｅ１の画素と隣接した各画素のうち、追加条件を満たす画素、すなわち次の式（８）および式（９）のうち一方または両方を満たす画素を検出し、当該画素を目標面４１１に追加する。追加された画素は目標面４１１の新たなエッジａｅ１の画素となる。このようにして目標面４１１が拡張される。

ここに、式（８）は、各画素の法線ベクトルと参照ベクトルｎ_ｍとの差の大きさが閾値Ｓ_ｃ１以下であることを示し、式（９）は、各画素の３次元位置と参照平面との距離の大きさが閾値Ｓ_ｃ２以下であることを示している。

さらに、目標面検出部３１３ａは、拡張された目標面４１１のエッジ画素と隣接した画素についても、次の式（８）および式（９）のうち一方または両方を満たす画素を検出し、当該画素を目標面４１１に追加する。目標面４１１の全てのエッジ画素についてこのような拡張処理を実行し、追加条件を満たす画素が無くなった時点で、目標面４１１の検出を終了する。目標面４１１のうち第一領域Ａ１の外側に領域拡張法によって拡張された領域が、目標面４１１の第二領域Ａ２となる。なお、目標面検出部３１３ａは、第二領域Ａ２の検出開始において、特定範囲Ａｄの端部から離れた第一領域Ａ１のエッジａｅ２の画素からは、上述した領域の拡張は実行しない。これは、Ｓ１１０４における第一領域Ａ１の検出において、エッジａｅ２の外側に隣接した目標面４１１が存在しないことが明らかだからである。

また、目標面検出部３１３ａは、深度画像中の第一領域Ａ１外の全てあるいは特定の一部の領域内の画素について法線ベクトルと参照ベクトルとの差の大きさを算出し、各画素における当該差（当該差の画像）について、ラベリングや領域分割法のような画像処理を実行することにより、第一領域Ａ１と繋がる第二領域Ａ２（以下、第三領域とする）を検出してもよい。また、目標面検出部３１３ａは、深度画像中の第一領域Ａ１外の全てあるいは特定の一部の領域内の画素について３次元位置と参照平面との距離の大きさを算出し、各画素における当該差（当該差の画像）について、ラベリングや領域分割法のような画像処理を実行することにより、第一領域Ａ１と繋がる第二領域Ａ２（以下、第四領域とする）を検出してもよい。あるいは、目標面検出部３１３ａは、第三領域にも含まれるとともに第四領域にも含まれる画素を、第二領域Ａ２として検出してもよい。

次に、移動制御部３１３は、位置方向算出部３１３ｂとして機能し、検出された目標面４１１について、目標位置Ｐｔおよび目標方向Ｄｔを算出する（Ｓ１１０８）。Ｓ１１０８において、位置方向算出部３１３ｂは、例えば、上述したように、検出された目標面４１１、すなわち検出された第一領域Ａ１および第二領域Ａ２に含まれる全画素の重心（中心、図心）を、目標位置Ｐｔとして設定する。また、Ｓ１１０８において、位置方向算出部３１３ｂは、例えば、上述したように、検出された目標面４１１、すなわち検出された第一領域Ａ１および第二領域Ａ２に含まれる全画素について、特異値分解を用いて算出した近似平面の法線ベクトル（単位ベクトル）を、目標方向Ｄｔに沿う単位ベクトルとして算出してもよいし、検出された目標面４１１に含まれる各画素の法線ベクトル（単位ベクトル）の例えば平均値のような代表値を、目標方向Ｄｔに沿う単位ベクトルとして算出してもよい。位置方向算出部３１３ｂは、位置算出部の一例であるとともに、方向算出部の一例でもある。

次に、移動制御部３１３は、指令値算出部３１３ｃとして機能し、位置偏差および方向偏差が小さくなるようにアクチュエータ１１３を作動させる指令値を算出する（Ｓ１１０９）。本実施形態では、深度カメラ２００がハンド１２０に取り付けられているため、深度カメラ２００に固定されたカメラ座標系では、現在位置Ｐｃが固定されている。よって、ここでは、目標位置Ｐｔが現在位置Ｐｃに近付くよう制御が実行される場合、すなわち、移動制御部３１３が、目標位置Ｐｔから深度カメラ２００の光軸方向と反対方向に距離ｄｃだけ離れた位置Ｐ_ｅが、深度画像の中央に対応する位置Ｐ_ｅ ^＊に近付き、かつ目標方向Ｄｔが深度カメラ２００の光軸方向に近付くよう、アクチュエータ１１３を制御する場合について、考察する。なお、距離ｄｃは、深度カメラ２００の性能に応じて設定される。一例として、深度カメラ２００の測定範囲が１０ｃｍ～１００ｃｍであれば、１０ｃｍ以上に設定される。

位置Ｐｅは、以下の式（１０）のように表せる。

ここに、［ｐ_ｘ，ｐ_ｙ，ｐ_ｚ］は、位置Ｐ_ｅの深度カメラ２００に固定された３次元空間座標系における位置、［ｕ_ｐ，ｖ_ｐ］は、位置Ｐ_ｅの深度画像上における位置座標、ρ＝ｚ_ｐ／ｄ_ｃ、ｚ_ｐは、位置Ｐ_ｅの深度（距離）である。
また、光軸方向の単位ベクトルは次の式（１１）のように表すことができる。

Ｓ１１０９において、指令値算出部３１３ｃは、次に、目標方向Ｄｔの法線ベクトルと光軸方向の単位ベクトル［０，０，１］^Ｔとの外積ベクトルｕを回転軸として、目標方向Ｄｔの法線ベクトルと光軸方向の単位ベクトルとの間の角度θ（角度偏差）を、これらのベクトルの内積から算出する。この場合、外積は以下の式（１２）となり、内積は以下の式（１３）となる。

目標方向Ｄｔの法線ベクトルと光軸方向の単位ベクトル［０，０，１］^Ｔとの偏差は、以下の式（１４），（１５）のように表すことができる。

Ｓ１１０９において、指令値算出部３１３ｃは、次に、ハンド１２０の移動に対するアクチュエータ１１３への指令値ｖ（速度指令値）を、以下の式（１６）のように算出する。これについては、非特許文献１に記載されている。

ここに、

が成り立つ。
また、λは視覚サーボの収束速度を調整する正のスカラーゲインであり、Ｉ_３は（３×３）の単位行列である。Ｐ_ｅ ^＊は、深度画像の中心に対応する位置であるため、
Ｐ_ｅ ^＊＝Ｍ_ｃ ^－１［ｗ／２，ｈ／２，１］^Ｔ ・・・（１７）
と表せる。

次に、移動制御部３１３は、偏差を閾値と比較する（Ｓ１１１０）。式（１６）のベクトルｅは、位置偏差と方向偏差とを含んでいる。Ｓ１１１０において、移動制御部３１３は、位置偏差とそれに対応する位置閾値とを比較するとともに、方向偏差とそれに対応する方向閾値とを比較する。移動制御部３１３は、位置偏差が位置閾値以下であるとともに、方向偏差が方向閾値以下である場合には（Ｓ１１１０でＹｅｓ）、移動制御を終了する。他方、移動制御部３１３は、位置偏差が位置閾値よりも大きいかあるいは方向偏差が方向閾値よりも大きい場合には（Ｓ１１１０でＮｏ）、目標位置Ｐｔのデータ（例えば、深度画像における座標、深度値、および３次元位置）、および目標方向Ｄｔのデータ（例えば、法線ベクトルの値）を、データ記憶部３３１に記憶する（Ｓ１１１１）。上述したように、データ記憶部３３１に記憶されたデータは、次のタイムステップでの移動制御で用いられる。Ｓ１１１１の後、次のタイムステップでのＳ１１が実行される。言い換えると、次のタイムステップでの移動制御が開始される。

以上、説明したように、本実施形態では、移動制御部３１３（制御部３１０）は、深度画像から検出された目標面４１１について所定の目標位置Ｐｔ（第一位置）および目標方向Ｄｔ（第一方向）を算出し、移動制御部３１３は、ハンド１２０（可動部材）に対して固定された現在位置Ｐｃ（第二位置）と目標位置Ｐｔとの位置偏差が小さくなるとともに、ハンド１２０に対して固定された現在方向Ｄｃ（第二方向）と目標方向Ｄｔとの方向偏差が小さくなるよう、アクチュエータ１１３を制御する。このような構成によれば、通常のビジュアルサーボに必要な目標物の画像が不要となるため、例えば、未知の物体のように目標物の画像が存在しない場合や、無地の箱のように画像処理による特徴量の抽出が難しいような場合、識別可能な目標面の画像が得られ難いような場合等にあっても、制御装置３００は、深度画像から得られた目標面４１１の位置および方向に基づくサーボ制御を実行することにより、ハンド１２０を、より容易に、目標位置Ｐｔへ位置させるとともに目標方向Ｄｔに向けさせることができる。

また、本実施形態では、位置方向算出部３１３ｂ（方向算出部）は、検出された目標面４１１の近似平面の法線方向として、目標方向Ｄｔを算出する。このような構成によれば、目標面４１１が、例えば、平面や、平面に略沿う面、平面に略沿って微小な凹凸を有する面、曲率半径が比較的大きい曲面のような、平面から大きく外れない面であるような場合に、比較的簡単な演算によって目標方向Ｄｔを設定することができるというメリットが得られる。

また、本実施形態では、位置方向算出部３１３ｂは、検出された目標面４１１内の各画素について、当該画素と隣接する複数の画素を含む近接領域Ａｐの三次元位置から目標面４１１の法線方向を算出し、検出された目標面４１１内の複数の画素における法線方向の代表値として、目標方向Ｄｔを算出する。このような構成によれば、目標面４１１が、例えば、平面や、平面に略沿う面、平面に略沿って微小な凹凸を有する面、曲率半径が比較的大きい曲面のような、平面から大きく外れない面であるような場合に、比較的簡単な演算によって目標方向Ｄｔを設定することができるというメリットが得られる。

また、本実施形態では、位置方向算出部３１３ｂは、近接領域Ａｐに沿う二つのベクトルＶ１，Ｖ２の外積から目標面４１１の法線方向を算出する。このような構成によれば、例えば、比較的簡単な演算によって法線方向を算出することができる。

また、本実施形態では、目標面検出部３１３ａは、深度画像の全域よりも狭い特定範囲Ａｄ内において目標面４１１の第一領域Ａ１を検出するとともに、特定範囲Ａｄ外において第一領域Ａ１と繋がる第二領域Ａ２を検出する。このような構成によれば、例えば、深度画像の全域について目標面４１１の検出を実行する場合や、当初検出された１点あるいは狭い領域から拡張して目標面４１１の全域を出する場合に比べて、目標面４１１をより迅速に検出することができる。

また、本実施形態では、後のタイムステップ（第二時刻）において、前のタイムステップ（第一時刻）で検出された目標面４１１内の少なくとも一点の深度画像における座標に基づいて設定された特定範囲Ａｄ内において第一領域Ａ１を検出し、特定範囲Ａｄ外において第二領域Ａ２を検出する。このような構成によれば、前のタイムステップで検出された目標面４１１の座標に基づいて後のタイムステップでの特定範囲Ａｄが設定されるので、特定範囲Ａｄが設定されない場合に比べて、目標面４１１をより迅速に検出することができる。なお、前のタイムステップは、直前のタイムステップよりも前のタイムステップであってもよい。

［第２実施形態］
図１３は、本実施形態のハンド１２０Ａの側面図である。図１３に示されるように、本実施形態では、ハンド１２０Ａは、二つのサクションカップ１２０ａを有しており、一つまたは二つのサクションカップ１２０ａによって目標面４１１を吸着保持することにより、目標物４１０を把持する。

目標物４１０が比較的軽いような場合や、目標物４１０が比較的小さいような場合には、二つのサクションカップ１２０ａのうちの一つによって目標物４１０を把持することができる。目標物４１０の大きさまたは重さが既知あるいは検出可能であり、少なくとも一方がそれぞれの閾値以下である場合、現在位置Ｐｃを二つのサクションカップ１２０ａのうちいずれか一つのサクションカップ１２０ａによる目標面４１１の吸着位置に設定するとともに、上記式（１７）に替えて、例えば、以下の式（１８）を用いる。
Ｐ_ｅ ^＊＝Ｍ_ｃ ^－１［ｗ／２，ｈ／２，１］^Ｔ＋［０，ｗ_ｃ／ｄ_ｃ，０］^Ｔ
・・・（１８）
ここに、ｗ_ｃ／ｄ_ｃは、光軸方向のオフセット長であり、ｗ_ｃは、サクションカップ１２０ａと深度カメラ２００との距離である。

他方、二つのサクションカップ１２０ａによって目標物４１０を把持する場合には、例えば、位置方向算出部３１３ｂは、検出された目標面４１１の目標方向Ｄｔに沿う回転軸回りの、目標面４１１の代表姿勢（第二目標方向）とハンド１２０の所定姿勢（基準方向）との間の角度偏差を算出する。

図１４は、この場合における制御装置３００による処理の手順を示すフローチャートであり、図１５は、角度偏差を示す図である。図１５に例示されるように、本実施形態における基準方向は、例えば、二つのサクションカップ１２０ａの中心を通り現在方向Ｄｃと直交する線分（基準線分）が延びる方向Ｄｃｌに設定され、移動制御部３１３は、検出された目標面４１１の長手方向Ｄｔｌを第二目標方向とし、当該長手方向Ｄｔｌと方向Ｄｃｌとの角度偏差δが小さくなるよう、アクチュエータ１１３を制御する。第二目標方向は、検出された目標面４１１から所定のアルゴリズムにしたがって一義的に定められる。長手方向Ｄｔｌ（第二目標方向）は、目標方向Ｄｔ（図５）と直交している。目標方向Ｄｔは、第一目標方向とも称されうる。

図１４と図８とを比較すれば明らかとなるように、本実施形態では、第一実施形態のＳ１１０８に対して、角度偏差の算出が追加されている。Ｓ１１０８において、位置方向算出部３１３ｂは、検出された目標面４１１の長手方向Ｄｔｌ（第二目標方向）を、深度カメラ２００の２次元座標系における、検出された目標面４１１に対応した位置座標に対する演算処理により算出する。これにより、第二目標方向および角度偏差の演算処理を比較的容易に実行することができる。これは、ハンド１２０の基準方向が深度カメラ２００の２次元座標系において固定されているからである。また、位置方向算出部３１３ｂは、主成分分析を用いて、長手方向Ｄｔｌ（第二目標方向）を算出する。位置方向算出部３１３ｂは、角度偏差算出部の一例である。

具体的に、Ｓ１１０８において、位置方向算出部３１３ｂは、検出された目標面４１１の深度画像における各画素の座標値（ｕ_ｉ，ｖ_ｉ）とその中心値（ｕ_ｍ，ｖ_ｍ）との差を算出する。Ｓ１１０８において、位置方向算出部３１３ｂは、次に、算出された差を含む次の式（１９）のような行列Ａを構成する。

Ｓ１１０８において、位置方向算出部３１３ｂは、次に、特異値分解により行列Ａを以下の式（２０）のように分解する。
Ａ＝ＵＤＶ^Ｔ ・・・（２０）
これにより、位置方向算出部３１３ｂは、第一主成分の方向であるＶの第一列ベクトル

を得る。

Ｓ１１０８において、位置方向算出部３１３ｂは、次に、深度カメラ２００の２次元座標系に当該２次元座標系と直交した１次元座標を加えた３次元空間上でのベクトルを以下の式（２１）のように構成する。

Ｓ１１０８において、位置方向算出部３１３ｂは、次に、長手方向Ｄｔｌと方向Ｄｃｌとの角度偏差として、式（２１）のベクトルと、次の式（２２）で表されるベクトルとの偏差を算出する。

式（２１）のベクトルと、式（２２）のベクトルとの偏差は、以下の式（２３）の外積と式（２４）の内積とを用いて、式（２５）として算出することができる。

Ｓ１１０８において、指令値算出部３１３ｃは、以下の式（２６）によって示される偏差θｕが小さくなるよう、指令値を算出する。

なお、式（２６）の偏差θｕには、目標方向Ｄｔと光軸方向の単位ベクトル［０，０，１］^Ｔとの偏差も含まれる。

以上、説明したように、本実施形態によれば、位置方向算出部３１３ｂ（角度偏差算出部）は、目標方向Ｄｔ（第一方向）に沿う回転軸回りの、ハンド１２０（可動部材）の方向Ｄｃｌ（基準方向）と検出された目標面４１１の長手方向Ｄｔｌ（第二目標方向、所定姿勢）との角度偏差を検出し、移動制御部３１３は、さらに当該角度偏差が小さくなるよう、アクチュエータ１１３を制御する。このような構成によれば、制御装置３００は、ハンド１２０を、目標面４１１の向きや、ハンド１２０のタイプ、ハンド１２０のスペック等に応じて、目標物４１０をより把持しやすい方向に、向けさせることができる。

なお、本実施形態で開示された第二目標方向の検出手法は、ハンド１２０が例えば挟持のような別の把持方法によって目標物４１０を把持するような構成にも、適用可能である。その場合、位置方向算出部３１３ｂは、例えば、第二目標方向としての目標面４１１の短手方向に対する基準線分の方向（基準方向）の角度偏差を算出し、指令値算出部３１３ｃは、当該角度偏差が小さくなるよう、指令値を算出することができる。この場合には、例えば、内積により、主成分分析によって得られる第二主成分ベクトルと、基準方向の単位ベクトルとの相対角度が角度偏差となる。

［第３実施形態］
［物体ハンドリングシステムの全体概要］
図１６は、学習済みのディープニューラルネットワーク（ＤＮＮ）で構成された目標決定部のブロック図である。ＤＮＮは、機械学習の一つである。本実施形態の作動システム１は、第１実施形態の作動システム１の目標物決定部３１２に替えて、機械学習を用いた演算処理を実行する目標物決定部３１２Ａを備えている。目標物決定部３１２Ａは、信号処理部３１２ａ、ＤＮＮ型把持計画部３１２ｂ、および出力選択部３１２ｃを有している。

信号処理部３１２ａには、３次元情報に変換可能な撮像情報が入力される。ここで、３次元情報に変換可能な撮像情報とは、例えば、深度画像や、視差若しくは超音波の計測により距離情報の取得が可能な複数枚の画像データ、ポイントクラウドのような３次元点群情報である。信号処理部３１２ａは、入力された撮像情報のデータ型式を、ＤＮＮ型把持計画部３１２ｂが演算処理可能な型式のデータ型式に変換する。この変換には、例えば、多視点画像から距離画像への変換のようなデータ形式の変換や、画像サイズの縮小拡大、アスペクトやカメラ歪みの補正などの画像処理、等が含まれる。

ＤＮＮ型把持計画部３１２ｂは、ハンド１２０で把持可能な把持姿勢候補の座標に加え、算出した座標の有効性を示す評価値、および算出した座標における把持成否予想ラベル情報のうちのいずれか、または両方の情報を出力する。

出力選択部３１２ｃは、入力された複数の情報から、最終的に移動制御部３１３に入力する把持点のＸ，Ｙ座標等の情報を選択し、出力する。この選択には、ＤＮＮ型把持計画部３１２ｂから出力された把持姿勢候補の中から最も評価値の高い把持姿勢候補を選択する方法や、高い評価値が集中する領域の中心を選択する方法などがある。

図１７は、入力画像に対するＤＮＮ型把持計画部３１２ｂによる出力結果のマッピングを示す説明図である。画像Ｉｉは、入力画像であり、ここでは、一例として、深度カメラ２００で撮影された深度画像である。画像Ｉｍは、入力画像ＩｉにＤＮＮ型把持計画部３１２ｂによる出力結果が重畳されたマッピング画像Ｉｍである。マッピング画像Ｉｍでは、入力画像Ｉｉ中の把持姿勢候補のそれぞれに、Ｇｏｏｄ（把持可能、丸形）およびＢａｄ（把持不可能、菱形）の把持成否予想ラベルが付与されるとともに、評価値が付与されている。白い丸形はＧｏｏｄの把持姿勢候補のうち、評価値が最も高いものである。なお、図１７の例では、ＤＮＮ型把持計画部３１２ｂが、限定的な範囲内の把持姿勢候補を評価する例が示されているが、これには限定されず、ＤＮＮ型把持計画部３１２ｂは、入力画像Ｉｉの全体に対して評価付けを行い、全画素に対してラベルや評価値を付与してもよい。

図１８は、目標物決定部３１２Ａによる初期把持姿勢および目標位置の決定の手順を示すフローチャートである。図１８に示されるように、目標物決定部３１２Ａは、まず、入力画像Ｉｉとしての深度画像を取得する（Ｓ１２１）。Ｓ１２１において、信号処理部３１２ａは、データの変換を実行する。次に、ＤＮＮ型把持計画部３１２ｂは、把持姿勢候補を取得する（Ｓ１２２）。次に、出力選択部３１２ｃは、複数の把持姿勢候補の中から評価値に基づいて初期把持姿勢を決定するとともに当該初期把持姿勢に対応した目標位置を決定する（Ｓ１２３）。

図１９は、ＤＮＮ型把持計画部３１２ｂが用いるＤＮＮを作成する学習システム３１５の構成を示すブロック図である。図１９に示されるように、学習システム３１５は、環境条件入力部３１５ａ、物理シミュレータ部３１５ｂ、画像取得部３１５ｃ、ハンド条件入力部３１５ｄ、把持姿勢候補生成部３１５ｅ、およびＤＮＮ学習処理部３１５ｆを有している。環境条件入力部３１５ａは、外部システムまたはユーザインタフェースを通して、ハンドリング対象の物体の基本形状や、出現比率、物体が置かれた周囲環境の情報等を、取得し、物理シミュレータ部３１５ｂに入力する。物理シミュレータ部３１５ｂは、設定された環境条件をベースとし、ランダムな要素を加味して、ハンドリング対象となる環境モデルを生成する。

図２０Ａは、物理シミュレータ部３１５ｂが計算空間上に生成したバラ積み状態のバーチャルデータモデルを示す斜視図であり、図２０Ｂは、物理シミュレータ部３１５ｂが生成したバラ積み状態のバーチャルデータモデルから生成した深度画像の例である。画像取得部３１５ｃは、環境モデルとしてのバーチャルデータモデルに対し、深度カメラ２００のような撮像処理を模擬した処理を実行し、深度画像を取得する。

ハンド条件入力部３１５ｄは、外部システムまたはユーザインタフェースを通して、ハンドリングを実行するハンド１２０の条件、例えば、ハンド１２０のサイズや、把持可能サイズを設定する。具体的に、ハンド条件入力部３１５ｄは、例えば、ハンド１２０が吸着ハンドである場合には、ハンド条件として、吸着パッドの位置や、パッドの大きさなどを設定し、ハンド１２０が挟持ハンドである場合には、ハンド条件として、挟持幅や、差し込み量などを設定する。ハンド条件は、物理的特性の一例である。

把持姿勢候補生成部３１５ｅは、入力されたハンド条件と物理シミュレータ部３１５ｂが生成した環境モデルから、把持姿勢候補の情報、具体的には、例えば、把持姿勢候補の座標、把持姿勢候補の把持成否予想ラベル情報、把持姿勢候補の有効性を示す評価値等を生成する。図２０Ｃは、把持姿勢候補情報の例を示す。

ＤＮＮ学習処理部３１５ｆは、入力された画像情報と把持姿勢候補を用いて教示または深層学習を実行することにより、ＤＮＮを生成する。ＤＮＮ型把持計画部３１２ｂは、ＤＮＮ学習処理部３１５ｆが生成したＤＮＮを用いて、演算処理を実行する。

以上説明したように、本実施形態では、目標物決定部３１２Ａは、撮像された深度画像に基づいて、物が置かれた状態やハンドの特性など複数の条件を加味した初期把持姿勢および目標位置を出力し、移動制御部３１３は、当該目標物決定部３１２Ａが出力した初期把持姿勢および目標位置を起点として移動制御を実行する。このように、深度画像を用いた目標物決定部３１２Ａおよび移動制御部３１３を備える作動システム１によれば、例えば、外部システムや人間が、把持候補や把持候補となるゴール画像（ＲＧＢ画像）を別途入力する必要が無く、新規の未知な物体に対しても、妥当な把持やハンドリングが実現される、という効果が得られる。

また、本実施形態によれば、例えば、目標物決定部３１２Ａは、把持姿勢候補の生成にニューラルネットワークを用いるため、多様な条件を設定できるとともに、候補算出の処理時間が比較的短いという効果が得られる。

さらに、本実施形態によれば、例えば、学習システム３１５は、ニューラルネットワークの学習時において、シミュレータで生成した環境モデルから、入力となる深度画像と、教示データとを自動的に生成し、教示あり学習を行うことにより、実環境やゴール画像などのカラー画像がない状況でも、目標物決定部３１２ＡのＤＮＮを生成することができる、という効果が得られる。

［第４実施形態］
図２１は、第４実施形態のピッキングシステム１０００の構成図である。図２１に示されるように、ピッキングシステム１０００は、ピッキングオーダー等の処理を行う上位システム２０００と接続されるとともに、ピッキングロボット管理部１００１、ピッキングロボット制御部１００２、作動システム１の一例であるピッキングロボットシステム１００３、収納箱搬送部１００４、送付箱搬送部１００５、収納箱撮影部１００６、および送付箱撮影部１００７を備えている。収納箱１００８は、ビンとも称され、送付箱１００９は、トートとも称されうる。

上位システム２０００は、送付する荷物を決定し、自動倉庫等の外部倉庫システムから送付する物品が収納されている収納箱１００８の掃き出し処理を実施し、ピッキングシステム１０００に対し、送付箱１００９へのピッキングオーダーを出力する。ピッキングシステム１０００では、ピッキングロボット管理部１００１がピッキングオーダーを受信し、ピッキングシステム１０００が処理する対象物品個数を取得し、対象物品の重量や外形サイズ、対応したハンドや処理パラメータ等の物品関連情報を決定して、それらの情報をピッキングロボット制御部１００２に入力する。ピッキングロボット制御部１００２では、物品関連情報に基づいて、ピッキング及び箱詰め作業を行うよう、ピッキングロボットシステム１００３を制御する。ピッキングロボットシステム１００３は、アーム１１１や、関節１１２、ハンド１２０、ツールチェンジャ部、力センサ、接触センサ、把持関節、吸着パッド等を備えている。

収納箱搬送部１００４はピッキングロボット管理部１００１の情報に従い、外部倉庫システムから吐き出された収納箱１００８をピッキングロボットシステム１００３の処理範囲内に引き込む。送付箱搬送部１００５は、ピッキングの開始には、空箱の引き込みを行い、搬送箱が満杯になった状態、またはピッキングが終了した状態になった場合には、物品が詰め込まれた送付箱１００９の出荷処理を実行する。収納箱撮影部１００６は、収納箱１００８がピッキングロボットシステム１００３の稼働範囲に搬送された状態において、収納箱１００８の深度画像または３次元状態を撮影する機能を有している。送付箱撮影部１００７は、送付箱１００９がピッキングロボットシステム１００３の稼働範囲に搬送された状態に置いて、送付箱１００９の深度画像または３次元状態を撮影する機能を有している。

図２２は、ピッキングシステム１０００におけるピッキングの手順を示すフローチャートである。ピッキングシステム１０００では、ピッキングロボット管理部１００１がピッキングオーダーを受信すると、ピッキングを開始する。

Ｓ２０１において、ピッキングロボット管理部１００１はデータベースからピッキングオーダーで指定された物品の情報を取得する。取得した情報から、ハンド１２０の大きさや、吸着あるいは挟持などの把持方法に基づいて、最適なハンドを決定する。そして、最適なハンド１２０を持ったピッキングロボットシステム１００３にピッキング指示を出すとともに、収納箱搬送部１００４を制御して、オーダーされた物品が収納された収納箱１００８を、決定されたピッキングロボットシステム１００３に届ける。

Ｓ２０２において、ピッキングロボットシステム１００３は収納箱撮影部１００６を用いて、引き込まれた収納箱１００８の撮影を実施し画像を取得する。ピッキングロボットシステム１００３の目標物決定部３１２または目標物決定部３１２Ａにより、取得した画像からピッキングの対象となる物品もしくは物品の把持面を決定する。

Ｓ２０３において、ピッキングロボットシステム１００３は、事前に決定された位置情報によりアクチュエータ１１３，１１４を制御して、ハンド１２０を収納箱１００８の近くの所定の位置に移動する。

Ｓ２０４において、ピッキングロボットシステム１００３は、収納箱撮影部１００６のカメラを用いて、Ｓ２０２で得られたピッキングの対象となる物品もしくは物品の把持面が、ハンド１２０に設けられた深度カメラ２００の視野に入るよう、位置制御を実行する。

Ｓ２０５において、ピッキングロボットシステム１００３は、ハンド１２０に設けられた深度カメラを用いて、図３で説明されたフローに従って、ハンド１２０を対象物品に近づける。

Ｓ２０６において、ピッキングロボットシステム１００３は、ハンド１２０に設けられた力センサの検出値、またはアクチュエータ１１３，１１４としてのモータのエンコーダ情報若しくは電流制御量等を用いて、ハンド１２０にかかる力を計測し、力制御を用いて対象物品にハンド１２０の突き当てを行い、ハンド１２０の手先の吸着パッド等を対象物品に倣わせ、対象物品の把持を実行する。

Ｓ２０７において、ピッキングロボットシステム１００３は、あらかじめ決められた位置情報に従って、アーム１１１およびハンド１２０を動かし、把持物品を送付箱１００９に近付くように移動する。

Ｓ２０８において、送付箱撮影部１００７は、送付箱１００９を撮影する。ピッキングロボットシステム１００３は、先に置かれた物品等の状態をセンシングし、箱詰め可能な位置を決定する。

Ｓ２０９において、ピッキングロボットシステム１００３は、箱詰め可能な位置情報を用いて、把持物品の位置制御による移動制御を実行し、把持物品を箱詰め可能な位置付近まで移動する。

Ｓ２１０において、ピッキングロボットシステム１００３は、力制御によりハンド１２０の移動を制御することにより把持物品を送付箱１００９の内面に押し当て、所定の力に達した状態で、物品の把持を解除し、物品を送付箱１００９内に置き、箱詰めする。

Ｓ２１１において、ピッキングロボットシステム１００３は、ピッキング終了か否かを判定する。オーダーされた物品で未処理のものが残っている場合には、Ｓ２０２からピッキング処理を再開する。オーダーされた物品の処理をすべて終了している場合には、Ｓ２１２へ移行する。

Ｓ２１２において、ピッキングロボットシステム１００３は、アーム１１１およびハンド１２０を初期位置に移動するとともに、送付箱１００９の出荷を行い、オーダーに対するピッキング処理を完了する。

以上のように、本実施形態では、作動システム１としてのピッキングロボットシステム１００３を有したピッキングシステム１０００について説明を行ったが、上記実施形態の構成や制御は、例えば、ロボットを用いて部品を保持する組立作業や、ロボットを用いた物体を把持したり搬送したりするフィールドでの活用、ロボットに取り付けたセンサ等のエンドエフェクタを物体に近づけてセンシングを行うような検査などのような、他のシステムにも適用することができる。

以上、本発明の実施形態を例示したが、上記実施形態は一例であって、発明の範囲を限定することは意図していない。実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、組み合わせ、変更を行うことができる。実施形態は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。また、実施形態の構成や形状は、部分的に入れ替えて実施することも可能である。また、各構成や形状等のスペック（構造や、種類、方向、形式、大きさ、長さ、幅、厚さ、高さ、角度、数、配置、位置、材質等）は、適宜に変更して実施することができる。

例えば、深度カメラの位置や数は、上記実施形態には限定されず、２個以上であってもよいし、他の位置でもよい。また、処理対象物（目標物）は、直方体でなくてもよく、種々の形状を有しうる。また、目標面は完全な平面で無くてもよく、目標面検出部における検出基準（閾値）の設定により、例えば、目標面が凹凸面や緩やかな曲面であるような場合にあっても、上記実施形態の構成および方法を適用することができる。また、記憶部には、前のステップでの目標面に含まれる少なくとも一点の座標に替えて、既に算出された特定範囲を示すデータや、後のタイムステップで特定範囲を得る元となるデータが記憶されてもよい。

１…作動システム、１１１…アーム（可動部材）、１１３，１１４…アクチュエータ、１２０，１２０Ａ…ハンド（可動部材）、２００…深度カメラ（深度センサ）、３００…制御装置、３１３…移動制御部、３１３ａ…目標面検出部、３１３ｂ…位置方向算出部（位置算出部、方向算出部、角度偏差算出部）、４１０…目標物、４１１…目標面、Ａ１…第一領域、Ａ２…第二領域、Ａｄ…特定範囲、Ａｐ…近接領域、Ｄｃ…現在方向（第二方向）、Ｄｔ…目標方向（第一方向）、Ｐｃ…現在位置（第二位置）、Ｐｔ…目標位置（第一位置）、Ｖ１，Ｖ２…ベクトル。

Claims (9)

1. 可動部材と、
   前記可動部材を動かすアクチュエータと、
   前記可動部材に設けられた深度センサによって得られた深度画像から目標物の目標面を検出する目標面検出部と、
   第一時刻において検出された目標面内の少なくとも一点について前記深度画像における座標を記憶する記憶部と、
   前記検出された目標面について所定の第一位置を算出する位置算出部と、
   前記検出された目標面について所定の第一方向を算出する方向算出部と、
   前記可動部材に対して固定された第二位置と前記第一位置との位置偏差が小さくなるとともに、前記可動部材に対して固定された第二方向と前記第一方向との方向偏差が小さくなるよう、前記アクチュエータを制御する移動制御部と、を備え、
   前記目標面検出部は、前記第一時刻よりも後の第二時刻において、前記記憶された座標に基づいて前記深度画像の全域よりも狭い特定範囲を設定し、前記特定範囲内において前記目標面の第一領域を検出し、前記特定範囲外において前記目標面として前記第一領域と繋がる第二領域を検出する、
   作動システム。
2. 前記方向算出部は、前記検出された目標面の近似平面の法線方向として、前記第一方向を算出する、請求項１に記載の作動システム。
3. 前記方向算出部は、前記検出された目標面内の各画素について、当該画素および当該画素と隣接する複数の画素を含む近接領域の三次元位置から法線方向を算出し、前記検出された目標面内の複数の画素における前記法線方向の代表値として、前記第一方向を算出する、請求項１に記載の作動システム。
4. 前記方向算出部は、前記近接領域に沿う二つのベクトルの外積から前記法線方向を算出する、請求項３に記載の作動システム。
5. 前記検出された目標面の前記第一方向に沿う回転軸回りの前記可動部材の所定姿勢からの角度偏差を算出する角度偏差算出部を備え、
   前記移動制御部は、さらに前記角度偏差が小さくなるよう、前記可動部材を動かすアクチュエータを制御する、請求項１～４のうちいずれか一つに記載の作動システム。
6. ３次元情報に変換可能な撮像情報および前記可動部材の物理的特性に基づいて生成された学習データにより学習されたネットワークを含み、取得された前記撮像情報に対応した前記目標物を決定する目標物決定部を備えた、請求項１～５のうちいずれか一つに記載の作動システム。
7. 可動部材に設けられた深度センサによって得られた深度画像から目標物の目標面を検出する目標面検出部と、
   第一時刻において検出された目標面内の少なくとも一点について前記深度画像における座標を記憶する記憶部と、
   前記検出された目標面について所定の第一位置を算出する位置算出部と、
   前記検出された目標面について所定の第一方向を算出する方向算出部と、
   前記可動部材に対して固定された第二位置と前記第一位置との位置偏差が小さくなるとともに、前記可動部材に対して固定された第二方向と前記第一方向との方向偏差が小さくなるよう、前記可動部材を動かすアクチュエータを制御する移動制御部と、を備え、
   前記目標面検出部は、前記第一時刻よりも後の第二時刻において、前記記憶された座標に基づいて前記深度画像の全域よりも狭い特定範囲を設定し、前記特定範囲内において前記目標面の第一領域を検出し、前記特定範囲外において前記目標面として前記第一領域と繋がる第二領域を検出する、
   制御装置。
8. ３次元情報に変換可能な撮像情報および前記可動部材の物理的特性に基づいて生成された学習データにより学習されたネットワークを含み、取得された前記撮像情報に対応した前記目標物を決定する目標物決定部を備えた、請求項７に記載の制御装置。
9. コンピュータを、請求項１～６のうちいずれか一つに記載の作動システムの、前記目標面検出部、前記位置算出部、前記方向算出部、および前記移動制御部、として機能させるプログラム。

Images