JP7204587B2 - 物体ハンドリング制御装置、物体ハンドリング装置、物体ハンドリング方法および物体ハンドリングプログラム - Google Patents

Description

本発明の実施形態は、物体ハンドリング制御装置、物体ハンドリング装置、物体ハンドリング方法および物体ハンドリングプログラムに関する。

従来、物流、製造現場等の自動化に向けて、ロボットアーム等を用いた物体ハンドリング装置の導入が進んでいる。物体ハンドリング装置は、ハンドにより把持した物体を移動元位置（例えば、コンベア上の位置や物体を収納した移動前容器等）から、移動先位置（例えば、所定の容器やパレット等）に移動（移載）する。この場合、物体ハンドリング装置は、把持した物体やハンドが周囲の障害物と干渉しないことが求められる。しかし、実際には把持した物体のサイズや把持位置、移動先の容器の壁部や既に移動済みの物体（以下、障害物という場合がある）等の計測誤差や、ハンドの動作精度などの影響により、干渉が発生してしまうことがある。そこで、ロボットハンドにエンドエフェクタからの反力を検出する力覚センサを備え、反力をもとにしたフィードバック制御(力制御)により、干渉した障害物から離れる方向へ動作する反発動作により、衝突でロボットが停止せずに動作し続けさせることができる技術が提案されている。

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

しかしながら、力制御による反発動作を用いた場合であっても、障害物に対するハンドや把持した物体の衝突時の速度によっては、ハンドや把持した物体が破損してしまうことがある。この場合、移動経路全体または障害物に近い範囲内で、アーム（ハンド）の動作速度を遅くすることで衝突時の衝撃を低減する方法が考えられるが、移載動作に要する時間が長くなり、作業効率が低下するという問題がある。また、移動経路を計画する際にハンドや把持した物体および障害物のサイズを仮想的に大きく見積もり、障害物との距離を十分に確保する（マージンを拡大する）ことで衝突危険性を低減する方法が考えられるが、狭い隙間にハンドが入り込む必要がある場合や、把持した物体を密に配置したい場合に移動経路が計画できないという問題がある。

そこで、本発明の課題の一つは、ハンドやハンドが把持した物体の破損を招くよう障害物との干渉を回避し効率的に物体を移動させることのできる、物体ハンドリング制御装置、物体ハンドリング装置、物体ハンドリング方法および物体ハンドリングプログラムを提供することを目的の一つとする。

実施形態の物体ハンドリング制御装置は、例えば、情報取得部と、領域設定部と、経路算出部と、を備える。情報取得部は、物体を把持可能な把持部に把持された状態の物体に関する物体情報と、物体の移動元位置と物体の移動先位置に関する状態情報と、を少なくとも取得する。領域設定部は、物体を把持した把持部が移動元位置から移動先位置に移動する場合に、移動元位置と移動先位置との間の空間に存在する障害物に制限されることなく移動が許可される第１領域と、障害物の存在に基づいて移動が制限される第２領域と、少なくとも一部が第２領域の下方に設定され把持部に力制御として力制御動作を実行させる第３領域と、を物体情報と状態情報に応じて設定する。経路算出部は、第１領域と第２領域と第３領域とに基づき物体を移動元位置から移動先位置に移動させる移動経路を算出する。

図１は、本実施形態の物体ハンドリング装置を含む物体ハンドリングシステムを例示的かつ模式的に示す概略構成図である。 図２は、本実施形態の物体ハンドリング装置を含む物体ハンドリングシステムの構成を例示的かつ模式的に示す構成ブロック図である。 図３は、本実施形態の物体ハンドリング装置を含む物体ハンドリングシステムの動作全体の流れを例示的かつ模式的に示す動作フロー図である。 図４は、本実施形態の物体ハンドリング装置における物体の配置計画の入出力を説明する例示的かつ模式的な説明図である。 図５は、本実施形態の物体ハンドリング装置による物体の配置計画の例示的なフローチャートである。 図６は、本実施形態の物体ハンドリング装置により物体の移動（配置、詰め込み）を行う場合に移動先の容器の壁との間にある空間が狭い場合にも、物体を配置して充填率を向上させる例を示す模式図である。 図７は、空間が狭い場合、把持している物体と移動先の容器や移動済みの物体とが衝突する可能性がある場合を説明する例示的かつ模式的な説明図である。 図８は、本実施形態の物体ハンドリング装置による力制御によって、物体を移動先の容器の壁や既配置の物体に押し当てながら移動させる押当制御動作の例示的かつ模式的な説明図である。 図９は、本実施形態の物体ハンドリング装置による力制御によって、ハンドや物体が移動先の容器の壁や既配置の物体の側面と干渉した場合に実行される反発制御動作の例示的かつ模式的な説明図である。 図１０は、本実施形態の物体ハンドリング装置において、移動先の容器の壁や既配置の物体の存在に基づいて、移動が制限される第２領域を設定して干渉を回避することを示す例示的かつ模式的な説明図である。 図１１は、本実施形態の物体ハンドリング装置において、移動先の容器の壁や既配置の物体の存在に基づいて、第１領域、第２領域、第３領域を設定して干渉を回避しながら物体（およびハンド）の移動経路を生成することを示す例示的かつ模式的な説明図である。 図１２は、本実施形態の物体ハンドリング装置において、第２領域の設定例を説明する模式的な上面図である。 図１３は、本実施形態の物体ハンドリング装置において、第２領域の設定例を説明する模式的な側面図である。 図１４は、本実施形態の物体ハンドリング装置において、第３領域における第１制御領域の設定例を説明する模式的な上面図である。 図１５は、本実施形態の物体ハンドリング装置において、第３領域における第１制御領域の設定例を説明する模式的な側面図である。 図１６は、本実施形態の物体ハンドリング装置において、第２領域を設定する場合に、ハンドが把持する物体のサイズを考慮する場合の物体の考慮値を説明する例示的かつ模式的な上面図である。 図１７は、本実施形態の物体ハンドリング装置において、第２領域を設定する場合にハンドが把持する物体のサイズを考慮する場合の物体の考慮値を説明する例示的かつ模式的な側面図である。 図１８は、本実施形態の物体ハンドリング装置において、ハンドが把持する物体のサイズを考慮した、第２領域の設定を説明する例示的かつ模式的な上面図である。 図１９は、本実施形態の物体ハンドリング装置において、ハンドが把持する物体のサイズを考慮した、第２領域の設定を説明する例示的かつ模式的な側面図である。 図２０は、本実施形態の物体ハンドリング装置において、第２領域を設定する場合に、ハンドのサイズを考慮する場合のバンドの考慮値を説明する例示的かつ模式的な上面図である。 図２１は、本実施形態の物体ハンドリング装置において、第２領域を設定する場合に、ハンドのサイズを考慮する場合のバンドの考慮値を説明する例示的かつ模式的な側面図である。 図２２は、本実施形態の物体ハンドリング装置において、ハンドのサイズを考慮して、第２領域の設定を説明する例示的かつ模式的な上面図である。 図２３は、本実施形態の物体ハンドリング装置において、ハンドのサイズを考慮して、第２領域の設定を説明する例示的かつ模式的な側面図である。 図２４は、本実施形態の物体ハンドリング装置において、各領域の設定と移動経路を示す例示的かつ模式的な説明図である。 図２５は、本実施形態の物体ハンドリング装置において、各領域の他の設定と他の移動経路を示す例示的かつ模式的な説明である。 図２６は、本実施形態の物体ハンドリング装置において、第２領域を設定した場合の干渉チェック高さを示す例示的かつ模式的な説明図である。 図２７は、本実施形態の物体ハンドリング装置において、移動経路を生成する場合の初回の経由点の位置の生成例を示す模式的な説明図である。 図２８は、本実施形態の物体ハンドリング装置において、第２領域の通過を回避するハンドの位置を算出することを示す例示的かつ模式的な説明図である。 図２９は、本実施形態の物体ハンドリング装置において、第２領域の通過を回避する経由点の生成を示す例示的かつ模式的な説明図である。 図３０は、図２９の経由点に続き、第２領域の通過を回避する次の経由点の生成を示す例示的かつ模式的な説明図である。 図３１は、図３０の経由点に続き、第２領域の通過を回避する次の経由点の生成を示す例示的かつ模式的な説明図である。 図３２は、本実施形態の物体ハンドリング装置において、第２領域の通過を回避できない場合を示す例示的かつ模式的な説明図である。 図３３は、図３２の場合に、押当制御動作を行うことにより移動経路を例外的に生成することを示す例示的かつ模式的な説明図である。 図３４は、図３３の押当制御動作を実行する場合、押当方向を決定するための例示的なフローチャートである。 図３５は、本実施形態の物体ハンドリング装置において、押当制御動作による押当面と押当位置の生成例を示す模式的な説明図である。 図３６は、本実施形態の物体ハンドリング装置において、物体（ハンド）の移動経路の経由点の生成を説明する例示的なフローチャートである。 図３７は、図３６の移動経路の経由点の生成において、経由点の位置を説明する例示的かつ模式的な説明図である。

以下、実施形態を図面に基づいて説明する。以下に記載する実施形態の構成、ならびに当該構成によってもたらされる作用および結果（効果）は、あくまで一例であって、以下の記載内容に限られるものではない。

［システム全体構成］
図１は、物体ハンドリング装置を含む物体ハンドリングシステム１の概略構成図である。物体ハンドリングシステム１は、制御装置１０、マニピュレータ２０（ロボットアーム）、センサ３０及び筐体４０を備えている。物体ハンドリングシステム１は、移動対象の物体ＯＢＪを移動元位置ＨＰで把持して移動（搬送）し、移動先位置ＲＰで解放する。このような動作は、移動対象の物体ＯＢＪのピッキング（移載、搬送）と呼ばれている。なお、制御装置１０のうち、マニピュレータ２０を制御する後述するロボット制御部５７（移動制御部）以外の構成を物体ハンドリング制御装置として別体に構成してもよい。

マニピュレータ２０は、アーム２１と、ハンド２２（エンドエフェクタ、把持部）と、を備えている。アーム２１は、例えば、複数（例えば６個）のサーボモータで回転駆動される回転部２１ａ～２１ｆを有した多関節ロボットとして構成されている。また、マニピュレータ２０は、ハンド２２を交換可能に構成されてもよい。その場合、ハンド２２の交換には、図示しないツールチェンジャーのような機構が用いられてもよい。アーム２１は、多関節ロボットには限定されず、スカラロボットあるいは直動ロボットであってもよい。また、アーム２１は、多関節ロボット、スカラロボット、および直動ロボットのうち少なくとも二つの組み合わせであってもよい。

ハンド２２は、図１の例においては、移動対象の物体ＯＢＪを把持するための、例えば吸着機構としての吸着パッド２２ａを有しており、図示しない圧力センサが設けられている。また、吸着パッド２２ａの数、配置、形状、大きさのようなスペックは、種々に変更することができる。

また、移動対象の物体ＯＢＪの把持機構としては、吸着機構に限定されず、例えば、ジャミング機構、挟み込み機構、または多指機構を有してもよい。また、ハンド２２は、吸着機構、ジャミング機構、挟み込み機構あるいは多指機構のうち、少なくとも二つを有してもよい。

ハンド２２は、屈曲部２２ｂを有している。なお、ハンド２２は、屈曲部２２ｂ以外にも、例えば、回転部や伸縮部のような可動部を有することができる。

また、ハンド２２は、センサ３０として、６軸(並進力３軸、モーメント３軸)の力覚センサ３１を有している。力覚センサ３１の位置は、図示の位置には限定されない。また、ハンド２２は、力覚センサ３１以外の各種センサを有してもよい。後述するが、本実施形態では、隙間なく物体ＯＢＪを所定の空間に詰め込むために、物体ＯＢＪの配置時には力覚センサ３１の検出値をフィードバックして、力制御によって物体ＯＢＪを移動先の障害物に押し当る動作が可能である。この物体ＯＢＪを移動先の障害物に押し当る動作による力制御を押当制御と称する。このほか、本実施形態では、力覚センサ３１の検出値に基づき、力制御によってハンド２２や物体ＯＢＪと障害物との干渉が発生した場合に、干渉した障害物から離れる方向へハンド２２および物体ＯＢＪを移動させる動作が可能である。この干渉した障害物から離れる方向へ物体ＯＢＪを移動させる動作による力制御を反発制御と称する。

センサ３０は、力覚センサ３１の他、回転部２１ａ～２１ｆに設けられるモータのトルクセンサ、回転センサあるいは電流センサであってもよい。この場合において、制御装置１０は、ハンド２２に作用する力を、力覚センサ３１の検出値から得てもよいし、各モータの電流値や回転値（回転センサの検出値）から算出してもよい。

また、物体ハンドリングシステム１は、センサ３０として、複数のカメラ３２ａ，３２ｂ、複数の測域センサ３３ａ，３３ｂ等を備えている。カメラ３２ａ，３２ｂは、例えば、ＲＧＢ画像カメラである。また、測域センサ３３ａ，３３ｂは、光をスキャニングしながら物体ＯＢＪまでの距離を測定する二次元走査型の光距離センサであり、ＬＲＦ(Laser rangefinder)、ＬＩＤＡＲ(Light Detection and Ranging)などの呼ぶ場合もある。

移動元用のカメラ３２ａは、把持（搬出）する移動対象の物体ＯＢＪが存在している移動元位置ＨＰにおいて、上方から移動対象の物体ＯＢＪおよびその周辺領域を撮影し、物体ＯＢＪに関する物体情報（例えば、形状やサイズ等）や状態情報（例えば、静止状態の姿勢等）を取得する。移動元位置ＨＰにおいて、移動対象の物体ＯＢＪは、例えばコンテナやパレットのような移動元用の容器１４ａ内に収容されている。その場合、移動元用のカメラ３２ａは、容器１４ａ内の一部または全部を撮影する。移動元位置ＨＰは、移動対象の物体ＯＢＪの移動開始位置や出発位置とも称することができる。なお、図１においては、容器１４ａは、ベルトコンベアのような搬送機構上に載置されているが、これに限定されるものではない。

移動先用のカメラ３２ｂは、移動対象の物体ＯＢＪを解放（配置、搬入）する移動先位置ＲＰにおいて、当該移動先位置ＲＰおよびその周辺領域を撮影し、物体ＯＢＪに関する物体情報（例えば、既配置の物体の形状、サイズ等）や状態情報（例えば、既配置の物体の姿勢等）を取得する。移動先位置ＲＰにおいて、配置対象の物体ＯＢＪは、例えばコンテナやパレットのような解放用の容器１４ｂ内に収容される。その場合、移動先用のカメラ３２ｂは、容器１４ｂ内の一部または全部を撮影する。移動先位置ＲＰは、移動対象の物体ＯＢＪの移動終了位置や到着位置とも称することができる。なお、図１では、容器１４ｂは、ベルトコンベアのような搬送機構上に載置されているが、これに限定されるものではない。

測域センサ３３ａ，３３ｂは、ハンド２２により把持された物体ＯＢＪがアーム２１の駆動により移動元位置ＨＰから移動先位置ＲＰに移動する間に、物体ＯＢＪのサイズ（カメラ３２ａ等で取得しきれない面のサイズ等を含む）等の物体情報やハンド２２による物体ＯＢＪの把持姿勢等の状態情報を取得する。カメラ３２ａ，３２ｂ、測域センサ３３ａ，３３ｂ等で取得した物体情報は、移動先位置ＲＰで、物体ＯＢＪを「密」の状態で容器１４ｂに詰め込む際の位置制御等に用いることができる。

筐体４０内には、例えば、モータのような電動アクチュエータを駆動させるための電源装置、流体アクチュエータを駆動させるためのボンベ、タンク、およびコンプレッサー、各種安全機構など、物体ハンドリングシステム１の種々の部品や機器類が収容されうる。なお、筐体４０内に、制御装置１０が収容されるようにすることも可能である。

［制御装置］
図２は、本実施形態の物体ハンドリング装置を含む物体ハンドリングシステム１を例示的かつ模式的に示す構成ブロック図であり、主として、制御装置１０の概要構成が示されている。制御装置１０は、例えば、統合部５１、画像処理部５２、信号処理部５３、把持計画生成部５４、領域設定部５５、経路算出部５６、ロボット制御部５７（移動制御部）、周辺機器・Ｉ／Ｏ制御部５８（I／Ｏ：インプットアウトプット）、学習制御部５９、エラー検出部６０および内部ＤＢ６１を有している。

統合部５１は、外部Ｉ／Ｆ７１（Ｉ／Ｆ：インタフェース）からのユーザ入力情報や、物体ハンドリングシステム１の状態、センサ３０による検出値等に基づいて、物体ハンドリングシステム１の作業計画の生成や、運用、管理を実行する。

画像処理部５２は、センサ３０としてのカメラ３２ａ，３２ｂから得られた画像を処理し、動作計画や、動作制御、エラー検出、学習等に必要な情報を生成する。

信号処理部５３は、センサとしての測域センサ３３ａ，３３ｂ等から得られた情報（検出値）を処理し、動作計画や、動作制御、エラー検出、等に必要な情報を生成する。なお、画像処理部５２や信号処理部５３は、情報取得部の一例である。

把持計画生成部５４は、移動元位置ＨＰにおける移動対象の物体ＯＢＪの把持方法や、把持姿勢、移動元位置ＨＰにマニピュレータ２０（ハンド２２）を移動させるための移動経路や経由点等を算出する処理を実行する。また、移動先位置ＲＰで物体ＯＢＪを解放した後のハンド２２が、次の移動対象の物体ＯＢＪを把持しに行くための帰路の移動経路や経由点等を算出する処理を実行する。なお、これらの場合の移動経路は、周囲の障害物（例えば、容器１４ａ，１４ｂの壁面や今回移動対象となる物体ＯＢＪ以外の物体等）に干渉することなくハンド２２を移動させるように移動経路および経由点を算出するために、カメラ３２ａで取得した物体情報等が利用される。

領域設定部５５は、物体ＯＢＪを把持したハンド２２が移動元位置ＨＰから移動先位置ＲＰに移動する場合に、移動元位置ＨＰから移動先位置ＲＰとの間の空間で、カメラ３２ａ，３２ｂや測域センサ３３ａ，３３ｂで取得した物体情報や状態情報に基づいて、領域設定を実行する。領域設定部５５は、例えば、第１領域、第２領域、第３領域等を設定する。第１領域は、移動元位置ＨＰと移動先位置ＲＰとの間の空間に存在する障害物（容器１４ａ，１４ｂや既配置の物体）に制限されることなく移動が許可される領域である。第２領域は、障害物の存在に基づいて移動が制限される領域である。また、第３領域は、少なくとも一部が第２領域の下方に設定され、ハンド２２に力制御として力制御動作を実行させる領域である。第１領域は、例えばハンド２２を高速で移動させることができる領域である。また、第２領域は、ハンド２２の通過を制限（禁止）する領域である。第３領域は、力覚センサ３１による検出を実行し、物体ＯＢＪやハンド２２に干渉が発生した場合に反発制御動作により移動経路を修正できる領域である。さらに第３領域において、例えば、ハンド２２（物体ＯＢＪ）の移動速度を低下させたり、力覚センサ３１による検出精度を一時的に向上させたりしてもよい。

上述した力制御は、押当制御と反発制御を含む。第３領域の一部を力制御のうち押当制御動作を行なう領域に設定し、他の部分を力制御のうち反発制御動作を行なう領域に設定することもできる。この場合、第３領域のうち押当制御動作を行なう領域を、原則的に第２領域のようにハンド２２の通過を制限（禁止）し、例外的に押当制御動作を行なう場合に物体ＯＢＪおよびハンド２２の通過を可能とする領域として設定してもよい。

経路算出部５６は、第１領域と第２領域と第３領域とに基づき、物体ＯＢＪを移動元位置ＨＰから移動先位置ＲＰに移動させる移動経路を算出する。例えば、ハンド２２が、物体ＯＢＪを把持した移動元位置ＨＰから移動先位置ＲＰにスムーズに移動できるように、動作方法や、動作速度を含めた移動経路の算出を実行する。第１領域、第２領域、第３領域を設定し、移動経路にしたがって移動するハンド２２の挙動を、各領域で切り替えることで、物体ＯＢＪをより高速で、かつ障害物と干渉するリスクを低減した状態で、効率的に移動（搬送）させることができる。第１領域、第２領域、第３領域の設定およびそれに関連するハンド２２（物体ＯＢＪ）の動作の詳細は後述する。

ロボット制御部５７は、移動経路にしたがい物体ＯＢＪを把持したハンド２２を移動させるように、ハンド２２を含むマニピュレータ２０の動作速度、動作方向、動作姿勢等を制御する。

周辺機器・Ｉ／Ｏ制御部５８は、各種搬送機器の制御や、安全ドアなどの周辺機器７０の制御、各種センサ情報の取得、照明の制御等のための、インプットアウトプット（Ｉ／Ｏ）制御を実行する。

学習制御部５９は、マニピュレータ２０の振動抑制など動作精度向上のためのロボットモデル学習や、物体ＯＢＪの把持性能向上のための把持制御パラメータ学習、把持データベース学習、作業計画の実施性能を向上するためのエラー検知学習等の学習機能を制御する。本実施形態では、状況に応じて、後に述べる力制御において、パラメータの最適な値を選択する。これらの値は人間が経験値から設定してもよいが、機械学習の結果を適用することで、より省力化しながら、より効率の高い処理を実現することができる。

エラー検出部６０は、物体ハンドリングシステム１の状態、作業計画の実施状態、駆動制御状態、移動対象の物体ＯＢＪの把持、移動（搬送）の状態等を観測し、エラーを検知する。このエラー検知は、例えば、力覚センサ３１の出力をハンド２２の手先座標に変換した値、それらの値をローパスフィルタに通したものを観測し、既定の値を超えたらエラーと判断することで実現できる。この結果、処理中の作業を中断し、その後のリカバリ動作に移るなどの処理が可能となる。

内部データベース（ＤＢ）６１は、例えば、いずれも図示しないロボットデータベース、ハンドデータベース、物品データベース、把持データベースおよび環境データベースを含んでいる。

ロボットデータベース（ロボットＤＢ）には、例えば、物体ハンドリングシステム１の構造、各部の寸法や重量、慣性モーメント、各駆動部の動作範囲や速度、およびトルク性能が、保存されている。

ハンドデータベース（ハンドＤＢ）には、例えば、ハンド２２の機能、およびハンド２２の把持の特性に関する情報が、保存されている。

物品データベース（物品ＤＢ）には、例えば、移動対象の物体ＯＢＪの名称、識別番号、カテゴリ、全面の画像情報、ＣＡＤモデル情報、重量情報、および把持時の特性情報（柔らかい、壊れやすい、形が変わる等）が、保存されている。なお、本実施形態では、移動対象の物体ＯＢＪは、多種多様な形状、サイズを有し、混在した状態で、移動元位置ＨＰに導入される。そして、移動先位置ＲＰにおいて、混在した状態でありながら、「密」に配置することで、一度に容器１４ｂに収まる物体ＯＢＪの量を増加し、物流効率を向上させる。

把持データベース（把持ＤＢ）には、例えば、移動対象の物体ＯＢＪについて、ハンド２２の把持方法ごとに、把持可能位置、把持可能姿勢、把持のしやすさなどのスコア情報、把持時の押込可能量、把持判定のための判定閾値、およびエラー検知のための判定閾値が、保存されている。ここで、把持方法は、例えば、吸着方式、並行２指方式、平行４指方式、または多関節方式等がある。

環境データベース（環境ＤＢ）には、例えば、物体ハンドリングシステム１が対応している作業台情報、および物体ハンドリングシステム１の動作範囲や周囲の障害物を示すような周囲環境情報が、保存されている。

また、外部Ｉ／Ｆ７１は、統合部５１（制御装置１０）と外部機器（不図示）との間のデータの送信および受信を実行する。

［物体ハンドリングシステム（物体ハンドリング装置）の制御動作の概略］
物体ハンドリングシステム１は、外部Ｉ／Ｆ７１を介して提供される上位システムからの全物体ＯＢＪの移動作業計画等にしたがって動作する。

物体ハンドリングシステム１の全体の動作フローを図３に示す。物体ハンドリングシステム１（物体ハンドリング装置）の動作は大きくわけて計画系動作、認識系動作、ロボットアーム動作（移動）、吸着パッド動作（把持）に分けられる。

まず、統合部５１は、計画系の動作として、上位のシステムによって指示される移動作業計画を参照してオーダチェックを行う。この場合、カメラ３２ａによって移動元位置ＨＰにおける容器１４ａ内の物体ＯＢＪの認識、およびカメラ３２ｂによって移動先位置ＲＰにおける容器１４ｂ内の既配置の物体の認識を行うことで、物体情報や状態情報の取得を行う。そして、把持計画生成部５４が、把持計画（複数の物体ＯＢＪの把持の順序等）を生成する。また、物体ＯＢＪを把持しに行くための移動経路（把持経路）の計画を生成する。続いて、ロボットアーム動作として、ロボット制御部５７は生成された把持経路にしたがい、ハンド２２を把持待機位置（ホームポジション）から移動元位置ＨＰに移動させる。そして、ロボット制御部５７は、把持位置において吸着パッド動作として、ハンド２２（吸着パッド２２ａ）により、移動対象の物体ＯＢＪの把持動作を実行する。ロボット制御部５７は、物体ＯＢＪを把持した状態のハンド２２を、移動待機位置（例えば、測域センサ３３ａ，３３ｂの測定領域）に移動させ、その移動中に認識系の動作として、測域センサ３３ａ，３３ｂ（例えば、ＬＲＦ）による姿勢推定（物体ＯＢＪの姿勢や大きさ、把持姿勢等の推定）を実行する。

そして、計画系の動作として、領域設定部５５は、認識系で取得した物体情報や状態情報に基づいて、第１領域、第２領域、第３領域の設定を行い、経路算出部５６は、物体ＯＢＪの配置計画として、物体ＯＢＪを把持した状態のハンド２２の移動経路計画を生成する。

続いて、ロボットアーム動作として、ロボット制御部５７は、移動待機位置で待機する物体ＯＢＪを把持した状態のハンド２２を移動経路にしたがい、移動先位置ＲＰに移動させる。そして、ロボット制御部５７は、吸着パッド動作として、移動先位置ＲＰで吸着パッド２２ａの把持解放動作を実行し、物体ＯＢＪを所定の移動先位置ＲＰに配置する。その後、ロボット制御部５７は、物体ＯＢＪを解放したハンド２２を把持待機位置に復帰させて、一連の物体ＯＢＪの移動（移載、配置）動作を一旦完了し、次の物体ＯＢＪの移動処理を繰り返し実行する。

［物体の配置計画の概要］
図４は、物体ハンドリングシステム１（物体ハンドリング装置）における物体ＯＢＪの移動（配置）計画の入出力を説明する例示的かつ模式的な説明図である。また、図５は、物体ＯＢＪの配置計画の例示的なフローチャートである。

経路算出部５６は、配置計画を作成する場合、例えば、カメラ３２ａや測域センサ３３ａ、３３ｂ等により、現在ハンド２２（吸着パッド２２ａ）で把持され、これから移動して行き移動先位置ＲＰ（容器１４ｂ）に配置する物体ＯＢＪのサイズや物体ＯＢＪの把持姿勢等を入力情報として取得する（Ｓ１００）。続いて、経路算出部５６は、物体ＯＢＪを容器１４ｂに配置した場合の物体ＯＢＪの姿勢を、ハンド２２が現在把持している物体ＯＢＪの把持姿勢やサイズ等に基づいて計算する（Ｓ１０２）。また、経路算出部５６は、容器１４ｂ内に既に置かれている既配置の物体ＯＢＪｓの状態情報等に基づき、把持した物体ＯＢＪを配置可能な位置およびそのときの物体ＯＢＪの姿勢（以下、位置および姿勢をまとめて「位置姿勢」と表現する場合がある）、つまり、移動先位置ＲＰの候補を計算する（Ｓ１０４）。また、経路算出部５６は、容器１４ｂ内に既に置かれている既配置の物体ＯＢＪｓの情報等に基づき、物体ＯＢＪの配置時のハンド２２の手先位置ＴＣＰの位置姿勢の候補を複数パターン計算し（Ｓ１０６）、その中から最適な位置姿勢の候補を選択する（Ｓ１０８）。また、経路算出部５６は、移動（配置）時にマニピュレータ２０（ハンド２２）が実施する力制御のための情報(力目標値、押当面の位置、押当方向等)を決定する（Ｓ１１０）。経路算出部５６は、物体ＯＢＪのサイズ、把持姿勢、既配置の物体ＯＢＪｓの状態、物体ＯＢＪを配置したときの手先位置ＴＣＰの位置姿勢等に基づき、物体ＯＢＪを移動させるために移動経路を構成する経由点（経由位置姿勢ＲＡＰ）を計算する（Ｓ１１２）。なお、候補として計算される移動先位置ＲＰおよび経由位置姿勢ＲＡＰには、スコア（予め定められた優先度等）が付随し、そのスコアに基づき最適な移動先位置ＲＰおよび経由位置姿勢ＲＡＰが選択される。なお、経路算出部５６において、既配置の物体ＯＢＪｓや容器１４ｂ等の障害物との干渉がない移動経路の生成に成功した場合、ロボット制御部５７は、マニピュレータ２０を動作させる。

なお、本実施形態において、物体ＯＢＪやハンド２２の位置姿勢を表現する場合は、４行４列の同次変換行列Ｔを用いる。この場合、変換前の座標系を表す記号を左上に付し、変換後の座標系を表す記号を右下に添え字として付する。また、姿勢のみを表現する場合は３行３列の回転行列Ｒを用い、位置のみを表現する場合は３行１列の並進ベクトルｔを用いる。例えば、ワールド座標系(記号Ｗ)で表された移動先位置ＲＰの座標系の位置姿勢^ＷＴ_ＲＰは、以下の（式１）で表される。

座標変換においては、以下の（式２）が成り立つ。ただし、式中のＳ，Ｕ，Ｖは任意の座標系を表す。

また、（式３）に示すように、行列Ａに対し、ｉ行ｊ列目の要素を抽出する操作をＡ（ｉ，ｊ）と表す。また、ｉ行目の行ベクトル、ｊ列目の列ベクトルを抽出する操作をそれぞれＡ（ｉ，：）、Ａ（：、ｊ）と表す。

同様に、（式４）に示すように、ベクトルｕに対しても、ｉ番目の要素を抽出する操作をｕ（ｉ）と表す。

また、本実施形態において、直方体形状の領域を表現する場合、（式５）で示すように、物体の座標系やサイズと同様に、図心の位置の座標系は^ＵＴ_Ａｒｅａ、サイズは、ベクトル^ＵＳ_Ａｒｅａで表現する。

ところで、物体ハンドリングシステム１（物体ハンドリング装置）においては、可能な限り多くの物体ＯＢＪを移動先の容器１４ｂに移動して配置することが求められる。つまり、充填率を向上することが求められる。充填率を向上することで、出荷する場合に容器１４ｂの数を減らすことができ、結果として運搬コストの削減に寄与できる。そこで、図６に示すように、既配置の物体ＯＢＪｓや移動先の容器１４ｂの壁との間にある空間Ｓ_０が狭い場合にも、物体ＯＢＪを配置できるようにすることで、充填率の向上が見込める。しかし、空間Ｓ_０が狭い場合には、図７に示すように、ハンド２２が把持している物体ＯＢＪと移動先の容器１４ｂに既に配置されている物体ＯＢＪｓとが衝突する危険性が高くなる。特に、測域センサ３３ａ，３３ｂによる測定結果や、カメラ３２ｂ等の三次元センサによる移動先の容器１４ｂ内の既配置の物体ＯＢＪｓの測定結果の誤差が大きい場合、配置しようとする物体ＯＢＪを例えば移動経路Ｒで移動させると衝突する危険性が高くなる。物体ＯＢＪが障害物と衝突した場合、物体ＯＢＪ自体の破損やハンド２２やマニピュレータ２０の破損の原因になることがある。

このような衝突を避けるためには、例えば、配置位置や移動経路を計算する際に物体ＯＢＪのサイズを実サイズより大きくするなどマージンを設定する方法が考えられるが、一律に大きなマージンを設定すると、容器１４ｂの狭い空間に物体ＯＢＪを配置する計画自体が生成（立案）できない場合がある。

そこで、本実施形態では、狭い空間を有効に利用するために、力制御による押当制御動作と反発制御動作を利用する。図１で示したように、物体ハンドリングシステム１のハンド２２には６軸の力覚センサ３１が搭載されている。したがって、図８に示すように、物体ＯＢＪを移動経路Ｒに沿って移動させる過程で、移動先の容器１４ｂの壁１４Ｗや他の既配置の物体ＯＢＪｓに力制御によって矢印Ａ方向に押し当てながら配置することが可能になる。つまり、押当制御（押当動作）により物体間の隙間を小さくして充填率を向上することができる。また、図９に示すように、物体ＯＢＪを移動経路Ｒに沿って移動させる過程で、力制御を用いることで、ハンド２２や物体ＯＢＪが移動先の容器１４ｂの壁１４Ｗや既配置の物体ＯＢＪｓの側面と干渉した場合に、それらと逆方向の矢印Ｂ方向に動くように制御する(反発動作)ことも可能である。以下に示す本実施形態では、上述したような力制御動作を利用することで、仮に配置計画や経路計画を立案する際に用いた測定値の測定誤差が大きくなった場合でもその誤差を吸収し、狭い空間に物体ＯＢＪを配置する移動経路を計画することができる。

本実施形態の物体ハンドリングシステム１は、移動経路を効率的に算出するために、物体ＯＢＪを移動および配置する際に、障害物（容器１４ｂの壁１４Ｗや既配置の物体ＯＢＪｓ等）と干渉の可能性がある方向を鉛直方向と水平方向に分類する。鉛直方向の干渉とは、図１０において、物体ＯＢＪを矢印Ｒ方向に移動させた場合、物体ＯＢＪの底面と既配置の物体ＯＢＪｓの上面または移動先の容器１４ｂの上端縁との干渉を意味する(図１０の上段参照)。一方、水平方向の干渉とは、物体ＯＢＪを矢印Ｒ方向に移動させた場合、物体ＯＢＪの側面と、既配置の物体ＯＢＪｓの側面または移動先の容器１４ｂの壁１４Ｗ（内壁）との干渉である(図１１の上段参照)。物体ハンドリングシステム１の場合、ハンド２２は移動（配置）動作において、把持している物体ＯＢＪを上方から下方に下ろす動作が基本になるため、鉛直方向の干渉とは物体ＯＢＪの衝突であり、把持した物体ＯＢＪや既配置の物体ＯＢＪｓを損傷する可能性が高い。一方で水平方向の干渉は、接触面同士の摺動になるので、物体ＯＢＪ，ＯＢＪｓを損傷する可能性はひくい。したがって、上述した反発動作を利用すれば仮に干渉があった場合でも移動（配置）動作を継続できる可能性が高くなる。

そこで、本実施形態の物体ハンドリングシステム１の領域設定部５５は、物体ＯＢＪを把持したハンド２２が移動元位置ＨＰから移動先位置ＲＰに移動する場合に、移動元位置ＨＰと移動先位置ＲＰとの間の空間を、第１領域、第２領域、第３領域に分類し、通過態様をそれぞれ異ならせている。

第１領域は、移動元位置ＨＰと移動先位置ＲＰとの間の空間に存在する障害物（容器１４ｂの壁１４Ｗや既配置の物体ＯＢＪｓ等）に制限されることなく移動が許可される領域である。

第２領域は、障害物の存在に基づいて移動が制限される領域で、例えば、ハンド２２が少なくとも障害物の上方向から下方向への移動成分をもって進入または移動することが制限される領域であり、干渉を回避するために、移動経路計画においてハンド２２の手先位置ＴＣＰが上方向から下方向への移動成分を持った動作を行う間に通過してはいけない領域である。また、第２領域における移動の制限に、下方向から上方向への移動成分を持つ移動や側方向の移動成分を持つ移動も含め、第２領域をハンド２２や物体ＯＢＪの進入または移動を禁止する禁止領域として設定してもよい。

第３領域は、少なくともその一部が第２領域の下方（下側）に例えば隣接して設定される領域であり、例えば、第１制御領域と、第２制御領域を備える。第３領域は、ハンド２２に後述する力制御動作を実行させる領域である。第３領域を第２領域の下側に設定するにあたり、第２領域と第３領域の間に少なくとも一部第１領域を挟んで設定してもよい。
第３領域のうちの第１制御領域は、第２領域の下側で障害物に沿って設定される領域であり、第２領域より障害物に対する制限幅（マージン）が少ない（すなわち第２領域より側方向の幅が小さい）領域である。第１制御領域は、その側方向の全体が第２領域の下側に隣接して位置するように設定され、この第１制御領域にハンド２２が進入する場合、障害物に対する側方向の移動成分をもって進入することとなる。換言すれば、第１制御領域は第２領域の下側に設定されることにより、第２領域の上方から第１制御領域内に物体ＯＢＪやハンド２２が接近することはない。また、第１制御領域の内部に物体ＯＢＪやハンド２２が進入する場合は側方向の移動成分を持った接近に限られる。第１制御領域は第２領域の下側で障害物に側方向に隣接する領域であり、ハンド２２に対して力制御動作のうち特に押当制御動作を実行させる領域である。第１制御領域については、原則的に第２領域１００と同様に、例えば上方向から下方向への移動成分を持つ移動などの、移動や進入が禁止される領域としてもよい。この場合、この第１制御領域については、この原則に基づいて移動や進入を禁止すると経路生成が困難である場合や生成された経路が非効率である場合に例外的に押当制御動作を行なうことを条件として移動や進入を許されるように設定してもよい。

第３領域のうちの第２制御領域は、第１制御領域の障害物とは反対側の側方に隣接する領域であり、第１制御領域への進入経路となる領域である。第２制御領域のうち、少なくとも第１制御領域の側の部分は、第２領域の下側（下方）に位置しており、好ましくは、側方向において第２制御領域の全体が第２領域の下側（下方）に位置する。第２制御領域は、力制御動作のうち特に反発制御動作によってハンド２２が制御される領域である。すなわち第２制御領域では、ハンド２２が把持する物体ＯＢＪの実際の大きさが異なることなどによって障害物との干渉が発生した場合に、干渉した障害物から離れる方向へハンド２２を反発動作させる。

また、第２制御領域では、第１制御領域内で上述の押当制御動作の実行を許容するため、少なくともハンド２２の挙動を制限してもよい。この場合に第２制御領域内で行なうハンド２２の挙動の制限とは、例えば、ハンド２２の移動速度であり、第２制御領域では、第１領域通過時の速度より遅い速度とし、力制御動作のうちの押当制御動作への切り替えを適切なタイミングで（すなわち、第１制御領域への進入時に）実行できるようにしている。なお、第３領域については、押当制御動作を行なう第１制御領域を設定せずに、その全体を反発制御動作が実行される第２制御領域として設定してもよい。

領域設定部５５は、第１領域、第２領域、第３領域（第１制御領域および第２制御領域）をカメラ３２ａ，３２ｂや測域センサ３３ａ，３３ｂ等が取得した物体情報や状態情報に応じて設定する。なお、領域設定部５５は、第２領域および第３領域が重なる領域（重複領域）がある場合は、第３領域を優先してこの重複領域を第３領域として取り扱うように構成されている。特に、第３領域のうちの第１制御領域が第２領域と重複する重複領域がある場合、領域設定部５５は、第１制御領域を優先してこの重複領域を第１制御領域として取り扱うように構成される。

例えば、図１０の上段に示すように、物体ＯＢＪが容器１４ｂの壁１４Ｗに沿った既配置の物体ＯＢＪｓの上方から接近する場合、図１０の下段に示すように、既配置の物体ＯＢＪｓの上面の位置付近に第２領域１００を設定する。ハンド２２が把持した物体ＯＢＪは、第２領域１００を通過しないように移動経路Ｒで移動することにより、物体ＯＢＪｓとの衝突を確実に回避することができる。図１１の下段は、第３領域１０２を第１制御領域１０２ａと第２制御領域１０２ｂに分けて設定した例である。この場合、ハンド２２が把持した物体ＯＢＪは、第２領域１００および第１制御領域１０２ａを通過しないように移動経路Ｒで移動することにより、物体ＯＢＪｓとの衝突を確実に回避することができる。なお、図１１の下段の場合、移動経路Ｒに沿って移動する際に、物体ＯＢＪは、Ｘ方向に移動する場合（上方から下方の移動成分を持つ接近ではない場合）に第２領域１００に進入している例である。また、図１１の下段では、第２領域１００の下方に隣接する状態で、第１制御領域１０２ａおよび第２制御領域１０２ｂからなる第３領域１０２が設定され、第２領域１００および第３領域１０２以外の領域が第１領域１０４として設定されている例である。第１領域１０４については後述する。

図１２～図２３を用いて、第２領域１００および第３領域１０２のうちの第１制御領域１０２ａの設定の詳細を説明する。

まず、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに、鉛直方向の干渉を回避するための手先位置ＴＣＰの通過制限領域、すなわち、第２領域１００の設定例を図１２、図１３を用いて説明する。つまり、把持した物体ＯＢＪやハンド２２のサイズが限りなく「０」に近いと仮定して手先位置ＴＣＰの通過制限領域を設定する。鉛直方向に干渉する可能性（危険性）がある箇所は、容器１４ｂおよび既配置の物体ＯＢＪｓを上面視した図１２において、容器１４ｂの壁１４Ｗに関して楕円Ｃ１で囲んだ部分と、物体ＯＢＪｓに関して楕円Ｃ２で囲んだ部分である。この場合、ハンド２２が把持する物体ＯＢＪの底面と、壁１４Ｗの上端部との干渉を回避するための第２領域１００は、壁１４Ｗの内側で所定の第１マージン設定値δｘ（Ｘ方向）、第１マージン設定値δｙ（Ｙ方向）を取った領域（第２領域１００ｔ_０）が、壁１４Ｗに沿って設定される。同様に、物体ＯＢＪｓの上面には、第２領域１００として、物体ＯＢＪｓの上面縁部から所定の第１マージン設定値δｘ（Ｘ方向）、第１マージン設定値δｙ（Ｙ方向）を取った領域（第２領域１００ｄ_０）が、物体ＯＢＪｓの周囲を覆うように設定される。また、図１３は、図１２の容器１４ｂおよび物体ＯＢＪｓを側面視した場合の、壁１４Ｗに関する楕円Ｃ１の位置、物体ＯＢＪｓに関する楕円Ｃ２の位置、および壁１４Ｗの上部に設定される第２領域１００ｔ_０および物体ＯＢＪｓの上面に設定される第２領域１００ｄ_０の位置が示されている。図１３に示すように、第２領域１００ｔ_０および第２領域１００ｄ_０には、高さ方向（Ｚ方向）に、所定の第１マージン設定値δｚが設定されている。なお、所定の第１マージン設定値δｘ、第１マージン設定値δｙ、第１マージン設定値δｚは、予め試験等により決定される。

具体的に、既配置の物体ＯＢＪｓの位置姿勢を^TOTEＴ_Obstacle,i（ｉ＝１，…，ｎ：ｎは物体ＯＢＪｓの数）、サイズを^TOTEＳ_Obstacle,iとする。この場合、各既配置の物体ＯＢＪｓに対応する鉛直方向の干渉回避のための手先位置ＴＣＰの通過制限領域の位置姿勢^TOTEＴ_KeepoutZ,iおよびサイズ^TOTEＳ_KeepoutZ,iは以下の（式６）、（式７）で算出される。

続いて、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに、水平方向の干渉を回避するための手先位置ＴＣＰの通過制限領域、すなわち、第３領域１０２のうち第１制御領域１０２ａを設定する例を図１４、図１５を用いて説明する。水平方向に干渉する可能性（危険性）がある箇所は、容器１４ｂおよび既配置の物体ＯＢＪｓを上面視した図１４において、容器１４ｂの壁１４Ｗに関して楕円Ｃ３で囲んだ部分と、物体ＯＢＪｓに関して楕円Ｃ４で囲んだ部分である。この場合、ハンド２２が把持する物体ＯＢＪの側面と、壁１４Ｗの内側面との干渉を回避するための第１制御領域１０２ａは、壁１４Ｗの内側で所定の第２マージン設定値εｘ（Ｘ方向）、第２マージン設定値εｙ（Ｙ方向）を取った領域（第１制御領域１０２ａｗ０）が、壁１４Ｗに沿って設定される。同様に、物体ＯＢＪｓの外側面には、第１制御領域１０２ａとして、物体ＯＢＪｓの外側面から所定の第２マージン設定値εｘ（Ｘ方向）、第２マージン設定値εｙ（Ｙ方向）を取った領域（第１制御領域１０２ａｏｗ０）が、物体ＯＢＪｓのその側面を覆うように設定される。また、図１５は、図１４の容器１４ｂおよび物体ＯＢＪｓを側面視した場合の、壁１４Ｗに関する楕円Ｃ３の位置、物体ＯＢＪｓに関する楕円Ｃ４の位置、および壁１４Ｗの内側面に設定される第１制御領域１０２ａｗ０および物体ＯＢＪｓの外側面に設定される第１制御領域１０２ａｏｗ０の位置が示されている。なお、所定の第２マージン設定値εｘ、第２マージン設定値εｙ、第２マージン設定値εｚは、予め試験等により第１マージン設定値δｘ、第１マージン設定値δｙ、第１マージン設定値δｚより制限幅（マージン幅）が少なく設定されている。

具体的に、既配置の物体ＯＢＪｓの位置姿勢を^TOTEＴ_Obstacle,i（ｉ＝１，…，ｎ：ｎは物体ＯＢＪｓの数）、サイズを^TOTEＳ_Obstacle,iとする。この場合、各既配置の物体ＯＢＪｓに対応する水平方向の干渉回避のための手先位置ＴＣＰの通過制限領域の位置姿勢^TOTEＴ_{KeepoutXY,ｉ,j}および、サイズ^TOTEＳ_{KeepoutXY,ｉ,j}は以下の（式８）、（式９）で算出される。なお、j＝１，２，３，４であり、ｊの値は±Ｘ方向、±Ｙ方向に対応する。

なお、前述したように、物体ＯＢＪが鉛直方向から接近する場合の干渉の可能性（危険性）に比べて、水平方向から接近する場合の干渉の可能性（危険性）は、少ないとみなせる。したがって、各マージン設定値は、δ_ｘ＞ε_ｘ、δ_ｙ＞ε_ｙ、δ_ｚ＞ε_ｚとすることができる。

続いて、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに設定した第２領域１００に対して、把持した物体ＯＢＪの位置とサイズを考慮した第２領域１００を算出する。

まず、図１６に示すように、物体ＯＢＪを把持しているハンド２２の手先位置ＴＣＰから物体ＯＢＪのＸ方向、Ｙ方向のそれぞれの端面までの距離（考慮値）を、Δｘ_Item-、Δｘ_Item+、Δｙ_Item-、Δｙ_Item+とする。また、図１７に示すように、ハンド２２の手先位置ＴＣＰから物体ＯＢＪの底面までのＺ方向の距離（考慮値）を^TOTEＳＺ_Itemとする。

ここで、鉛直方向の干渉では把持した物体ＯＢＪの底面との干渉が問題になる。一方、水平方向の干渉においては、物体ＯＢＪの底面が干渉しなければ、側面には干渉しないと見なせる。つまり、物体ＯＢＪの底面の位置とサイズのみを考慮して第２領域１００を設定すればよいことになる。したがって、把持した物体ＯＢＪを考慮した第２領域１００は、上面視では図１８のように、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに設定した第２領域１００ｄ_０に対して、Ｘ方向にΔｘ_Item-、Δｘ_Item+、Ｙ方向にΔｙ_Item-、Δｙ_Item+だけマージンを広げることで、物体ＯＢＪの位置とサイズを考慮したＸＹ方向の第２領域１００ｄ_１が算出できる。同様に、把持した物体ＯＢＪを考慮した第２領域１００は、側面視では図１９のように、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに設定した第２領域１００ｄ_０に対して、上方に^TOTEＳＺ_Itemだけマージンを広げることで、物体ＯＢＪの位置とサイズを考慮したＺ方向の第２領域１００ｄ_１が算出できる。

上述した（式６）～（式９）を用いて設定した手先位置ＴＣＰが通過できない第２領域１００の位置姿勢を^TOTEＴ_Keepout,i（ｉ＝１，…，ｎ：ｎは物体ＯＢＪｓの数）、サイズを^TOTEＳ_Keepout,iとする。この場合、把持した物体ＯＢＪを考慮した、手先位置ＴＣＰが通過できない第２領域１００の位置姿勢^TOTEＴ_{KeepoutItem,i}および、サイズ^TOTEＳ_{KeepoutItem,i}は以下の（式１０）、（式１１）で算出される。

ただし、Ｘ方向のΔｘ_Item-、Δｘ_Item+、Ｙ方向のΔｙ_Item-、Δｙ_Item+は、物体ＯＢＪの側面の手先位置ＴＣＰからの変位量を示し、配置位置における手先位置ＴＣＰの位置姿勢を^ＴＯＴＥＴ_RP、物体ＯＢＪの位置姿勢を^TOTEＴ_Item、サイズを^TOTEＳ_Itemとすると、以下の（式１２）で算出される。

続いて、把持した物体ＯＢＪのサイズやハンド２２のサイズを考慮せずに設定した第２領域１００に対して、物体ＯＢＪを把持するハンド２２の位置とサイズを考慮した第２領域１００を算出する。

まず、図２０、図２１に示すように、ハンド２２を手先位置ＴＣＰから当該ハンド２２の端面までのＸ方向、Ｙ方向のそれぞれの距離を、Δｘ_{ｈａｎｄ－}、Δｘ_{ｈａｎｄ＋}、Δｙ_{ｈａｎｄ－}、Δｙ_{ｈａｎｄ＋}とする。

ここで、ハンド２２の場合も物体ＯＢＪと同様に、鉛直方向の干渉ではハンド２２底面との干渉が問題になる。一方、水平方向の干渉においては、ハンド２２が干渉しなければ、側面は干渉しないと見なせる。つまり、ハンド２２の底面の位置とサイズのみを考慮して第２領域１００を設定すればよいことになる。したがって、ハンド２２を考慮した第２領域１００は、図２２、図２３に示すように、物体ＯＢＪの位置とサイズを考慮した第２領域１００ｄ_０に対して考慮値として、Ｘ方向にΔｘ_{ｈａｎｄ－}、Δｘ_{ｈａｎｄ＋}、Ｙ方向にΔｙ_{ｈａｎｄ－}、Δｙ_{ｈａｎｄ＋}だけマージンを広げることで、ハンド２２の位置とサイズを考慮したＸＹ方向の第２領域１００（第２領域１００ｄ_２）が算出できる。

上述した（式６）～（式９）を用いて設定した手先位置ＴＣＰが通過できない第２領域１００の位置姿勢を^TOTEＴ_Keepout,i（ｉ＝１，…，ｎ：ｎは物体ＯＢＪｓの数）、サイズを^TOTEＳ_Keepout,iとする。この場合、ハンド２２を考慮した手先位置ＴＣＰが通過できない第２領域１００の位置姿勢^TOTEＴ_{KeepoutHand,i}および、サイズ^TOTEＳ_{KeepoutHand,i}は以下の（式１３）、（式１４）で算出される。

ただし、Ｘ方向のΔｘ_Hand-、Δｘ_Hand+、Ｙ方向のΔｙ_Hand-、Δｙ_Hand+は、移動先の容器１４ｂの座標系と直角平行なハンド２２の外接直方体側面の手先位置ＴＣＰからの位置を示し、配置位置における手先位置ＴＣＰの位置姿勢を^TOTEＴ_RP、ハンド２２の外接直方体の位置姿勢を^TOTEＴ_Ｈａｎｄ、サイズを^TOTEＳ_Ｈａｎｄとすると、以下の（式１５）で算出される。

そして、ハンド２２が把持する物体ＯＢＪを考慮して算出した第２領域１００（第２領域１００ｄ_１）とハンド２２を考慮して算出した第２領域１００（第２領域１００ｄ_２）を併せて、最終的な第２領域１００を決定（算出）することができる。

続いて、第３領域１０２のうち第２制御領域１０２ｂの設定について説明する。上述したように、第３領域１０２のうち第１制御領域１０２ａは上方に隣接する第２領域１００の存在により垂直方向（上方）から物体ＯＢＪやハンド２２が接近することはなく、水平方向（側方）からの接近に限られる。そして、第３領域１０２の第２制御領域１０２ｂにおいては、仮に障害物に対して物体ＯＢＪやハンド２２が干渉（接触）する場合でも、上述したような反発制御動作を力制御動作として実行することにより、物体ＯＢＪやハンド２２の損傷を回避可能としている。反発制御動作にあたっては、物体ＯＢＪやハンド２２に作用する力を正確に制御しつつ、急激に外力が作用しないように、力覚センサ３１の測定値をフィードバックしながら物体ＯＢＪやハンド２２を低速で接近させる必要がある。

一方、力制御のうちの特に第１制御領域１０２ａに適用される押当制御には、物体ＯＢＪの移動中に壁１４Ｗまたは既配置の物体ＯＢＪｓの側面に対して実施する第１押当制御と、移動先位置ＲＰに物体ＯＢＪを配置する場合に実施する第２押当制御とがある。第１押当制御は、上述したように主として狭い空間で移動経路を生成する場合に、第１制御領域１０２ａのマージンを実質的に「０」にして通過を可能にする制御である。したがって、第１押当制御は、壁１４Ｗの表面に対して物体ＯＢＪを略直交する方向に、例えば、所定閾値以下の押圧力で、所定閾値以下の移動速度を満たすような第１押当条件で押し当てる。なお、この場合の所定閾値以下の押圧力や所定閾値以下の移動速度は、物体ＯＢＪの種類（形状や材質、耐性等）に応じて、予め試験等により決定しておくことができる。

また、第２押当制御は、上述したように主として移動先位置ＲＰに物体ＯＢＪを最終的に配置する場合に、第１制御領域１０２ａのマージンを実質的に０にして、斜め上方からゆっくりと接近させる制御である。したがって、第２押当制御は、物体ＯＢＪｓの側面や壁１４Ｗの表面に対して、物体ＯＢＪを、例えば、所定の接近角度で、所定閾値以下の移動速度を満たすような第２押当条件で押し当てる。なお、この場合の所定接近角度や所定閾値以下の移動速度は、物体ＯＢＪの種類（形状や材質、耐性等）に応じて、予め試験等により決定しておくことができる。

上述したように、第１制御領域１０２ａの障害物とは反対側に側方向に隣接する領域を、第２制御領域１０２ｂとして設定する。第２制御領域１０２ｂは、力制御動作が行われる第３領域１０２の一部であり、力制御動作のうちの反発制御動作が実行され領域である。第２制御領域１０２ｂにおいては、物体ＯＢＪやハンド２２と障害物との干渉が検出された場合でも反発制御動作が行われることから、物体ＯＢＪやハンド２２の移動が許容される。また、第２制御領域１０２ｂにおいて、第１制御領域１０２ａで押当制御動作を実行するための準備として、少なくともハンド２２の挙動を制限するように設定してもよい。この場合のハンド２２の挙動の制限とは、例えば、移動速度の制限であり、第１領域１０４より低速（例えば、１／２や１／３の速度）とすることができる。

力制御動作のうち反発制御動作を実行する場合も物体ＯＢＪやハンド２２を力覚センサ３１の測定値をフィードバックしながら物体ＯＢＪを把持したハンド２２が移動する。反発制御動作の場合、所定閾値以下の移動速度で、壁１４Ｗや物体ＯＢＪの側面に接近し、実質的な反力が発生する前に、すなわち、接触を検出したとともに、接触面から離反するように制御される。例えば、計算上障害物と干渉しない移動経路が生成された場合でも、センサ３０の測定誤差に起因して、壁１４Ｗや物体ＯＢＪｓの側面に接触する場合がある。この場合、反発制御を実行することで、測定誤差を吸収し、壁１４Ｗや物体ＯＢＪｓの側面に衝突することによる破損を回避することができる。

ここで、図２４、図２５を用いて、移動先位置ＲＰである容器１４ｂに対して、第２領域１００、第３領域１０２の特に第２制御領域１０２ｂおよび第１領域１０４の設定、その際のハンド２２（手先位置ＴＣＰ）の移動経路Ｒの設定について一例を説明する。

図２４は、図１２～図２３で説明した例にしたがい、物体ＯＢＪｓが既に配置されている容器１４ｂに第２領域１００および第３領域１０２の第１制御領域１０２ａを設定した後、第２領域１００の下側で、かつ第１制御領域１０２ａの障害物と反対側の側方向に隣接する位置に第２制御領域１０２ｂを設定した例である。そして、第２領域１００、ならびに、第１制御領域１０２ａおよび第２制御領域１０２ｂからなる第３領域１０２以外の領域は、物体ＯＢＪを把持したハンド２２が制限されることなく比較的高速で移動可能な第１領域１０４として設定される。

図２４の場合、物体ＯＢＪｓに対する第２領域１００と容器１４ｂの壁１４Ｗに対する第２領域１００との間に第１領域１０４が存在し、物体ＯＢＪを把持したハンド２２が比較的自由に移動できる。図２４の場合、ハンド２２（手先位置ＴＣＰ）は、第１領域１０４を垂直下方に高速で、経由位置であるＲＡＰ１からＲＡＰ［０］に移動する。その後、手先位置ＴＣＰは、第３領域１０２の第２制御領域１０２ｂに進入する。第２制御領域１０２ｂでは、力覚センサ３１の測定値をフィードバックしながら物体ＯＢＪを把持したハンド２２が反発制御動作により所定閾値以下の移動速度で移動する。反発制御動作では、例えば、壁１４Ｗや物体ＯＢＪの側面に接近し、実質的な反力が発生する前に、接触を検出するとともに、物体ＯＢＪが接触面から離反するようにハンド２２が制御される。さらに、第２制御領域１０２ｂにおいて、物体ＯＢＪｓの側面に隣接して設定された第１制御領域１０２ａで、移動先位置ＲＰに向かう押当制御動作への切り替えをスムーズかつ遅延することなく実行できるように、第１領域１０４の場合より低速の準備速度でＲＡＰ［１］に移動させるように構成してもよい。そして、ＲＡＰ［１］から移動先位置ＲＰの間の第１制御領域１０２ａでは、第２押当条件を満たすように第２押当制御が実行される。このように、第１領域１０４、第２領域１００、第３領域１０２（第１制御領域１０２ａおよび第２制御領域１０２ｂ）の設定位置に基づいて、移動経路Ｒを決定するとともに、移動態様（移動速度等）を決定することにより、物体ＯＢＪを迅速かつ正確に、そしてダメージを与えにくい動作で移動先位置ＲＰに移動（配置）することができる。

図２５の場合、物体ＯＢＪｓに対する第２領域１００と容器１４ｂの壁１４Ｗに対する第１制御領域１０２ａとが接近し、間に第１領域１０４が存在しない例である。図２５の場合、ハンド２２（手先位置ＴＣＰ）は、第１領域１０４を垂直下方に高速で、ＲＡＰ１からＲＡＰ［０］に移動する。その後、手先位置ＴＣＰは、第２制御領域１０２ｂに進入し、ＲＡＰ［１］、ＲＡＰ［２］、ＲＡＰ［３］まで第１領域１０４の場合より低速の準備速度で進む。そして、ＲＡＰ［３］から移動先位置ＲＰの間で第２押当条件を満たすように第２押当制御を実行する。この場合も、第１領域１０４、第２領域１００、第２制御領域１０２ｂの設定位置に基づいて、移動経路Ｒを決定するとともに、移動態様（移動速度等）を決定する。その結果、図２４の場合より、第２制御領域１０２ｂを通過する期間が長いため時間はかかるが、物体ＯＢＪを最大限迅速かつ正確に、そしてダメージを与えにくい動作で移動先位置ＲＰに移動（配置）することができる。

ところで、上述した図２４、図２５で示したように移動経路Ｒを設定する場合、移動経路が第２領域１００と何処で干渉（接触）するかを正確に把握する必要がある。そこで、経路算出部５６は、容器１４ｂに第２領域１００を設定した場合、干渉チェック高さ（Ｚ方向）のリストを生成する。干渉チェック高さは、図２６に示すように、第２領域１００の上面と下面の高さで定義することができる。図２６は、容器１４ｂに既配置の物体ＯＢＪｓが存在し、図１２～図２３で説明した例にしたがい第２領域１００および第３領域１０２の第１制御領域１０２ａが設定されている場合の干渉チェック高さの一例を示している。

具体的には、手先位置ＴＣＰが通過できない第２領域１００の位置姿勢を^TOTEＴ_Keepout,i（ｉ＝１…，ｎ）、サイズを^TOTEＳ_Keepout,iとすると、干渉チェック高さのリスト^TOTEＺ_Check,j（ｊ＝１…，２ｎ）は以下の（式１６）で算出できる。

なお、干渉チェック高さのリスト^TOTEＺ_Check,jは、高さの低い順(昇順)にＨ１～Ｈ４等のように並んでいるものとする。

次に、通過が制限される第２領域１００を回避する手先位置ＴＣＰの算出について説明する。なお、物体ＯＢＪを把持したハンド２２は、例えば容器１４ｂの上方から容器１４ｂの中に進入する移動経路Ｒを辿るが、移動経路Ｒの計画時は、移動先位置ＲＰから進入元方向に逆算して移動経路Ｒの経由点を決定する。図２７は、容器１４ｂの壁１４Ｗに密着した既配置の物体ＯＢＪｓの右側面に、ハンド２２で把持した物体ＯＢＪを密着配置する例である。本実施形態の場合、経由点算出のルーチンの初回では、移動先位置ＲＰから斜め上方向にオフセットした位置をＲＡＰ［０］を設定する。前述したように、物体ＯＢＪを既配置の物体ＯＢＪｓや壁１４Ｗに密着配置させる場合、第２押当制御を実行する。その場合、例えば、物体ＯＢＪを斜め上方から接近させるとスムーズに密着させることができる。したがって、予め試験等によりオフセット量を決定しておく。例えば、高さ２０ｍｍから４５°で接近できるように、オフセット量、つまり、ＲＡＰ［０］を設定することができる。

次に、最初の経由位置（姿勢）であるＲＡＰ［０］に基づき、次の経由点を決定する。ここで、経路算出部５６は、図２６で説明した各干渉チェック高さにおいて第２領域１００を抽出し、ＲＡＰ［０］から真上に向かう経路の一部が第２領域１００に含まれることが分かった場合、第２領域１００を回避するような経由点を算出する。回避経由点は第２領域１００に含まれない領域の中で、ＲＡＰ［０］に最も近い点とする。回避経由点の算出例を図２８に示す。

まず、第２領域１００の各頂点のうち、他の第２領域１００に含まれない点を回避位置候補点Ｐｓとする（図２８の場合、第２領域１００の四隅の点）。ここで、手先位置ＴＣＰが通過できない第２領域１００の位置姿勢を^TOTEＴ_Keepout,i（ｉ＝１…，ｎ）、サイズを^TOTEＳ_Keepout,iとすると、回避位置候補点に追加する第２領域１００の各頂点^TOTEｔ_{AvoidCorner,ｉ,j}（ｊ＝１，２，３，４）は以下の（式１７）で算出できる。

続いて、回避前位置（図２８の^TOTEｔ_RAP[０]）からＸ方向、Ｙ方向に伸ばした直線と第２領域１００の各辺との交点のうち、他の第２領域１００に含まれない点を回避位置候補点Ｐｓ（^TOTEｔ_{AvoidParallel,ｉ,j}（ｊ＝１，２，３，４））として追加する。追加する^TOTEｔ_{AvoidParallel,ｉ,j}は、以下の（式１８）で算出できる。

そして、（式１７）で算出した^TOTEｔ_{AvoidCorner,ｉ,j}と、（式１８）で算出した
^TOTEｔ_{AvoidParallel,ｉ,j}をあわせて回避位置候補点リスト^TOTEｔ_AvoidAll,iとする。この候補点リストの中でＲＡＰ［０］の位置（回避前位置）^TOTEｔ_RAP[０]と距離が最も近い点を回避位置^TOTEｔ_Avoidとする。回避位置^TOTEｔ_Avoidは、以下の（式１９）で算出することができる。

続いて、経路算出部５６は、図２８で算出した^TOTEｔ_Avoidと図２７で設定したＲＡＰ［０］を結ぶ直線の移動経路が、第２領域１００を通過しないかをチェックする。そして、通過する場合は、その第２領域１００について、再度、図２８で説明した手順で回避位置を算出する。具体的な処理の例を図２９～図３１に示す。

例えば、図２９では、前回のＲＡＰ［０］＝点Ｐ１から直線を延ばし第２領域１００との交点を新たなＲＡＰ［０］＝点Ｐ２とする。そして、点Ｐ２から第２領域１００を回避するように上方に直線を延ばすと、上側の第２領域１００底面の点Ｐｂで交差する。この点Ｐｂを回避前位置とする場合、第２領域１００との交差を回避する回避位置として第２領域１００の端部の点Ｐｃが考えられる。ただし、この場合、点Ｐ２と点Ｐｃを直線で結ぶと、下側の第２領域１００を通過することになる。そこで、図３０に示すように、点Ｐ２と点Ｐｃを結ぶ直線が下側の第２領域１００の上面と交差する点Ｐｂを、新たな回避前位置とする。この場合、第２領域１００との交差を回避する回避位置として第２領域１００の端部の点Ｐ３が考えられる。したがって、回避経路として、点Ｐ２と点Ｐ３を結ぶ。そして、図３１に示すように、前回、上側の第２領域１００底面において回避位置とした点Ｐｃを点Ｐ４と改め、点Ｐ３と点Ｐ４を結ぶ。なお、図３１の場合、点Ｐ４から上方に直線を延ばしても第２領域１００に交差することはない。したがって、この時点で、回避経路が完成する。つまり、点Ｐ１，Ｐ２，Ｐ３，Ｐ４を結ぶ移動経路が完成する。

以上に示した処理をより具体的に示す。
手先位置ＴＣＰが通過できない第２領域１００の位置姿勢を^TOTEＴ_Keepout,i（ｉ＝１…，ｎ）、サイズを^TOTEＳ_keepout,iとすると、ＲＡＰ［０］の位置^TOTEｔ_RAP[０]と回避位置^TOTEｔ_Avoidとを結ぶ線分Ｌ_RAP[0],Aviodが各第２領域１００の底面および上面と交点を持つかどうかを判定すればよい。各第２領域１００の底面および上面のＺ座標^TOTEＺ_KeepoutTop,iおよび^TOTEＺ_{KeepoutBottom,i}は、（式２０）で算出できる。

そして、線分Ｌ_RAP[0],Aviodが^TOTEＺ_KeepoutTop,i、^TOTEＺ_{KeepoutBottom,i}と交差するには，少なくとも以下の（式２１）の条件を満たす必要がある。

このとき、線分Ｌ_RAP[0],Aviodの^TOTEＺ_KeepoutTop,i、^TOTEＺ_{KeepoutBottom,i}における座標^TOTEｔ_CrossTop,i、^TOTEｔ_{CrossBottom,i}は、以下の（式２２）で算出できる。

こららの交点^TOTEｔ_CrossTop,i、^TOTEＺ_{CrossBottom,i}が第２領域１００内に含まれるのは，以下の（式２３）で示す条件を満たしているときである。

線分Ｌ_RAP[0],Aviodが第２領域１００と交差することが分かった場合、交差すると判定された第２領域１００の上面または底面の高さ^TOTEＺ_KeepoutTop,i、^TOTEＺ_{KeepoutBottom,i}のうち、最小の値となる高さにおける線分Ｌ_{RAP[０]，Aviod}の座標を新たに回避前位置とし、図２８で説明した手順により回避位置を算出する。

ところで、上述した手順で移動経路を算出する場合、移動先位置ＲＰまでのスペースが狭い場合、センサ３０（カメラ３２ｂ、測域センサ３３ａ，３３ｂ等）の認識誤差を考慮したマージンでは経由点が生成できない場合がある。その場合、本実施形態のでは、図３２、図３３に示すように、力制御により物体ＯＢＪを移動先の容器１４ｂの壁１４Ｗまたは既配置の物体ＯＢＪｓの側面に押し当てることで認識誤差を吸収し、少なくとも一部においてマージンなしで移動先位置ＲＰまでの移動経路を算出する。図３２、図３３の場合、既配置の物体ＯＢＪｓに設定された第２領域１００が、壁１４Ｗに設定された第３領域１０２（第１制御領域１０２ａ）と一部が重なって重複領域となっている。その結果、図３２に示すように、物体ＯＢＪを把持したハンド２２は、移動先位置ＲＰに接近できない。この場合、経路算出部５６は、第１制御領域１０２ａを優先し、この重複領域について第１制御領域１０２ａとして扱う。すなわち、図３３に示すように、力制御により物体ＯＢＪを移動先の容器１４ｂの壁１４Ｗに矢印Ｄ方向（壁１４Ｗに対して略直交する方向）に押し当る第１押当制御を実行し、第２領域１００（第１制御領域１０２ａ）を通過して移動経路Ｒを生成する。この場合、第２領域１００（第１制御領域１０２ａ）で力制御動作を実行して移動を許容するために少なくともハンド２２は、第３領域１０２の第１制御領域１０２ａを通過する際に第１押当条件を満たすような挙動（例えば、移動速度が閾値以下に制限された移動や所定閾値以下の押圧力等）で第１押当制御を実行する。その結果、押当制御動作に起因した外力が必要以上に物体ＯＢＪ等にかかることを回避することができる。特に、押当制御動作を行なう領域である第１制御領域１０２ａを、第２領域と同様にハンド２２の通過を原則的に制限（禁止）し、例外的に押当制御動作を行なう場合に限り物体ＯＢＪおよびハンド２２の通過を可能とする領域に設定している場合を考える。この場合、第２領域１００と第１制御領域１０２ａとの重複領域が生じるときにのみ、この重複領域については第１制御領域１０２ａを優先させて、第１制御領域１０２ａとしてハンド２２の押当制御動作による経路を生成するようにしてもよい。

図３４には、第３領域１０２のうち第１制御領域１０２ａで押当制御動作を実行する場合の押当方向を決定する手順例が示されている。まず、経路算出部５６は、Ｘ方向のマージンを無視した通過制限領域を生成する（Ｓ２００）。そして、Ｘ方向のマージンを無視したことにより回避位置が算出できた場合（Ｓ２０２のＹｅｓ）、押当方向として、Ｘ方向を決定し（Ｓ２０４）、一旦フローを終了する。Ｓ２０２において、回避位置が算出できない場合（Ｓ２０２のＮｏ）、経路算出部５６は、Ｙ方向のマージンを無視した通過制限領域を生成する（Ｓ２０６）。そして、Ｙ方向のマージンを無視したことにより回避位置が算出できた場合（Ｓ２０８のＹｅｓ）、押当方向として、Ｙ方向を決定し（Ｓ２１０）、一旦フローを終了する。

Ｓ２０８において、回避位置が算出できない場合（Ｓ２０８のＮｏ）、経路算出部５６は、Ｘ，Ｙ方向のマージンを無視した通過制限領域を生成する（Ｓ２１２）。そして、Ｘ，Ｙ方向のマージンを無視したことにより回避位置が算出できた場合（Ｓ２１４のＹｅｓ）、押当方向として、Ｘ，Ｙ方向を決定し（Ｓ２１６）、一旦フローを終了する。Ｓ２１４において、回避位置が算出できない場合（Ｓ２１４のＮｏ）、経路算出部５６は、物体ＯＢＪを配置しようとした移動先位置ＲＰを、物体ＯＢＪのリリース候補から除外する。この場合、他の位置に移動先位置ＲＰを設定し物体ＯＢＪを移動させるように再計算を行う。

なお、Ｘ方向またはＹ方向のマージンを無視した通過制限領域（第２領域１００）を生成するには、図１２～図２３で説明した第２領域１００の設定の際に、δ_ｘ＝σ_ｘ＝０またはδ_ｙ＝σ_ｙ＝０とすればよい。Ｘ方向にのみ押し当てる場合を例とすると、δ_ｘ＝σ_ｘ＝０として生成した通過制限領域（第２領域１００）の位置姿勢を^TOTEＴ_{KeepoutPressX,i}（ｉ＝１，…，ｎ）、サイズを^TOTEＳ_{KeepoutPressX,i}とする。図３５（ａ）～図３５（ｃ）を用いて、押当制御による経由点の生成について、具体的な処理の流れを示す。

まず、図３５（ａ）に示すように、回避前位置である点Ｐｂの手先位置ＴＣＰ高さにおいて、押し当て可能な位置ＰＰを探索する。押当面を探索するため、図１２～図２３で説明した第２領域１００の設定の際に、全方向へのマージンを「０」（δ_ｘ＝σ_ｘ＝δ_ｙ＝σ_ｙ＝δ_ｚ＝σ_ｚ＝０）に設定する。マージンを無視することで，ハンド２２または把持した物体ＯＢＪと、既配置の物体ＯＢＪｓまたは移動先の容器１４ｂの壁１４Ｗとが実際に接触する領域を生成することができる。このようにして生成した領域を接触領域と呼ぶ。このときの位置姿勢を^TOTEＴ_Contact,i（ｉ＝１，…，ｎ）、サイズを^TOTEＳ_Contact,iとする。Ｘ方向にのみ押し当てる場合、接触領域の各面のうちＸ軸に垂直な面のＸ座標^TOTEＴ_ContactX,j（ｊ＝１，…，２ｎ）は以下の（式２４）で算出できる。

この中から、回避前位置Ｐｂ（＝^TOTEｔ_RAP[０]）とＹ，Ｚ座標が等しくなる領域に含まれている面のＸ座標を抽出し、^TOTEｔ_RAP[０]のＹ，Ｚ座標とあわせて押し当て可能位置ＰＰ（＝^TOTEｔ_PressX,i）とする。

続いて、図３５（ｂ）に示すように、回避前位置Ｐｂの手先位置ＴＣＰ高さの次に高い干渉チェック高さにおいて、押し当て可能な位置ＰＰを探索する。図２６に示したように、干渉チェック高さのリストは高さの低い順に並んでいることから、回避前位置Ｐｂを計算した干渉チェック高さを^TOTEＺ_Check,iとすると、次に高い干渉チェック高さは^TOTEＺ_Check,i+1である。この位置を基準として、図３５（ａ）と同様の方法で押当可能位置ＰＰ（＝^TOTEｔ_PressX,i）に追加する。

図３５（ａ）、図３５（ｂ）で探索した押当可能位置ＰＰ＝^TOTEｔ_PressX,iの中で，Ｘ，Ｙ座標が一致し、Ｚ座標のみが異なる２点のセットを抽出し、押当面ＰＴとする。なお、押当面ＰＴが複数抽出された場合は、その中から、回避前位置Ｐｂ（＝^TOTEｔ_RAP[０]）に最も距離が近い面を押当面ＰＴとして選択する。そして、選択された押当面ＰＴの上端にあたる位置を^TOTEｔ_PressX＋、下端に当たる位置を^TOTEｔ_PressX－とする。

次に、図３５（ｃ）に示すように、選択した押当面ＰＴにあわせて押当待機位置Ｐｒ、押当開始位置Ｐstart、押当終了位置Ｐendを設定し、これらを移動経路Ｒの経由点に追加する。

押当開始位置Ｐstartは、押し当てによる力目標値が満たされるまで押し当てを継続する位置で、具体的には、図３５（ｂ）で算出した押当面の上端^TOTEｔ_PressX＋とすることができる。また、押当終了位置Ｐendは、押当開始位置Ｐstartから押し当てを継続したまま移動する目標位置で、具体的には、図３５（ｂ）で算出した押当面の下端^TOTEｔ_PressX－とすることができる。

また、押当待機位置Ｐｒは、押当開始位置Ｐstartに向かうためのマージンを確保した位置である。押当待機位置ＰｒのＸ方向については、押当開始位置Ｐstartから回避前位置Ｐｂ（＝^TOTEｔ_RAP[０]）に向かう方向、Ｚ方向については、鉛直上方向にマージン分オフセットした位置とすることができる。ここで、マージン設定値を^TOTEＸ_PressMargin（Ｘ方向、正の値）、^TOTEＺ_PressMargin（Ｚ方向、正の値）とすると、押当待機位置Ｐｒ=^TOTEｔ_PressXAppは、（式２５）で算出することができる。

なお、物体ＯＢＪの押当面に対して、押当面ＰＴの広さが所定閾値以下の場合、つまり、物体ＯＢＪの物体側押当面と障害物の障害物側押当面との接触面積が接触閾値未満の場合、押当制御（力制御動作）を非実行とする。この場合、押当を行った場合、接触面の不足により、ハンド２２が把持する物体ＯＢＪの把持姿勢が変化したり、物体ＯＢＪに外力が加わり破損や変形を起こしたりする可能性がある。したがって、このような場合、無理に押当制御を実行することなく、経路算出部５６は、移動先位置ＲＰを変更して移動経路Ｒの再算出を実行する。

以上のように構成される物体ハンドリングシステム１（物体ハンドリング装置）において、領域設定部５５が領域設定を行った場合の物体ＯＪＢ（ハンド２２）の移動経路の経由点の生成例を図３６のフローチャートを用いて説明する。また、図３７（ａ）～図３７（ｆ）は、移動経路の経由点の生成において、経由点の位置を説明する例示的かつ模式的な説明図である。なお、図３７では、既に説明した図面を統合し、経由点の生成が理解し易いよう記載している。また、図３７では、説明の簡略化のため、第２領域１００および第３領域１０２の第１制御領域１０２ａのみを図示している。

まず、領域設定部５５は、手先位置ＴＣＰの通過制限領域、すなわち、第２領域１００を算出する（Ｓ３００）。例えば、図３７（ａ）に示すように、領域設定部５５は、容器１４ｂの形状や壁１４Ｗの位置、容器１４ｂに既配置の物体ＯＢＪｓの位置、サイズ、大きさ等を、例えば画像処理部５２が取得したカメラ３２ｂが撮像した画像に基づく状態情報に基づいて取得する。そして、領域設定部５５は、図１２～図２３を用いて説明した手順にしたがい第２領域１００および第３領域１０２の第１制御領域１０２ａを算出する。なお、第２領域１００の算出（設定）により、第３領域１０２（第１制御領域１０２ａおよび第２制御領域１０２ｂ）、第１領域１０４の算出（設定）も実行される。

続いて、図３７（ｂ）において、図２７で説明したように、移動先位置ＲＰから斜め上方にオフセットした位置に、移動経路Ｒの生成時の最初の経由点であるＲＡＰ［０］を設定する（Ｓ３０２）。さらに、経路算出部５６は、図２６で説明したように、干渉チェック高さのリストを算出する（Ｓ３０４）。

そして、順次、経由点を設定するように、以下のステップを繰り返し実行する。すなわち、図２７～図３５を用いて説明した手順にしたがい、経由点を順次決定して行く。まず、図３７（ｃ）に示すように、経路算出部５６は、最初に設定した経由点であるＲＡＰ［０］から真上に向かう経路（直線）を算出する（Ｓ３０６）、そして、その経路（直線）の一部が通過制限領域（第２領域１００）に含まれるか否か判定する（Ｓ３０８）。真上に向かう経路（直線）の一部が通過制限領域（第２領域１００）に含まれる場合（Ｓ３０８のＹｅｓ）、図３７（ｄ）に示すように、通過制限領域（第２領域１００）を回避するための経由点を、図２８で説明した手順にしたがい算出する（Ｓ３１０）。なお、図３７（ｄ）は、さらに次の経由点が算出されている状態が示されている。

新たな経由点が算出できない場合（Ｓ３１２のＮｏ）、図３２、図３３で説明したように、押当制御による回避経由点を算出する（Ｓ３１４）。また、Ｓ３１２で経由点が算出できた場合（Ｓ３１２のＹｅｓ）、またはＳ３１４で回避経由点が算出できた場合、図３７（ｄ）に示すように、算出した経由点をＲＡＰ［ ］に先頭から順に追加する（Ｓ３１６）。例えば、算出された最新の経由点が、ＲＡＰ［０］になるように、ナンバリングを行う。そして、経路算出部５６は、最新の経由点を含んで算出した移動経路Ｒが第２領域１００と干渉しないか干渉チェックを行い（Ｓ３１８）、次の経由点を探索するために、Ｓ３０６に戻る。なお、図３７（ｅ）は、次の経由点の探索処理のときに、最新の経由点ＲＡＰ［０］から真上に向かう経路を算出した結果、第２領域１００に干渉する例で、回避経由点を算出する必要がある場合である。

図３７（ｆ）では、最新の回避経由点ＲＡＰ［０］が算出された状態が示されている。さらに、次の経由点を算出するために、Ｓ３０６からの処理を繰り返した結果、Ｓ３０８で、経路算出部５６は、真上に向かう経路（直線）の一部が通過制限領域（第２領域１００）に含まれないと判定された場合（Ｓ３０８のＮｏ）、このフローを一旦終了する。つまり、移動経路Ｒを作成するにあたり、干渉する第２領域１００は既にないことにある。この場合、図３７（ｆ）に示すように、例えば、既配置の物体ＯＢＪｓの下面縁部を移動先位置ＲＰとして、図３７（ｂ）で最初に設定した経由点がＲＡＰ［３］になり、次に算出された経由点がＲＡＰ［２］になり、さらに次に算出された経由点がＲＡＰ［１］になり、最新の経由点がＲＡＰ［０］となる。つまり、容器１４ｂの上空から接近する物体ＯＢＪを把持したハンド２２は、作成された移動経路Ｒに沿って、経由点ＲＡＰ［０］、ＲＡＰ［１］、ＲＡＰ［２］、ＲＡＰ［３］を順に移動することにより、移動先位置ＲＰに障害物と干渉することなく迅速に到達し、物体ＯＢＪを配置することができる。

なお、図３６のフローチャートでは、省略しているが、Ｓ３１４で、押当制御によっても回避経由点が算出できない場合、最初に設定した移動先位置ＲＰに物体ＯＢＪを移動（配置）することができないと判定され、経路算出部５６は、別の移動先位置ＲＰについて、このフローチャートを適用して、移動経路Ｒの生成を行う。

なお、上述した例では、物体ＯＢＪｓの上面に第２領域１００を設定する例を示したが、積上げ配置可能な場合は、既配置の物体ＯＢＪｓの上面は、第２領域１００以外の領域として、積上げ配置が可能な移動経路Ｒを生成できるようにしてもよく、移動先位置ＲＰにおける配置条件に従って、設定する領域を適宜変更してもよい。

このように、本実施形態の物体ハンドリングシステム１（物体ハンドリング装置）は、ハンド２２、画像処理部５２，信号処理部５３等の情報取得部と、領域設定部５５と、経路算出部５６と、ロボット制御部５７（移動制御部）と、を備える。ハンド２２は、物体ＯＢＪを把持可能である。画像処理部５２、信号処理部５３等の情報取得部は、ハンド２２に把持された状態の物体ＯＢＪに関する物体情報と、物体ＯＢＪの移動元位置ＨＰと物体ＯＢＪの移動先位置ＲＰに関する状態情報と、を少なくとも取得する。領域設定部５５は、第１領域１０４と、第２領域１００と、第３領域１０２とを物体情報と状態情報に応じて設定する。第１領域１０４は、物体ＯＢＪを把持したハンド２２が移動元位置ＨＰから移動先位置ＲＰに移動する場合に、移動元位置ＨＰと移動先位置ＲＰとの間の空間に存在する障害物（容器１４ｂや既配置の物体ＯＢＪｓ等）に制限されることなく移動が許可される領域である。第２領域１００は、障害物の存在に基づいて移動が制限される領域である。第３領域１０２は、少なくとも一部が第２領域１００の下方に設定されハンド２２に力制御動作を実行させる領域である。経路算出部５６は、第１領域１０４と第２領域１００と第３領域１０２とに基づき物体ＯＢＪを移動元位置ＨＰから移動先位置ＲＰに移動させる移動経路Ｒを算出する。ロボット制御部５７は、マニピュレータ２０を制御して、移動経路Ｒにしたがい物体ＯＢＪを把持したハンド２２を移動させる。

この構成によれば、ハンド２２やハンド２２が把持した物体ＯＢＪの破損を招くよう障害物との衝突を回避しつつ、狭い空間にも移動経路Ｒが生成可能で、物体ＯＢＪを効率的かつ安全に移動させることができる。つまり、物体ＯＢＪの容器１４ｂへの充填率を容易に最大化して運搬コストの低減ができる。また、第１領域１０４を利用した高速移動および第３領域１０２を利用して第２領域１００と干渉しない安全な移動が実現され、物体ＯＢＪの移動（配置）効率（移載サイクル）の向上ができる。

また、物体ハンドリングシステム１（物体ハンドリング装置）の領域設定部５５は、第２領域１００として、ハンド２２が障害物の上方向から下方向への移動成分をもって接近することを制限する第２領域１００と、第２領域１００の下側で障害物に沿って設定されるとともにハンド２２が障害物の側方向から接近することを制限する第２領域１００より障害物に対する制限幅（マージン）が少ない第１制御領域１０２ａと、を設定するようにしてもよい。この構成によれば、物体ＯＢＪやハンド２２の動作に応じた第２領域１００の設定を容易に行うことができる。

また、物体ハンドリングシステム１（物体ハンドリング装置）の領域設定部５５は、第１制御領域１０２ａの障害物の反対側に隣接して第２制御領域１０２ｂを設定するようにしてもよい。この構成によれば、第３領域１０２の第１制御領域１０２ａで押当制御動作を適切に行うための第２制御領域１０２ｂを容易に設定することができる。

また、物体ハンドリングシステム１（物体ハンドリング装置）における、力制御動作は、経路算出部５６が移動経路Ｒを算出する場合で第１制御領域１０２ａの通過が必要な場合に、障害物の表面に対して物体ＯＢＪを略直交する方向に第１押当条件を満たすように押し当てる第１押当制御により実行されるようにしてもよい。この場合、第２領域１００の存在により移動経路が生成できない場合でも、物体ＯＢＪの損傷しつつ移動させることができる移動経路Ｒの生成がし易くなる。

また、物体ハンドリングシステム１（物体ハンドリング装置）における力制御動作は、経路算出部５６が移動経路Ｒを算出する場合で物体ＯＢＪを移動先位置ＲＰを含む載置領域に着地させる場合に、載置領域の縁部に向かって第１制御領域１０２ａの中を物体ＯＢＪが第２押当条件を満たすように押し当てる第２押当制御により実行されるようにしてもよい。この構成によれば、容器１４ｂの壁１４Ｗや既配置の物体ＯＢＪｓに物体ＯＢＪを密着させた状態で配置することができる。また、その際に物体ＯＢＪやハンド２２にダメージが加わることを回避することができる。

また、物体ハンドリングシステム１（物体ハンドリング装置）における力制御動作は、移動経路Ｒに沿って物体ＯＢＪを把持したハンド２２が特に第２制御領域１０２ｂ内を移動する際に障害物に接触する場合に、物体ＯＢＪを障害物の表面から離反する方向に反発移動させて通過を可能にする反発制御により実行されるようにしてもよい。この場合、センサ３０等に測定誤差が存在する場合でも、測定誤差を吸収し、物体ＯＢＪやハンド２２が壁１４Ｗや物体ＯＢＪｓの側面に衝突することが回避可能となり、破損の回避が容易にできる。

なお、実施形態においては、第３領域１０２を第１制御領域１０２ａと第２制御領域１０２ｂに分けて設定する例を示しているが、第３領域１０２を第１制御領域１０２ａと第２制御領域１０２ｂの２つの領域に分割せず一つの領域として設定してもよい。この場合、力制御動作が行われる領域である第３領域１０２においては、力制御のうち押当制御および反発制御のいずれか一方が行われる領域として設定できる。または、第３領域１０２全体を、例えば力覚センサ３１の検出値に応じて押当制御動作および反発制御動作を切り替えるなど、押当制御および反発制御を組み合わせた力制御を行なう領域として設定してもよい。

また、物体ハンドリングシステム１（物体ハンドリング装置）の経路算出部５６は、第１押当制御を実行する場合に、物体ＯＢＪの物体側押当面と障害物の障害物側押当面との接触面積が接触閾値未満の場合、力制御動作を非実行として、移動先位置ＲＰを変更して移動経路Ｒの再算出を実行してもよい。この構成によれば、接触面の不足により、ハンド２２が把持する物体ＯＢＪの把持姿勢が変化したり、物体ＯＢＪに外力が加わり破損や変形を起こしたりする可能性がある。したがって、このような場合、無理に押当制御を実行することなく、経路算出部５６は、移動先位置ＲＰを変更して移動経路Ｒの再算出を実行する。

また、本実施形態の物体ハンドリング方法は、ハンド２２に把持された状態の物体ＯＢＪに関する物体情報と、物体ＯＢＪの移動元位置ＨＰと物体ＯＢＪの移動先位置ＲＰに関する状態情報と、を少なくとも取得する情報取得ステップを備える。また、物体ＯＢＪを把持したハンド２２が移動元位置ＨＰから移動先位置ＲＰに移動する場合に、移動元位置ＨＰと移動先位置ＲＰとの間の空間に存在する障害物に制限されることなく移動が許可される第１領域１０４と、障害物の存在に基づいて移動が制限される第２領域１００と、ハンド２２に力制御動作を実行させる第３領域１０２と、を物体情報と状態情報に応じて設定する領域設定ステップを備える。また、第１領域１０４と第２領域１００と第３領域１０２とに基づき物体ＯＢＪを移動元位置ＨＰから移動先位置ＲＰに移動させる移動経路Ｒを算出する経路算出ステップを含む。そして、移動経路Ｒにしたがい物体ＯＢＪを把持したハンド２２を移動させる移動制御ステップを含む。

この構成によれば、ハンド２２やハンド２２が把持した物体ＯＢＪの破損を招くよう障害物との衝突を回避しつつ、狭い空間にも移動経路Ｒが生成可能で、物体ＯＢＪを効率的かつ安全に移動させることができる。つまり、物体ＯＢＪの容器１４ｂへの充填率を容易に最大化して運搬コストの低減ができる。また、第１領域１０４を利用した高速移動および第３領域１０２を利用して第２領域１００と干渉しない安全な移動が実現され、物体ＯＢＪの移動（配置）効率（移載サイクル）の向上ができる。

本実施形態の物体ハンドリング装置で実行されるハンドリングプログラムは、インストール可能な形式または実行可能な形式のファイルでＣＤ－ＲＯＭ、ＵＳＢメモリ、ＳＳＤ等の半導体メモリ装置、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）等のコンピュータで読み取り可能な記録媒体に記録されて提供されることができる。

また、本実施形態の物体ハンドリング装置で実行されるハンドリングプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成しても良い。また、本実施形態の物体ハンドリング装置で実行されるハンドリングプログラムをインターネット等のネットワーク経由で提供または配布するように構成しても良い。また、本実施形態の物体ハンドリング装置で実行されるハンドリングプログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１…物体ハンドリングシステム、１０…制御装置、１４Ｗ…壁、１４ａ，１４ｂ…容器、２２…ハンド、３１…力覚センサ、３２ａ，３２ｂ…カメラ、３３ａ，３３ｂ…測域センサ、５２…画像処理部（情報取得部）、５３…信号処理部（情報取得部）、５５…領域設定部、５６…経路算出部、５７…ロボット制御部（移動制御部）、１００…第２領域、１０２…第３領域、１０２ａ…第１制御領域、１０２ｂ…第２制御領域、１０４…第１領域、６１…内部ＤＢ、ＲＰ…移動先位置、ＴＣＰ…手先位置。

Claims (7)

1. 物体を把持可能な把持部に把持された状態の前記物体に関する物体情報と、前記物体の移動元位置と前記物体の移動先位置に関する状態情報と、を少なくとも取得する情報取得部と、
   前記物体を把持した前記把持部が前記移動元位置から前記移動先位置に移動する場合に、前記移動元位置と前記移動先位置との間の空間に存在する障害物に制限されることなく移動が許可される第１領域と、前記障害物の存在に基づいて移動が制限される第２領域と、少なくとも一部が前記第２領域の下方に設定され前記把持部に力制御として力制御動作を実行させる第３領域と、を前記物体情報と前記状態情報に応じて設定する領域設定部と、
   前記第１領域と前記第２領域と前記第３領域とに基づき前記物体を前記移動元位置から前記移動先位置に移動させる移動経路を算出する経路算出部と、
   を備える、物体ハンドリング制御装置。
2. 前記第２領域は、少なくとも前記把持部が前記障害物の上方向から下方向への移動成分をもって接近することが制限される領域である、請求項１に記載の物体ハンドリング制御装置。
3. 前記第３領域は、前記力制御として押当制御が実行される第１制御領域および前記力制御として反発制御が実行される第２制御領域を備える、請求項１または請求項２に記載の物体ハンドリング制御装置。
4. 前記押当制御は、前記障害物の表面に対して前記物体を略直交する方向に第１押当条件を満たすように押し当てて実行され、
   前記経路算出部は、前記押当制御を実行する場合に、前記物体の物体側押当面と前記障害物の障害物側押当面との接触面積が接触閾値未満の場合、前記移動先位置を変更して前記移動経路の再算出を実行する、請求項３に記載の物体ハンドリング制御装置。
5. 請求項１乃至請求項３のいずれか１項に記載の物体ハンドリング制御装置と、
   前記把持部と、
   前記経路算出部が算出した前記移動経路にしたがい前記物体を把持した前記把持部を移動させる移動制御部と、
   を備える物体ハンドリング装置。
6. 把持部に把持された状態の物体に関する物体情報と、前記物体の移動元位置と前記物体の移動先位置に関する状態情報と、を少なくとも取得する情報取得ステップと、
   前記物体を把持した前記把持部が前記移動元位置から前記移動先位置に移動する場合に、前記移動元位置と前記移動先位置との間の空間に存在する障害物に制限されることなく移動が許可される第１領域と、前記障害物の存在に基づいて移動が制限される第２領域と、少なくとも一部が前記第２領域の下方に設定され前記把持部に力制御として力制御動作を実行させる第３領域と、を前記物体情報と前記状態情報に応じて設定する領域設定ステップと、
   前記第１領域と前記第２領域と前記第３領域とに基づき前記物体を前記移動元位置から前記移動先位置に移動させる移動経路を算出する経路算出ステップと、
   前記移動経路にしたがい前記物体を把持した前記把持部を移動させる移動制御ステップと、
   を備える、物体ハンドリング方法。
7. コンピュータに、
   把持部に把持された状態の物体に関する物体情報と、前記物体の移動元位置と前記物体の移動先位置に関する状態情報と、を少なくとも取得する情報取得ステップと、
   前記物体を把持した前記把持部が前記移動元位置から前記移動先位置に移動する場合に、前記移動元位置と前記移動先位置との間の空間に存在する障害物に制限されることなく移動が許可される第１領域と、前記障害物の存在に基づいて移動が制限される第２領域と、少なくとも一部が前記第２領域の下方に設定され前記把持部に力制御として力制御動作を実行させる第３領域と、を前記物体情報と前記状態情報に応じて設定する領域設定ステップと、
   前記第１領域と前記第２領域と前記第３領域とに基づき前記物体を前記移動元位置から前記移動先位置に移動させる移動経路を算出する経路算出ステップと、
   前記移動経路にしたがい前記物体を把持した前記把持部を移動させる移動制御ステップと、
   を実行させるための、物体ハンドリングプログラム。

Images