JP7145700B2 - ハンド制御装置 - Google Patents

Description

実施形態は、ハンド制御装置に関する。

従来、物体を第一位置でハンドにより把持して第二位置へ搬送するマニピュレータを有したロボットが知られている。

特開２０１５－８５５０１号公報

例えば、物体をより確実に把持することができるなど、より不都合の少ない新規なハンド制御装置が得られれば、有益である。

実施形態のハンド制御装置は、計画生成部と、動作制御部と、を備える。計画生成部は、物体を把持するハンドの動作計画を生成する。動作制御部は、ハンドに作用する力またはモーメントに基づいてハンドの動作を制御する第一制御モードと、第一制御モードとは異なる制御モードと、を切り替えてハンドの動作を制御する。

図１は、実施形態の物体ハンドリングシステムの模式的かつ例示的な構成図である。 図２Ａは、実施形態のハンドの一例の模式的な側面図である。 図２Ｂは、実施形態のハンドの図２Ａとは別の一例の模式的な側面図である。 図２Ｃは、実施形態のハンドの図２Ａ，２Ｂとは別の一例の模式的な側面図である。 図２Ｄは、実施形態のハンドの図２Ａ，２Ｂ，２Ｃとは別の一例の模式的な側面図である。 図３Ａは、図２Ａのハンドが対象物を把持した状態を示す例示的かつ模式的な側面図である。 図３Ｂは、図２Ｂのハンドが対象物を把持した状態を示す例示的かつ模式的な側面図である。 図３Ｃは、図２Ｃのハンドが対象物を把持した状態を示す例示的かつ模式的な側面図である。 図３Ｄは、図２Ｄのハンドが対象物を把持した状態を示す例示的かつ模式的な側面図である。 図４は、実施形態のハンド制御装置の例示的なブロック図である。 図５は、実施形態のハンド制御装置により制御されたハンドが対象物を把持し、移動して、解放する手順を示す例示的なフローチャートである。 図６は、実施形態のハンド制御装置により制御されたハンドの移動経路を示す例示的かつ模式的な説明図である。 図７Ａは、実施形態のハンド制御装置の力制御による対象物の解放位置への移動の一例を示す模式的な斜視図である。 図７Ｂは、実施形態のハンド制御装置の力制御による対象物の解放位置への移動の別の一例を示す模式的な斜視図である。 図８は、実施形態のハンド制御装置に含まれる動作制御部のブロック図である。 図９は、実施形態のハンド制御装置に含まれる手先座標力制御部のブロック図である。 図１０は、実施形態のハンド制御装置に含まれる重力補償部のブロック図である。 図１１は、実施形態のハンド制御装置により制御されたハンドが対象物を把持する状態の一例を示す模式的な斜視図である。 図１２は、実施形態のハンド制御装置に含まれる動作制御部による把持動作時の制御手順を示す例示的なフローチャートである。 図１３Ａは、実施形態のハンドの把持動作における所定位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１３Ｂは、実施形態のハンドの把持動作における図１３Ａとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１３Ｃは、実施形態のハンドの把持動作における図１３Ａ，１３Ｂとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１４Ａは、実施形態のハンドの解放動作の一例を示す模式的かつ例示的な斜視図である。 図１４Ｂは、実施形態のハンドの解放動作の別の一例を示す模式的かつ例示的な斜視図である。 図１５は、実施形態のハンド制御装置に含まれる動作制御部による解放動作時の制御手順を示す例示的なフローチャートである。 図１６Ａは、実施形態のハンドの解放動作における所定位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｂは、実施形態のハンドの解放動作における図１６Ａとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｃは、実施形態のハンドの解放動作における図１６Ａ，１６Ｂとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｄは、実施形態のハンドの解放動作における図１６Ａ～１６Ｃとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｅは、実施形態のハンドの解放動作における図１６Ａ～１６Ｄとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｆは、実施形態のハンドの解放動作における図１６Ａ～１６Ｅとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｇは、実施形態のハンドの解放動作における図１６Ａ～１６Ｆとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１６Ｈは、実施形態のハンドの解放動作における図１６Ａ～１６Ｇとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１７Ａは、実施形態のハンドの別の解放動作における所定位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１７Ｂは、実施形態のハンドの別の解放動作における図１７Ａとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１７Ｃは、実施形態のハンドの別の解放動作における図１７Ａ，１７Ｂとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１７Ｄは、実施形態のハンドの別の解放動作における図１７Ａ～１７Ｃとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１８Ａは、実施形態のハンドのさらに別の解放動作における所定位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１８Ｂは、実施形態のハンドのさらに別の解放動作における図１８Ａとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１８Ｃは、実施形態のハンドのさらに別の解放動作における図１８Ａ，１８Ｂとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。 図１８Ｄは、実施形態のハンドのさらに別の解放動作における図１８Ａ～１８Ｃとは別の位置でのハンドの状態または移動経路を示す模式的かつ例示的な側面図である。

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

［システム全体構成］
図１は、物体ハンドリングシステム１の概略構成図である。物体ハンドリングシステム１は、マニピュレータ２００、制御装置１００、センサ３００、および筐体４００を、備えている。物体ハンドリングシステム１は、対象物１０を把持位置Ｐｈで把持して搬送し、解放位置Ｐｒで解放する。このような動作は、対象物１０の搬送、あるいは対象物１０のピッキングと称されうる。制御装置１００は、ハンド制御装置の一例である。対象物１０は、搬送の対象としての物体の一例である。

マニピュレータ２００は、アーム２１０と、ハンド２２０と、を備えている。アーム２１０は、例えば、複数（例えば６個）のサーボモータで回転駆動される回転部２１０ａ～２１０ｆを有した多関節ロボットである。アーム２１０は、これには限定されず、スカラロボットあるいは直動ロボットであってもよい。また、アーム２１０は、多関節ロボット、スカラロボット、および直動ロボットのうち少なくとも二つの組み合わせであってもよい。マニピュレータ２００は、ロボットアームとも称されうる。

ハンド２２０は、一例としては、図１に示されるような吸着機構を有するが、これには限定されず、例えば、ジャミング機構、挟み込み機構、または多指機構を有してもよい。また、ハンド２２０は、吸着機構、ジャミング機構、挟み込み機構、および多指機構のうち少なくとも二つを有してもよい。なお、ハンド２２０は、エンドエフェクタとも称されうる。

図２Ａ～２Ｄは、ハンド２２０の構成図である。図２Ａ～２Ｄに示されるように、ハンド２２０は、それぞれ、力センサ３１０を有している。力センサ３１０の位置は、図２Ａ～２Ｄには限定されない。また、ハンド２２０は、力センサ３１０以外の各種センサを有してもよい。力センサ３１０は、センサ３００の一例である。

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

図２Ａ～２Ｃに示されるように、ハンド２２０は、吸着機構としての吸着パッド２２０ａを有している。その場合、ハンド２２０は、例えば、吸着パッド２２０ａの圧力センサを有してもよい。また、吸着パッド２２０ａの数、配置、形状、大きさのようなスペックは、種々に変更することができる。

また、図２Ｄに示されるハンド２２０は、挟み込み機構あるいは多指機構としてのフィンガー２２０ｃを有している。

また、マニピュレータ２００は、ハンド２２０を交換可能に構成されてもよい。その場合、ハンド２２０の交換には、ツールチェンジャーのような機構（不図示）が用いられてもよい。

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

図３Ａ～３Ｄは、図２Ａ～２Ｄの各ハンド２２０が対象物１０を把持した状態を示す図である。図３Ａ～３Ｄ中の点Ｐｏは、ツールセンタポイントと称する。また、ツールセンタポイントＰｏを原点とし、ハンド２２０の把持基準軸Ａｘと重なるＺ軸と、当該Ｚ軸と直交するＸ軸およびＹ軸と、を有する座標系を、手先座標と称する。なお、把持基準軸Ａｘは、例えば、ハンド２２０を延び方向回りに回転させるハンド２２０に最も近い回転部（例えば、図１中の回転部２１０ｆ）の中心軸であり、ハンド２２０が屈曲部２２０ｂ（図３Ａ）を有する場合にあっては、ハンド２２０の先端部２２０ｄの重心（または対称中心）を通り当該先端部２２０ｄの延び方向に沿う軸、あるいは屈曲部２２０ｂの伸長状態においてハンド２２０に最も近い回転部（例えば、図１中の回転部２１０ｆ）の中心軸と重なる先端部２２０ｄの軸である。ハンド２２０は、例えば、把持基準軸ＡｘがツールセンタポイントＰｏを通り、かつ当該ツールセンタポイントＰｏがハンド２２０による対象物１０の接触領域の重心または中心となるよう、構成されるが、これには限定されない。なお、ツールセンタポイントＰｏは、コンプライアンスセンターとも称されうる。

また、図１に示されるように、物体ハンドリングシステム１は、複数のカメラ３２０を備えている。カメラ３２０は、センサ３００の一例であり、画像センサとも称されうる。カメラ３２０は、例えば、ＲＧＢ画像カメラや、距離画像カメラ、レーザーレンジファインダ、laser imaging detection and ranging（ＬｉＤＡＲ）など、画像情報や距離情報（３次元情報）を取得可能なセンサである。カメラ３２０は、観測部の一例である。

物体ハンドリングシステム１は、カメラ３２０として、例えば、把持用のカメラ３２１、把持状態確認用あるいはキャリブレーション用のカメラ３２２、解放用のカメラ３２３、および一時載置用のカメラ３２４を、備えている。

把持用のカメラ３２１は、把持（搬出）する対象物１０が存在している把持位置Ｐｈにおいて、対象物１０およびその周辺領域を撮影かつ検出する。当該把持位置Ｐｈにおいて、対象物１０は、例えばコンテナやパレットのような把持用の容器１４０ａ内に収容されている。その場合、把持用のカメラ３２１は、容器１４０ａ内の一部または全部を撮影かつ検出する。把持位置Ｐｈは、対象物１０の移動開始位置や出発位置とも称されうる。なお、図１では、容器１４０ａは、ベルトコンベアのような搬送機構上に載置されているが、これには限定されない。

キャリブレーション用のカメラ３２２は、把持用の容器１４０ａ外であるとともに把持位置Ｐｈよりも上方に設定された対象物１０のキャリブレーション位置において、対象物１０を撮影かつ検出する。

解放用のカメラ３２３は、対象物１０を解放（搬入）する解放位置Ｐｒにおいて、当該解放位置Ｐｒおよびその周辺領域を撮影かつ検出する。解放位置Ｐｒにおいて、対象物１０は、例えばコンテナやパレットのような解放用の容器１４０ｂ内に収容される。その場合、解放用のカメラ３２３は、容器１４０ｂ内の一部または全部を撮影かつ検出する。解放位置Ｐｒは、対象物１０の移動終了位置や到着位置とも称されうる。なお、図１では、容器１４０ｂは、ベルトコンベアのような搬送機構上に載置されているが、これには限定されない。

仮置き用のカメラ３２４は、把持位置Ｐｈと解放位置Ｐｒとの間で対象物１０を一時的に置いておく仮置位置Ｐｔおよびその周辺領域を撮影かつ検出する。仮置位置Ｐｔにおいて、対象物１０は、例えばテーブルやステージのような仮置き面１５０ｃ上に置かれる。その場合、仮置き用のカメラ３２４は、仮置き面１５０ｃの一部または全部を撮影かつ検出する。仮置位置Ｐｔは、対象物１０の途中位置や経由位置とも称されうる。なお、仮置位置Ｐｔは、載置面で無くてもよい。

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

［制御装置］
図４は、制御装置１００のブロック図である。制御装置１００は、例えば、統合部１０１、画像処理部１０２、信号処理部１０３、把持計画生成部１０４、解放計画生成部１０５、動作計画生成部１０６、ロボット制御部１０７、周辺機器・Ｉ／Ｏ制御部１０８（I／Ｏ：インプットアウトプット）、学習制御部１０９、エラー検出部１１０、および内部ＤＢ１２０を有している。

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

画像処理部１０２は、カメラ３２０から得られた画像や情報（検出値）を処理し、動作計画や、動作制御、エラー検出、学習等に必要な情報を生成する。

信号処理部１０３は、センサ３００から得られた情報（検出値）を処理し、動作計画や、動作制御、エラー検出、等に必要な情報を生成する。信号処理部１０３は、検出値取得部の一例である。

把持計画生成部１０４は、マニピュレータ２００による対象物１０の把持方法や、把持位置Ｐｈや把持姿勢、必要な個所が周囲環境に干渉することなく把持位置Ｐｈおよび把持姿勢に移動可能な経路および経由点を算出する処理を、実行する。把持計画生成部１０４は、計画生成部の一例であり、把持姿勢決定部の一例でもある。また、把持姿勢は、第一把持姿勢の一例である。ここで、必要な個所とは、例えば、マニピュレータ２００の全体やアーム２１０等の一部である。一方で、本実施形態の物体ハンドリングシステム１では、力の制御が可能であるため、吸着パッドや弾性関節部などある程度の柔軟性を持つ部分に関しては、多少の接触を許容してもよい。

解放計画生成部１０５は、マニピュレータ２００が把持した対象物１０を容器１４０ｂに箱詰めしたり設置したりする場所における例えば押し付けのような設置方法や、把持方法、解放方法、解放位置Ｐｒや解放姿勢、必要な個所が周囲環境に干渉することなく解放位置Ｐｒおよび解放姿勢に移動可能な経路および経由点を算出する処理を、実行する。解放計画生成部１０５は、計画生成部の一例である。また、解放姿勢は、解放する直前までの把持姿勢と略同じであるから、解放計画生成部１０５も、把持姿勢決定部の一例であると言うことができる。また、解放姿勢は、第一把持姿勢の一例であると言うことができる。

動作計画生成部１０６は、統合部１０１からの指示に従って、マニピュレータ２００が現在位置から移動可能な経路（経由点）を経由して把持位置Ｐｈや、仮置位置Ｐｔ、解放位置Ｐｒ等に移動するよう、動作方法や、動作速度、動作経路等のロボット動作情報の算出を実行する。動作計画生成部１０６は、計画生成部の一例である。

ロボット制御部１０７は、把持計画生成部１０４、解放計画生成部１０５、あるいは動作計画生成部１０６で生成されたロボット動作情報および統合部１０１からの各種の動作切り替え指示等に従って、マニピュレータ２００を含む物体ハンドリングシステム１を制御する。また、ロボット制御部１０７は、統合部１０１から取得した各種姿勢情報や軌道計画情報等に従い、マニピュレータ２００を制御する。ロボット制御部１０７は、動作制御部の一例である。

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

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

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

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

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

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

物品データベース（物品ＤＢ）には、例えば、対象物１０の名称、識別番号、カテゴリ、全面の画像情報、ＣＡＤモデル情報、重量情報、および把持時の特性情報（柔らかい、壊れやすい、形が変わる等）が、保存されている。

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

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

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

［ピッキング処理の手順］
図５は、制御装置１００が実行する対象物１０を把持し、移動して、解放する手順を示すフローチャートである。

まず、Ｓ１－１において、統合部１０１は、外部Ｉ／Ｆ１３０を経て、外部機器から対象物１０の搬送指示（ピッキング指示）を受け取る。

Ｓ１－２において、統合部１０１は、カメラ３２１の撮影した画像や、光電センサやマイクロスイッチのような他のセンサ３００の検出値に基づいて、対象物１０または当該対象物１０を収容した容器１４０ａが把持位置Ｐｈに到達したことを検出する。

Ｓ１－３において、統合部１０１は、対象物１０の把持位置Ｐｈおよびその周辺、例えば対象物１０が収容されている容器１４０ａ内を、撮像するよう、カメラ３２１を制御する。

Ｓ１－４において、画像処理部１０２は、カメラ３２１が撮影した画像から、対象物１０の有無の判定、対象物１０の把持可能面の識別を実行する。さらに、画像処理部１０２は、対象物１０の把持可能面の形状や大きさ、３次元空間内での位置や姿勢等の把持面情報を算出する。

Ｓ１－５において、把持計画生成部１０４は、把持面情報に基づいて、マニピュレータ２００が対象物１０の把持可能面を把持するための複数の把持情報を算出する。把持情報には、把持位置情報や把持姿勢情報が含まれる。ここで、把持計画生成部１０４は、容器１４０ａ内の対象物１０以外の物品（周辺部品）などの把持位置Ｐｈの周辺情報や、把持面情報や物品ＤＢの情報に基づいて、例えば、後述の力制御を実行する際のハンド２２０の移動方向や、押付力の大きさ、移動許容範囲のような付加情報を算出し、これを把持情報に付加する。

Ｓ１－６において、統合部１０１は、把持情報に基づいて、把持対象とする対象物１０、その把持位置Ｐｈ、および把持姿勢を、選択あるいは決定する。そして、動作計画生成部１０６は、マニピュレータ２００の現在位置から、把持位置Ｐｈおよび把持姿勢に至るロボット動作情報を生成する。ロボット動作情報の生成については、例えば、マニピュレータ２００と対象物１０以外の物体との干渉あるいは衝突の虞が無いあるいは確率が比較的低い領域では、高速な動作が選択される。他方、例えば、ハンド２２０が容器１４０ａに近付いたり容器１４０ａ内に入ったりするなど、ハンド２２０または対象物１０と当該対象物１０以外の物体との干渉あるいは衝突の虞があるあるいは確率が比較的高い領域では、速度を抑えた動作が選択される。さらに、動作計画生成部１０６は、この領域でハンド２２０に発生する力の目標値を決定する。

Ｓ１－７において、ロボット制御部１０７は、生成されたロボット動作情報に従って動作し、把持位置Ｐｈおよび把持姿勢で対象物１０を把持するよう、マニピュレータ２００およびハンド２２０を制御する。対象物１０の把持位置Ｐｈの周辺領域では、力制御のみによるハンド２２０の押付動作が実行され、吸着パッド２２０ａやフィンガー２２０ｃが対象物１０に十分に接触した状態で、吸着や挟み込み等の把持動作が実行される。把持位置Ｐｈの周辺領域では、ハンド２２０が、対象物１０以外の物体のような障害物Ｏに当たった場合には、与えられた動作方式や力の目標値に従って、ハンド２２０が障害物Ｏから逃げる動作等が実行される。また、対象物１０の把持面が比較的大きな傾きを持っているような場合にあっても、与えられた押付方法や押付力により、ハンド２２０を対象物１０の把持面に倣わせる動作が実行される。

統合部１０１またはロボット制御部１０７は、マニピュレータ２００やハンド２２０の動作や把持の状態を監視している。Ｓ１－８において、把持の成功が確認された場合には（Ｓ１－８でＹ）、処理はＳ１－１０に移行し、把持の失敗が確認された場合には（Ｓ１－８でＮ）、処理はＳ１－９に移行する。

Ｓ１－９では、統合部１０１は、マニピュレータ２００の退避動作や、把持の失敗を示す情報を物品ＤＢに登録する登録処理などのリトライ準備動作を実行する。Ｓ１－９の後、処理はＳ１－３に移行する。

Ｓ１－１０において、ロボット制御部１０７は、ハンド２２０による対象物１０の把持状態が撮影可能な位置に移動するよう、マニピュレータ２００を制御する。また、統合部１０１は、当該位置で、対象物１０を把持したハンド２２０を撮影するよう、カメラ３２２を制御する。

Ｓ１－１１において、画像処理部１０２は、カメラ３２２が撮影した画像から、例えば、ハンド２２０に対する対象物１０の相対位置、ハンド２２０に対する対象物１０の相対姿勢、および対象物１０の状態や形状のような、対象物１０情報を生成する。

Ｓ１－１２において、統合部１０１は、対象物１０の解放位置Ｐｒおよびその周辺、例えば把持されている対象物１０が解放され収容される容器１４０ｂ内を、撮影するよう、カメラ３２３を制御する。また、力センサ３１０のリセット等の重力補償動作が実施される。ここで、既に次の解放姿勢が決定している場合には、ロボット制御部１０７は、ハンド２２０が、解放姿勢と略同じかあるいは近いキャリブレーション姿勢となるよう、マニピュレータ２００およびハンド２２０を制御し、この状態で、重力補償部１０７ｄは、重力補償を実行する。これにより、より精度の高い重力補償動作を実施することができる。この際、合わせて力センサ３１０による対象物１０の重量計測を実施しても良い。

Ｓ１－１３において、画像処理部１０２は、カメラ３２３が撮影した画像から、解放位置Ｐｒおよびその周辺における、対象物１０以外の物体の有無の判定、当該物体の大きさや、位置、当該物体の表面の位置等の、周辺情報を生成する。解放位置Ｐｒの周辺情報は、障害物情報とも称されうる。

Ｓ１－１４において、解放計画生成部１０５は、対象物１０情報、把持ＤＢの情報、周辺情報等に基づいて、マニピュレータ２００が把持している対象物１０の解放情報を生成する。解放情報には、解放位置情報や、解放姿勢情報、経由点位置情報、経由点姿勢情報等が含まれる。ここで、解放計画生成部１０５は、力制御を伴うハンド２２０の押付方向や押付力の大きさ、力制御の移動許容範囲等の付加情報を算出し、これを解放情報に付加する。

Ｓ１－１５において、統合部１０１は、解放情報に基づいて、把持している対象物１０の解放位置Ｐｒおよび解放姿勢を、決定する。そして、動作計画生成部１０６は、マニピュレータ２００の現在位置から、解放位置Ｐｒおよび解放姿勢に至るロボット動作情報を生成する。この場合も、ロボット動作情報の生成については、例えば、マニピュレータ２００と対象物１０以外の物体との干渉あるいは衝突の虞が無いあるいは確率が比較的低い領域では、高速な動作が選択される。他方、例えば、ハンド２２０が容器１４０ｂに近付いたり容器１４０ｂ内に入ったりするなど、ハンド２２０または対象物１０と当該対象物１０以外の物体との干渉あるいは衝突の虞があるあるいは確率が比較的高い領域では、速度を抑えた動作が選択される。さらに、動作計画生成部１０６は、この領域でハンド２２０に発生する力の目標値を決定する。

Ｓ１－１６において、ロボット制御部１０７は、生成されたロボット動作情報に従って動作し、解放位置Ｐｒへ対象物１０を移動させ当該解放位置Ｐｒで対象物１０を解放するよう、マニピュレータ２００およびハンド２２０を制御する。対象物１０の解放位置Ｐｒの周辺領域では、対象物１０を容器１４０ｂにより高密度に配置するために、容器１４０ｂの壁や、既に置かれている十分な大きさの物品がある場合には、ハンド２２０により当該壁や物品の被押付面に対して対象物１０を押し付ける動作が、実行される。また、被押付面が比較的大きな傾きを持っているような場合にあっても、与えられた押付方法や押付力により、対象物１０を被押付面に倣わせて、密着した状態で解放する動作が実行される。

Ｓ１－１７では、対象物１０の解放後、ロボット制御部１０７は、容器１４０ｂから脱出するようハンド２２０を制御し、待機姿勢をとるようマニピュレータ２００を制御する。

Ｓ１－１８において、重力補償部１０７ｄは、力センサ３１０の検出値のリセット（この状態での検出値を０とする）のような、重力補償を実行する。ここで、次の対象物１０の把持姿勢が既に決定している場合には、ロボット制御部１０７は、ハンド２２０が、把持姿勢と略同じかあるいは近いキャリブレーション姿勢となるよう、マニピュレータ２００およびハンド２２０を制御し、この状態で、重力補償部１０７ｄは、重力補償を実行する。

Ｓ１－１９において、統合部１０１は、外部Ｉ／Ｆ１３０を経て、外部機器から次の対象物１０の搬送指示（ピッキング指示）を受け取った場合には、Ｓ１－２へ移行し、次の対象物１０に対する一連の制御を開始する（Ｓ１－１９でＹ）。次の対象物１０が無い場合には、一連の制御を終了する（Ｓ１－１９でＮ）。

［ハンドの移動経路］
図６は、ピッキング動作中におけるハンド２２０のツールセンタポイントＰｃの経路を示す。

位置Ｐ０００は、初期位置、もしくは待機位置である。ピッキング処理が実行されていない場合や、次の指示を待機する際には、ハンド２２０は、この位置Ｐ０００で待機する。また、位置Ｐ０００において重力補償を実行することにより、他の位置で改めて重力補償を実行する場合に比べてピッキングの処理時間を短縮することが可能である。

位置Ｐ００１は、容器１４０ａの外側に位置されている。位置Ｐ００１において、ハンド２２０の姿勢は、把持姿勢または把持姿勢に近い姿勢に設定される。例えば、屈曲部２２０ｂを持つようなハンド２２０では、この位置Ｐ００１において屈曲部２２０ｂの傾きを把持位置Ｐｈでの把持姿勢と同じ傾きに変更することができる。これにより、容器１４０ａ内でハンド２２０の姿勢を変化させる場合に比べて、より衝突の危険を低減することができる。また、位置Ｐ００１では、力センサ３１０の重力補償が実行されてもよい。これにより、後述の位置Ｐ００２および把持位置Ｐｈの周辺でのハンド２２０の対象物１０への押し付け、すなわちタッチ動作を実行する際に、より精度の高い力制御を実現することができる。なお、ハンド２２０は、位置Ｐ０００から位置Ｐ００１へは、例えば、位置（および姿勢）を制御量とする制御モード（第二制御モード）によって移動する。第二制御モードは、位置制御とも称されうる。また、姿勢は、例えば、把持基準軸Ａｘ回りの角度あるいは回転角度とも称されうる。

位置Ｐ００２は把持位置Ｐｈの直前の位置である。位置Ｐ００２は、一例としては、把持位置Ｐｈおよび把持姿勢から、ハンド２２０の押付方向の逆側に数ｃｍ離れた位置である。位置Ｐ００２でのハンド２２０の姿勢は把持位置Ｐｈでの姿勢とほぼ同じであってもよい。なお、ハンド２２０は、位置Ｐ００１から位置Ｐ００２へは、例えば、移動速度が比較的低い状態での位置を制御量とする制御（第二制御モード）によって移動してもよいし、力による退避動作が可能となるよう、位置に加えて力（およびモーメント）を制御量とする制御（第三制御モード）によって移動してもよい。第三制御モードは、位置・力制御とも称されうる。また、モーメントは、例えば、把持基準軸Ａｘ回りの回転モーメントとも称されうる。

把持位置Ｐｈは、指定された（演算された、推定された、あるいは決定された）把持位置Ｐｈである。把持位置Ｐｈの周辺（近傍）において、ハンド２２０は、例えば、力制御によって制御される。これにより、対象物１０との接触力を制御することができ、より確実性の高い把持が実現されやすい。なお、ハンド２２０は、位置Ｐ００２から把持位置Ｐｈへは、例えば、位置および力を制御量とする制御（第三制御モード）か、あるいは力（およびモーメント）のみを制御量とする制御（第一制御モード）で移動する。この場合、位置Ｐ００２から位置Ｐ００２と把持位置Ｐｈとの間の位置Ｐｃａまでの区間を第三制御モードにより制御し、位置Ｐｃａから把持位置Ｐｈまでを第一制御モードにより制御してもよい。第一制御モードは、力制御モードとも称されうる。把持位置Ｐｈは、目標位置の一例である。また、位置Ｐｃａは切替位置とも称されうる。

把持位置Ｐｈ’は、実際にハンド２２０による対象物１０の把持が実行された位置、または実行されたと判定された位置である。力制御が行われた場合において、例えば、対象物１０が実際には把持位置Ｐｈよりも手前側にあった場合には、ハンド２２０は対象物１０との接触力の増大に応じて、把持位置Ｐｈよりも手前で停止する。このような場合、把持位置Ｐｈ’と把持位置Ｐｈとは互いに異なることになる。また、対象物１０の把持面の実際の傾きが、指定された把持位置Ｐｈにおける把持面の傾きとは異なっていた場合、ハンド２２０は、力制御による倣い動作によって、実際の把持面に倣うことができる。また、ハンド２２０が把持位置Ｐｈに到達した場合にあっても、対象物１０が推定された位置からずれていたような場合、ハンド２２０と対象物１０との接触が発生せず、指定された力に達していない場合には、ハンド２２０は力制御によってさらに移動し、移動許容リミットに達するか、把持位置Ｐｈからずれていた把持面に接触して、接触力が指定された力に達した時点で停止する。このような場合にも、把持位置Ｐｈと把持位置Ｐｈ’とは異なる位置となる。また、ハンド２２０が位置Ｐ００２と把持位置Ｐｈとの間で、例えば障害物Ｏのような対象物１０とは異なる物体と接触し、指定された力目標を超えないように逃げるような動作をした場合にあっても、把持位置Ｐｈと把持位置Ｐｈ’とは異なる位置となる。

位置Ｐ００３は容器１４０ａの外側に位置される。位置Ｐ００３において、ハンド２２０の姿勢は、把持位置Ｐｈまたは把持位置Ｐｈ’での把持姿勢と略同じに設定される。把持位置Ｐｈ’と位置Ｐ００３との間では、対象物１０やハンド２２０は、他の位置に比べて、周囲の物体に接触しやすい。このため、ハンド２２０は、位置Ｐ００３へは、なるべく姿勢を変化させずに、容器１４０ａ内から脱出する。この場合、力制御はなるべく抑制され、位置制御によって、ハンド２２０は移動する。

位置Ｐ００４では、ハンド２２０の姿勢は、重力に対して対象物１０の把持をなるべく維持しやすい姿勢に設定される。対象物１０の把持を維持しやすい姿勢とは、例えば、吸着方式の場合には吸着パッド２２０ａが下向きになる姿勢であり、挟み込み方式の場合にはフィンガー２２０ｃが横方向に対して少し下側を向く姿勢である。

位置Ｐ００５および位置Ｐ００６への移動区間において、ハンド２２０は、把持状態の撮影に適した撮影姿勢に設定される。撮影姿勢は、言い換えると、把持した対象物１０を計測するためのカメラ３２２の撮像精度が高くなる姿勢である。この区間では撮影のための正確な位置情報が必要であるため、力制御をおこなわず、位置制御を実行する。また、この移動区間において、ハンド２２０の姿勢を変化させながら、力センサ３１０の重力補償を実行することができる。これにより、より精度の高い重力補償を実現できる。

位置Ｐ００６と位置Ｐ００７は待機位置である。容器１４０ｂの撮像や、解放計画の作成、解放に伴う動作情報の決定などが完了するまで、ハンド２２０は、位置Ｐ００６または位置Ｐ００７で待機する。また、このような待機位置において力センサ３１０の重力補償を実行することにより、他の位置で改めて重力補償を実行する場合に比べてピッキングの処理時間を短縮することが可能である。さらに、位置Ｐ００６または位置Ｐ００７において、解放姿勢、すなわち解放する直前の把持姿勢と等価な重力が作用するキャリブレーション姿勢で重力補償が実行されることにより、解放時の力制御をより高精度に実行することが可能となる。解放姿勢（解放する直前の把持姿勢）は、第一把持姿勢の一例であり、キャリブレーション姿勢は、第二把持姿勢の一例である。

位置Ｐ００８は容器１４０ｂから直上に外れた位置である。位置Ｐ００８において、ハンド２２０の姿勢は、解放姿勢または解放姿勢に近い姿勢に設定される。例えば、屈曲部２２０ｂを持つようなハンド２２０では、この位置Ｐ００８において屈曲部２２０ｂの傾きを解放位置Ｐｒでの解放姿勢と同じ傾きに変更することができる。これにより、容器１４０ｂ内でハンド２２０の姿勢を変化させる場合に比べて、より衝突の危険を低減することができる。なお、ハンド２２０は、位置Ｐ００６から位置Ｐ００８へは、例えば、位置を制御量とする制御（第二制御モード）によって移動する。

位置Ｐ００９は解放位置Ｐｒ直前の位置である。位置Ｐ００９は、一例としては、解放位置Ｐｒおよび解放姿勢から、ハンド２２０の押付方向の逆側に数ｃｍ～十数ｃｍ離れた位置である。位置Ｐ００９でのハンド２２０の姿勢は、解放位置Ｐｒでの姿勢とほぼ同じであってもよい。なお、ハンド２２０は、位置Ｐ００８から位置Ｐ００９へは、例えば、移動速度が比較的低い状態での位置を制御量とする制御（第二制御モード）によって移動してもよいし、力による退避動作が可能となるよう、位置に加えて力を制御量とする制御（第三制御モード）によって移動してもよい。

解放位置Ｐｒは、指定された（演算された、推定された、あるいは決定された）解放位置Ｐｒである。解放位置Ｐｒの周辺（近傍）において、ハンド２２０は、例えば、力制御によって制御される。これにより、既に置かれた物品など周囲の環境との接触力を制御することができ、より確実性の高い解放が実現されやすい。なお、ハンド２２０は、位置Ｐ００９から解放位置Ｐｒへの移動は、例えば、位置および力を制御量とする制御（第三制御モード）か、あるいは力のみを制御量とする制御（第一制御モード）で移動する。この場合、位置Ｐ００９から当該位置Ｐ００９と解放位置Ｐｒとの間の位置Ｐｃｂまでの区間を第三制御モードにより制御し、位置Ｐｃｂから解放位置Ｐｒまでを第一制御モードにより制御してもよい。解放位置Ｐｒは、目標位置の一例である。また、位置Ｐｃａは切替位置とも称されうる。

解放位置Ｐｒ’は、実際にハンド２２０による対象物１０の解放が実行された位置、または実行されたと判定された位置である。力による制御が行われた場合において、例えば、壁や既に置かれている物品のようなストッパが実際には解放位置Ｐｒよりも手前側にあった場合には、ハンド２２０は障害物Ｏ（周辺物体、隣接物体、先行物体）との接触力の増大に応じて、解放位置Ｐｒよりも手前で停止する。このような場合、解放位置Ｐｒ’と解放位置Ｐｒとは互いに異なることになる。また、解放位置Ｐｒ’近傍のストッパの被押付面の実際の傾きが、指定された解放位置Ｐｒにおける被押付面の傾きとは異なっていた場合、ハンド２２０は、力制御による倣い動作によって、実際の被押付面に倣うことができる。また、ハンド２２０が解放位置Ｐｒに到達した場合にあっても、ストッパが推定された位置からずれていたような場合、ハンド２２０とストッパとの接触が発生せず、指定された力に達していない場合には、ハンド２２０は力制御によってさらに移動し、移動許容リミットに達するか、解放位置Ｐｒからずれていた被押付面に接触して、接触力が指定された力に達した時点で停止する。このような場合にも、解放位置Ｐｒと解放位置Ｐｒ’とは異なる位置となる。また、ハンド２２０が位置Ｐ００９と解放位置Ｐｒとの間で、例えば障害物Ｏのような物体と接触し、指定された力目標を超えないように逃げるような動作をした場合にあっても、解放位置Ｐｒと解放位置Ｐｒ’とは異なる位置となる。

位置Ｐ０１０は容器１４０ｂの外側に位置される。位置Ｐ０１０において、ハンド２２０の姿勢は、解放位置Ｐｒまたは解放位置Ｐｒ’での解放姿勢と略同じに設定される。解放位置Ｐｒと解放位置Ｐｒ’との間では、対象物１０やハンド２２０は、他の位置に比べて、周囲の物体に接触しやすい。このため、ハンド２２０は、位置Ｐ０１０へは、なるべく姿勢を変化させずに、容器１４０ｂ内から脱出する。この場合、位置制御によって、ハンド２２０はより高速に移動する。

また、ハンド２２０が位置Ｐ００５または位置Ｐ００６から解放位置Ｐｒ’を経由した位置Ｐ０００までの区間においては、カメラ３２１により、ハンド２２０や対象物１０が障害となることなく、容器１４０ａ内の撮影が可能であり、ハンド２２０が位置Ｐ０１１から把持位置Ｐｈ’を経由した位置Ｐ００７までの区間においては、カメラ３２１により、ハンド２２０や対象物１０が障害となることなく、容器１４０ｂ内の撮影が可能である。また、本実施形態では、図６に示されるハンド２２０の経路の任意の位置において、力センサ３１０の検出値に基づいて、ハンド２２０と周囲環境との衝突の有無を検知することができる。

また、重力補償部１０７ｄは、ハンド２２０の経路内の、実際に押付を実行する解放位置Ｐｒに対して位置制御による移動区間（位置Ｐ００５から位置Ｐ００９）を挟んだ異なる位置Ｐ００５，Ｐ００６において、力センサ３１０の重力補償を実行することができる。これにより、経路から外れた別の位置で力センサ３１０の重力補償を実行する場合に比べて、ピッキングの処理時間を短縮することが可能である。

このように、本実施形態の物体ハンドリングシステム１では、ピッキング処理おける把持や箱詰め動作の際に、経路の状況に応じて、位置制御や、力制御、および位置制御および力制御を並行して実行するハイブリッド制御を切り替えることにより、より効率良くピッキング処理を実行することができる。また、種々の状況に応じて、より適切な位置や、時刻、ハンド２２０の姿勢において、力センサ３１０の重力補償動作を実行することにより、より精度の高い力制御動作を実現することができる。

図６に例示される経路では、例えば、容器１４０ａ内の位置Ｐ００２または位置Ｐｃａから把持位置Ｐｈまでの区間および容器１４０ｂ内の位置Ｐ００９または位置Ｐｃｂから解放位置Ｐｒまでの区間が、第一制御モードによる第一区間の一例であり、容器１４０ａ，１４０ｂ外の位置Ｐ００３から位置Ｐ００８までの区間および位置Ｐ０１０から位置Ｐ００１までの区間が、第二制御モードによる第二区間の一例である。また、容器１４０ａ内の位置Ｐ００２から位置Ｐｃａまでの区間および容器１４０ｂ内の位置Ｐ００９から位置Ｐｃｂまでの区間が、第三制御モードによる第三区間の一例である。また、位置Ｐ００１から位置Ｐ００２までの区間、把持位置Ｐｈから位置Ｐ００３までの区間、位置Ｐ００８から位置Ｐ００９までの区間、および解放位置Ｐｒから位置Ｐ０１０までの区間の、容器１４０ａ，１４０ｂ内を通る第四区間にあっては、位置制御を含む第二制御モードまたは第三制御モードによりハンド２２０の制御が実行されるが、これら区間におけるハンド２２０の移動速度は、容器１４０ａ，１４０ｂ外の第二制御モードによる第二区間でのハンド２２０の移動速度よりも低く設定される。

［力制御］
図７Ａは、力制御による対象物１０の解放位置Ｐｒへの移動を示す斜視図であって、対象物１０を解放位置Ｐｒの周辺に存在する部品（障害物Ｏ）に押し付ける場合を示す図、図７Ｂは、力制御による対象物１０の解放位置Ｐｒへの移動を示す側面図であって、対象物１０を解放位置Ｐｒの周辺に存在する部品（障害物Ｏ）の斜面に沿って押し付ける場合を示す図である。

ここでは、特にハンド２２０により対象物１０を容器１４０ｂ内に隙間なく詰める場合、所謂箱詰めの場合における、ロボット制御部１０７の力制御系（以下、単に力制御系と称する）の構成および動作について説明する。力制御系は、ロボット制御部１０７の位置制御系（以下、単に位置制御系と称する）に対し、マニピュレータ２００の手首部（具体的には回転部２１０ｆとハンド２２０との間）に配置された力センサ３１０の検出値をフィードバックし、リアルタイムで位置補正を実行する。力センサ３１０は、例えば、６軸力覚センサであり、３方向の力と３方向を軸とする回転モーメントを検出する。力制御系は、具体的には、箱詰め動作において、対象物１０を密着させる面に対して当該対象物１０を押し付けることが可能となる長めの位置目標値軌道を生成しておき、対象物１０が被押付面に接触したときに倣うように、言い換えると力を逃がすように、制御する。倣う方向としては、並進の３自由度と鉛直軸回りの姿勢の１自由度としておけば、図７Ａ，７Ｂの密着動作に対応することができる。姿勢の残りの２自由度については、後述するバネ動作として対象物１０を把持するハンド２２０の姿勢が変化したり対象物１０がずれたりしないようにする。このように、本実施形態の物体ハンドリングシステム１は、容器１４０ａ，１４０ｂのような、ハンド２２０の挿入先（箱詰め先）の状態によって各軸の力制御の方式を切り替えることにより、効果的な把持や箱詰めを実現することができる。

図８は、ロボット制御部１０７のブロック図である。本実施形態では、一例として、ロボット制御部１０７は、位置制御系をベースとした力制御系のブロック構成を有している。力センサ３１０が手首部にあるため、力制御系は手先座標系でフィードバックループを構成し、マニピュレータ２００のベース座標系への座標変換をリアルタイムに実行しながら、関節座標系での位置制御を実行する。ロボット制御部１０７は、手先座標力制御部１０７ａ、逆運動学演算部１０７ｂ、関節座標位置制御部１０７ｃ、および重力補償部１０７ｄ、を有している。手先座標力制御部１０７ａは手先位置の目標値、すなわちマニピュレータ２００（ハンド２２０）のツールセンタポイントＰｏの位置および姿勢の目標値と力制御に係るパラメータおよび力センサ３１０の検出値に基づく力情報を受け取り、手先位置の指令値を逆運動学演算部１０７ｂに出力する。逆運動学演算部１０７ｂは、一般に逆運動学演算（インバースキネマティクス）と呼ばれる演算を行い、ツールセンタポイントＰｏの位置および姿勢の情報から、当該位置および姿勢を実現するマニピュレータ２００の各関節軸の回転位置を算出する。関節座標位置制御部１０７ｃは、逆運動学演算部１０７ｂから出力された各関節軸の回転位置指令値と、マニピュレータ２００の各関節軸の関節位置情報を受け取り、関節位置指令値を出力する。関節座標位置制御部１０７ｃは、関節位置を所定の目標値に維持するようフィードバック制御を実行する。重力補償部１０７ｄは、ハンド２２０が所定の位置および姿勢にある状態で、対象物１０の重力補償のため、力センサ３１０のキャリブレーションを実行する。キャリブレーションが実行される位置は、ハンド２２０の経路の途中に組み込まれる。また、キャリブレーションは、ハンド２２０が対象物１０を把持した状態で、対象物１０を把持する直前の姿勢と略同じ姿勢、または対象物１０を解放する直前の姿勢と略同じ姿勢において、実行される。

図９は、手先座標力制御部１０７ａのブロック図である。手先座標力制御部１０７ａは、手先座標系における力・モーメント目標値と検出値との差を力フィードバック制御部１０７ａ２で演算して微小変位・回転角（後述）を算出し、位置目標値をリアルタイム（サンプル周期は数ｍｓ毎）で修正することにより、外力・モーメントに応じてハンド２２０または対象物１０を被押付面に倣わせる。ツールセンタポイントＰｏは、力・モーメント制御（力制御）の作用点であり、手先座標系の原点であって、上述したように、吸着方式の場合、吸着パッド２２０ａの中心に設定される。ツールセンタポイントＰｏでは、並進の力が加わると並進動作だけが生じ、モーメントが加わると回転動作だけが生じる。ここで、力・モーメントの検出値を力センサ３１０の座標系から手先座標系へ変換する必要がある。手先座標力制御部１０７ａは、管理部１０７ａ１、力フィードバック制御部１０７ａ２、変換部１０７ａ３、および手先位置制御部１０７ａ４を有している。管理部１０７ａ１は、入力された力制御モードの指令に従って、力フィードバック制御部１０７ａ２のモード切り替パラメータや変換切り替えパラメータを設定する。さらに、各軸の力・モーメントの目標値を設定する。ここで、力フィードバック制御部１０７ａ２のモード切替とは、各軸の力制御のオンオフや、積分動作、ばね動作などの切り替え、経路の状況による制御ゲインの切り替え等を意味する。これは、力制御の動作モードの切り替えの一例である。また、変換切り替えパラメータは、ハンド２２０の状態やハンド２２０による対象物１０の把持の状態に応じて、ツールセンタポイントＰｏの位置を切り替える際に用いられる。力フィードバック制御部１０７ａ２は、力センサ３１０の座標系から手先座標系に座標変換された力・モーメントの検出値である力情報と力・モーメントの目標値の差分信号を入力として、ツールセンタポイントＰｏの微小変位・回転角指令値を出力する。変換部１０７ａ３は、力センサ３１０からの３軸の力と３軸回りのモーメントである力情報をツールセンタポイントＰｏでの力情報に変換する処理を実行する。この際、力センサ３１０の設置位置とツールセンタポイントＰｏの幾何的な位置関係から、座標変換により変換処理を実行することができる。ここで、例えば屈曲部２２０ｂなどの稼働により、力センサ３１０とツールセンタポイントＰｏとの相対姿勢および相対位置が変化した場合は、これを考慮した変換を実行する。また、ハンド２２０が対象物１０を把持した場合には、対象物１０の重心位置をツールセンタポイントＰｏとしてもよいし、動作中に画像認識等により、把持された対象物１０が何かに接触するようなことが推定される場合には、その接触推定点をツールセンタポイントＰｏとしても良い。このようなツールセンタポイントＰｏの切り替えは、変換切り替えパラメータによって行われる。

力フィードバック制御部１０７ａ２は、３軸方向の力と、３軸方向のモーメントの６個の制御器で構成されている。制御器は、同じ構造を有し、次の式（１）で表される。

・・・ （１）
ここに、Ｋ_Ｇは、力制御比例ゲイン、Ｋ_Ａは、力制御積分ゲイン、Ｍは、モデルマス定数、Ｄは、モデルダンピング定数、Ｋは、モデルバネ定数である。式（１）は、バネ・マス・ダンパモデルを表す２次フィルタに比例積分（ＰＩ）制御器が直列に接続された伝達関数（ディジタルでの実装ではサンプル周期毎の矩形積分）である。ここで、Ｋ_Ｇ＝０とすると、Ｇｆ（ｓ）全体が０となる。この場合は、手先座標力制御部１０７ａの力制御寄与分は無くなり、位置制御になる。このように、力フィードバック制御部１０７ａ２では、位置制御のみの制御と力制御の切換がスムースに行える。また、Ｋ_Ｇ≠０、Ｋ_Ａ＝０とすると、外力に対するバネ動作が実現でき、Ｋ_Ｇ≠０、Ｋ_Ａ≠０とすると、外力に応じて被押付面に倣う積分動作が実現できる。

ここで、バネ動作とは、仮想的にバネがあるような姿勢・位置の復元を伴う動作である。例えば、力の目標値を０とした力の制御中にある軸方向に外力が加わると、力フィードバック制御部１０７ａ２は力センサ３１０に検知された外力を０にするように、指令値を出力する。この結果、マニピュレータ２００はある軸の外力と反対方向に位置を移動する。ここで、バネ動作時には、外力がなくなった場合には元の位置に戻る。このような動作はバネ動作と称される。

また、積分動作とは外力に従って移動し、被押付面に倣う動作である。例えば、力の目標値を０とした力の制御中にある軸方向に外力が加わると、力制御器は力センサ３１０に検知された外力を０にするように、指令値を出力する。この結果、マニピュレータ２００はある軸の外力と反対方向に位置を移動する。ここで、積分動作時には、マニピュレータ２００は外力がなくなった後も、外力によって最終的に移動した位置にとどまる。このような動作は積分動作と称される。

力フィードバック制御部１０７ａ２は、２～３次のローパスフィルタと見なすことができ、力センサ信号のノイズ除去も同時に行える。力センサ信号のＡＤ入力において、アンチエリアシングフィルタとして簡易なアナログローパスフィルタを入れておけば十分である。

本実施形態の物体ハンドリングシステムでは例えば、図７Ａ，７Ｂに示されるような押し付けの動作において、力フィードバック制御部１０７ａ２の設定を、並進の３自由度と鉛直軸回りを積分動作、姿勢の残りの２自由度をバネ動作とする。このように、押付方向や回転方向に対して適切な力制御モードを設定することで、より確実に密着した箱詰め動作を実現することができる。

積分動作ではドリフトを防ぐために、あらかじめ力センサ３１０のキャリブレーションの一種である重力補償が必要である。これは、力センサ３１０の先にある質量によって発生する力の成分をキャンセルし、衝突時や押し付け時における外力をより高精度に検出するための処理である。

図１０は、重力補償部１０７ｄの構成図である。図１０に示されるように、重力補償部１０７ｄは、ローパスフィルタ１０７ｄ１（ＬＰＦ）、管理部１０７ｄ２、メモリ１０７ｄ３、座標変換部１０７ｄ４を有している。ローパスフィルタ１０７ｄ１は、例えば、重力補償値を得るために、次の式（２）で表せる力センサ信号６軸分に、遅い時定数Ｔ_ｆ０、１秒のオーダ）の１次ローパスフィルタを力フィードバック制御部１０７ａ２と並行して動作させておく。

・・・ （２）
管理部１０７ｄ２は、統合部１０１から、ロボット姿勢情報、対象物情報、および管理情報を受け、重力補償動作を実行する。ロボット姿勢情報は、ハンド２２０を含むマニピュレータ２００の姿勢を示す情報である。対象物情報は、対象物１０の重量や、大きさ、把持状態等の情報である。管理情報は、例えば、補償実行指示や、力制御開始指示等である。統合部１０１は、上述した位置Ｐ００６や、位置Ｐ００７、位置Ｐ０００のような、ハンド２２０が重力補償値を取得する位置に到来した時点で、管理部１０７ｄ２に、管理情報としての補償実行指示を出す。管理部１０７ｄ２は、補償実行指示を取得すると、その時点でのロボット姿勢情報や、対象物情報とともに、ローパスフィルタ１０７ｄ１を通過した後の６軸各軸の力情報をメモリ１０７ｄ３に保持する。この時、メモリ１０７ｄ３がマニピュレータ２００の複数の姿勢に対応したロボット姿勢情報や複数の対象物情報を保持しておくことにより、座標変換部１０７ｄ４における重力補償値の算出の精度をより高めることができる。各軸のメモリ情報はピッキングの動作中以外でも、事前に蓄積することも可能である。管理部１０７ｄ２は、管理情報としての力制御開始指示を受け取ると、座標変換部１０７ｄ４に、ロボット姿勢情報、対象物情報、補償値出力開始指示を出す。座標変換部１０７ｄ４は、補償値開始指示に対応して、重力補償値を出力する。ここで、座標変換部１０７ｄ４はメモリ１０７ｄ３に保持されたロボット姿勢情報や対象物情報毎に保持された力情報を用い、管理部１０７ｄ２から入力された動作中のロボット姿勢情報、対象物情報に従って、重力補償値を算出し、出力する。一例としては、座標変換部１０７ｄ４は、保持された力情報と動作中のロボット姿勢情報が同じか近い値の場合の力情報を、重力補償値（オフセット）として出力し、力センサ３１０の計測値（生値）から引いておけば良い。メモリ１０７ｄ３内に現在の姿勢に近いロボット姿勢情報に対応する重力補償値が保持されていない場合には、他のロボット姿勢情報に対応する重力補償値から補完処理を行うなどすることにより、重力補償値を算出する。なお、ローパスフィルタ１０７ｄ１の出力値は、力・モーメント目標値達成の閾値判定にも使える。このような簡易的な重力補償は演算が高速に可能なため、高速な処理が必要なピッキング処理に用いる重力補償として適している。

ただし、力制御時の姿勢が大きく変化すると、補償値の誤差が大きくなる。特に、重力方向と力センサ３１０の力検出軸のなす角が変化すると誤差が大きくなる。そこで、本実施形態の物体ハンドリングシステムで１は、力制御を実行する行程と同じか、より近い姿勢をとった状態で、重力補償を実行する。少なくとも、重力方向と力センサ３１０の力検出軸のなす角が略一致するような姿勢をとった状態、例えば、対象物１０の押し付け時の力センサ３１０のＺ軸の向きが、重力方向と並行である場合には、重力補償時も同様に力センサ３１０のＺ軸の向きが、重力方向と並行とする。この際、Ｚ軸回りの回転位置、すなわちＸ軸やＹ軸の方向は異なる状態でも構わない。これらの方向は力制御実施時に、姿勢を考慮した演算を実行することで、安定した力制御を可能としている。

図９において、力フィードバック制御部１０７ａ２の出力を微小変位・回転角［ΔＸ，ΔＹ，ΔＺ，ΔＡ，ΔＢ，ΔＣ］と見なすと、これは、同次変換行列としては、次の式（３）のように表せる。

・・・ （３）
で表される。手先位置制御部１０７ａ４では、この行列をベース座標系で表された手先の（ノミナルの）位置目標値軌道［Ｘ，Ｙ，Ｚ，Ａ，Ｂ，Ｃ］から導出した同次変換行列Ｔに右から掛けて、Ｔ（Δ＋Ｉ）を計算することによって、修正された位置目標値軌道のベース座標系での同次変換行列をリアルタイムで得ることができる。さらに、図８の逆運動学演算部１０７ｂにおける逆座標変換によって各関節の位置制御系への目標値が算出される。

ここで、上述したような「密着させる面に対してハンド２２０または対象物１０を押し付ける（長めの）位置目標値軌道を生成しておき、対象物１０が被押付面に接触したときに当該被押付面に倣うように力制御系を機能させる」だけでは、密着が完了するとは限らない。そこで、位置目標値が終了してから、力制御器だけを機能させて密着させる次のようなシーケンスが必要となる。（１）～（５）により、対象物１０の容器１４０ａ，１４０ｂの隅部への密着が完了する。
（１）対象物を吸着し、容器１４０ａ，１４０ｂの上方へ移動
（２）力センサ３１０の現在値から重力補償値を決定
（３）並進Ｘ，Ｙ，Ｚの３方向、垂直軸（例えばＸ軸）回りを積分動作とし、姿勢方向（例えば、Ｙ軸回り方向、Ｚ軸回り方向）をバネ動作として、力目標値＝０（ゼロ）で、容器１４０ａ，１４０ｂの隅付近のＸ，Ｙ，Ｚ座標を目標値に設定して直線補間で移動
（４）位置目標値軌道が終了したら、さらに、押付力目標値を設定して待機
（５）位置制御モードに戻す

（４）では、（ノミナルの）直線補間の目標値が終了した後に、改めてＸ，Ｙ（平面）方向の力目標値を設定し、例えば、容器１４０ａ，１４０ｂの壁にはね返されても、力制御だけで密着動作をさせることをねらっている。また、Ｚ（鉛直）方向の力目標値とＣ（鉛直軸）回りのモーメント目標値は０（ゼロ）のままとしている。前者は、重力を利用すればＺ方向の力制御は不要であること、後者は、Ｘ，方向Ｙの力目標値があるので、Ｃ回りにはモーメントの目標値をゼロとする制御を実行すれば十分という技術的思想に基づいている。

また、本実施形態の物体ハンドリングシステム１では、図８，１０に示されるようなロボット制御部１０７において、衝突検知等のエラー検知を実行することができる。例えば、ロボット制御部１０７に入力される力情報を図示しないローパスフィルタに通し、エラー検出部１１０（図４）にてモニタリングを実行する。ここで、位置制御中に衝突等が発生し、力センサ３１０やハンド２２０が壊れるような力が発生した場合や、力制御中であっても、力をキャンセルするマニピュレータ２００の移動速度（速度限界、上限速度）を超え、力センサ３１０やハンド２２０が壊れるような力が発生した場合には、マニピュレータ２００の動作を急停止する。また、図９の変換部１０７ａ３の出力を活用し、手先に発生する力をモニターすることで、力センサ３１０やハンド２２０が壊れないような力の範囲であっても、手先に把持した物品が壊れるような範囲の力が発生した際に、マニピュレータ２００の動作を緊急停止することができる。このように力制御と力検出によるマニピュレータ２００の動作の切り替えを併用することにより、より安定的に把持、箱詰め動作を実現することができる。

［把持動作］
次に、物体ハンドリングシステム１が、対象物１０を把持する際に実施する把持動作の手順について説明する。ロボット制御部１０７は、把持動作時には、力制御によりハンド２２０を対象物１０に優しく押し付ける。

図１１は、ハンド２２０が対象物１０を把持する状態を示す斜視図である。図１１に示されるように、ロボット制御部１０７は、対象物１０の把持面に対して鉛直方向にハンド２２０を近付けて比較的軽く押し付ける。これは、タッチ動作とも称されうる。タッチ動作においては、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、所定の押付力に相当する大きさかつ図１１中太矢印で示す把持面との直交方向（方向Ｚ）のベクトルの力目標値を設定し、押し付けない方向、言い換えると押付方向との直交方向（方向Ｘ，方向Ｙ）には力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値を０、または不感帯以下の値に設定したばね動作を実行し、Ｚ軸周りは力目標値を０に設定した積分動作を実行する。ここで不感帯とは、実際には力センサ３１０で力が検出されていたとしても、フィードバック制御を行わない力の範囲である。不感帯は、例えば、押付力のような制御力以下に設定される。

図１２は、把持動作時におけるロボット制御部１０７によるマニピュレータ２００の制御手順を示すフローチャートである。図１３Ａ～１３Ｇは、把持動作における各位置でのハンド２２０の状態または移動経路を示す側面図である。

Ｓ２－１において、ロボット制御部１０７は、統合部１０１等から把持動作の開始指示を受けると、位置制御を実行する第二制御モードにより、ハンド２２０を位置Ｐ００１までより高速に移動させる。

Ｓ２－２において、ロボット制御部１０７は、位置Ｐ００１において、ハンド２２０の姿勢を、把持位置Ｐｈでの姿勢と略同じ姿勢か近い姿勢に設定する。

Ｓ２－３において、画像処理部１０２および統合部１０１は、カメラ３２１が撮影した画像から、把持位置Ｐｈに到達するまでの間に、ハンド２２０が対象物１０以外の障害物Ｏと接触するか否かを判断する。接触するあるいは接触する可能性が高いと判断された場合（Ｓ２－３でＹ）、Ｓ２－１１に移行する。接触しないあるいは接触する可能性が低いと判断された場合（Ｓ２－３でＮ）、Ｓ２－４に移行する。

Ｓ２－４において、ロボット制御部１０７は、図１３Ａに示される位置Ｐ００１から位置Ｐ００２までの区間において、ハンド２２０を、位置制御を実行する第二制御モードにより、例えば容器１４０ａ，１４０ｂ外の区間のような他の位置制御による区間よりも遅い速度で、移動させる。このように、位置制御において移動速度がより低い区間を設定することにより、衝突検知のための力の閾値をより低く設定することができ、当該閾値が高い場合に比べて、把持動作をより確実に実行することができるとともに、障害物Ｏと干渉した場合に当該障害物Ｏから受ける力をより小さくすることができる。

Ｓ２－５において、ハンド２２０は、図１３Ａに示される位置Ｐ００２に到達する。

Ｓ２－６において、ロボット制御部１０７は、ハンド２２０の制御モードを、位置制御を実行する第二制御モードから、位置制御とともに力制御を実行する第三制御モードに切り替える。第三制御モードでは、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値を０、または不感帯以下の値を設定したばね動作を実行し、Ｚ軸周りは力目標値を０、または不感帯以下の値に設定した積分動作を実行する。当該制御において制御量が目標値に達したら、Ｓ２－７に移行する。

Ｓ２－７において、ロボット制御部１０７は、位置Ｐ００２から把持位置Ｐｈへ移動する途中の図１３Ｂに示される位置Ｐｃａにおいて、言い換えると、把持位置Ｐｈの手前の位置Ｐｃａに到達した時点で、ハンド２２０の制御モードを、位置制御とともに力制御を実行する第三制御モードから、力制御を実行する第一制御モードに切り替える。ロボット制御部１０７は、第一制御モードでの目標値を、ハンド２２０による対象物１０の把持面（接触面）に対する垂直方向への押し付けが可能となるように設定する。具体的には、例えば、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、所定の押付力（例えば５Ｎに相当する大きさ）および把持面に対する垂直方向（方向Ｚ）のベクトルの力目標値を設定し、押し付けない方向（方向Ｘ，方向Ｙ）には力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値に０、または不感帯以下の値を設定したばね動作を実行し、Ｚ軸周りは力目標値に０の積分動作を実行する。

ここで、仮に、位置制御モードでは、制御上は、ハンド２２０は目標位置（この場合は把持位置Ｐｈ）に到達していたとしても、実際には、ハンド２２０が目標位置の手前で物に接触し、そこで止まってしまうような場合もある。この点、本実施形態によれば、ロボット制御部１０７は、ハンド２２０を、位置Ｐ００２から把持位置Ｐｈへ、位置制御とともに力制御を実行する第三制御モードまたは力制御を実行する第一制御モードによって移動させるため、ハンド２２０の対象物１０に対する押圧力が所期の値に設定されやすくなるとともに維持されやすくなる。また、本実施形態では、第一制御モードでは、Ｘ軸、Ｙ軸回りのモーメントは、目標値＝０のバネ動作に設定されているため、対象物１０の把持面が傾いていた場合においても、ハンド２２０に発生するモーメントが減殺され、ハンド２２０が把持面に対して直交し、言い換えるとハンド２２０の吸着パッド２２０ａが把持面に倣い、これにより、ハンド２２０による対象物１０のより確実な把持状態が得られやすくなる。

Ｓ２－８において、ロボット制御部１０７は、制御量が目標値に達したか否かを判断する。目標値に達していない場合、Ｓ２－９に移行する。制御量が目標値に達している場合には、Ｓ２－１０に移行する。

Ｓ２－９では、ロボット制御部１０７は、各パラメータの値がリミット（閾値）に達しているか否かを判定する。パラメータは、例えば、予め設定されている位置リミットや、所定時間を超えた処理を設定する時間リミット等である。リミットにより、マニピュレータ２００（ハンド２２０）が位置や姿勢の所定範囲を超えていないかを判定することができる。リミットに達した場合（Ｓ２－９でＹ）、一連の制御を終了する。リミットに達していない場合（Ｓ２－９でＮ）、Ｓ２－１０に移行する。

Ｓ２－１０では、ロボット制御部１０７は、図１３Ｃに示される把持位置Ｐｈにおける対象物１０の把持をより確実に実行するため、ハンド２２０による把持状態を少なくとも所定時間以上維持する。Ｓ２－１０において、把持状態が所定時間以上継続できた場合には、把持動作が終了され、次の手順（例えば、ハンド２２０の位置Ｐ００３への移動）に移行する。Ｓ２－１０において、把持状態が所定時間以上継続できなかった場合には、リカバリ動作等に移行する。

他方、Ｓ２－３において、接触するあるいは接触する可能性が高いと判断された場合（Ｓ２－３でＹ）、Ｓ２－１１において、ロボット制御部１０７は、位置Ｐ００１から、すなわち容器１４０ａ外から位置Ｐ００５へ、位置制御を実行する第二制御モードによらず、位置制御とともに力制御を実行する第三制御モードによって、ハンド２２０を移動させる。これにより、ハンド２２０が障害物Ｏと接触した場合にあっても、大きな接触力を発生させずに、接触点から逃げるような動作を実行することができる。ただし、第三制御モードによる移動距離が長くなると、所要時間が長くなる。よって、Ｓ２－１１では、Ｓ２－６よりも速度を高くしてもよい。Ｓ２－１１の後は、Ｓ２－５へ移行する。

このような把持動作時の制御により、対象物１０がばらばらに積まれているような場合であっても、力制御によって、ハンド２２０を、対象物１０の把持面に、その垂直方向に所定の大きさの力で押し付けることができる。よって、例えば、ハンド２２０の押圧や衝突による対象物１０の破損や、ハンド２２０と対象物１０とが接触していないことによる把持の失敗等を、回避できる。

［解放動作］
次に、物体ハンドリングシステム１が、対象物１０を解放する際に実施する解放動作の手順について説明する。ロボット制御部１０７は、解放動作時には、力制御により対象物１０を解放位置Ｐｒの周辺の壁や物体に優しく押し付けるタッチ動作を実行する。

図１４Ａ，１４Ｂは、ハンド２２０による解放動作の一例を示す斜視図である。図１４Ａは、解放動作においてハンド２２０の回転を伴わない場合の例であり、図１４Ｂは、解放動作においてハンド２２０の回転を伴う場合の例である。解放動作においては、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、所定の押付力に相当する大きさかつ図１４Ａ，１４Ｂ中の太矢印で示す押付方向のベクトルの力目標値を設定し、押し付けない方向には力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値を０、または不感帯以下の値に設定したばね動作を実行し、Ｚ軸周りは力目標値を０に設定した積分動作を実行する。図１４Ａの例では、押付方向は、障害物Ｏと容器１４０ｂの壁１４０ｂ１との間の隅部に挿入する方向であり、ロボット制御部１０７は、対象物１０を、当該障害物Ｏおよび壁１４０ｂ１に押し付ける。この場合、各方向の押付力の大きさを仮に５［Ｎ］とした場合、（Ｘ，Ｙ，Ｚ，Ａ，Ｂ，Ｃ）方向の力目標値は（５［Ｎ］，－５［Ｎ］，－５［Ｎ］，０［Ｎｍ］，０［Ｎｍ］，０［Ｎｍ］）を設定する。これにより、対象物１０は容器１４０ｂおよび既に置いてある障害物Ｏに密着し、より集積度の高い箱詰めが実現されうる。

図１４Ｂに示されるように、解放動作においてハンド２２０の回転を伴う場合にあっても、図１４Ａの場合と同様の制御が実行される。この場合も、各方向の押付力の大きさを仮に５［Ｎ］とした場合、（Ｘ，Ｙ，Ｚ，Ａ，Ｂ，Ｃ）方向の力目標値は（５［Ｎ］，－５［Ｎ］，－５［Ｎ］，０［Ｎｍ］，０［Ｎｍ］，０［Ｎｍ］）に設定されている。ただし、図１４Ｂでは、ロボット制御部１０７は、対象物１０を、障害物Ｏの側面Ｏ１に斜めに押し付ける。すると、対象物１０およびハンド２２０は、障害物Ｏとの接触点を中心とするＺ軸回りのモーメントが生じる。この場合、Ｚ軸回りのモーメントの目標値は０［Ｎｍ］であるため、発生するモーメントを打ち消すように、ハンド２２０は回転し、既に置いてある障害物Ｏに倣うように、すなわち障害物Ｏの側面Ｏ１と平行な状態に、対象物１０を配置することができる。このように、本実施形態では、物体ハンドリングシステム１は、各軸方向のパラメータにそれぞれ適切な値を設定することにより、容器１４０ｂ内の障害物Ｏ等の状況に応じて対象物１０を密着して配置することが可能となり、よりスペース効率の高い対象物１０の容器１４０ｂへの収納が実現されうる。

図１５は、ロボット制御部１０７によるマニピュレータ２００の制御手順を示すブローチャートである。図１６Ａ～１６Ｈは、解放動作における各位置でのハンド２２０または対象物１０の状態または移動経路を示す図である。

Ｓ３－１において、ロボット制御部１０７は、位置制御を実行する第二制御モードにより、ハンド２２０を位置Ｐ００８までより高速に移動させる。

Ｓ３－２において、ロボット制御部１０７は、位置Ｐ００８において、ハンド２２０の姿勢を、把持位置Ｐｈでの姿勢と略同じ姿勢か近い姿勢に設定する。

Ｓ３－３において、画像処理部１０２および統合部１０１は、カメラ３２２が撮影した画像から、解放位置Ｐｒに到達するまでの間に、ハンド２２０が対象物１０以外の障害物Ｏと接触するか否かを判断する。接触するあるいは接触する可能性が高いと判断された場合（Ｓ３－３でＹ）、Ｓ３－１１に移行する。接触しないあるいは接触する可能性が低いと判断された場合（Ｓ３－３でＮ）、Ｓ３－４に移行する。

Ｓ３－４において、ロボット制御部１０７は、図１６Ａに示される位置Ｐ００８から位置Ｐ００９までの区間において、ハンド２２０を、位置制御を実行する第二制御モードにより、例えば容器１４０ａ，１４０ｂ外の区間のような他の位置制御による区間よりも遅い速度で、移動させる。このように、位置制御において移動速度がより低い区間を設定することにより、衝突検知のための力の閾値をより低く設定することができ、当該閾値が高い場合に比べて、把持動作をより確実に実行することができるとともに、障害物Ｏと干渉した場合に当該障害物Ｏから受ける力をより小さくすることができる。

Ｓ３－５において、対象物１０を把持したハンド２２０は、図１６Ａ，１６Ｂに示される位置Ｐ００９に到達する。

Ｓ３－６において、ロボット制御部１０７は、ハンド２２０の制御モードを、位置制御を実行する第二制御モードから、位置制御とともに力制御を実行する第三制御モードに切り替える。この場合、図１６Ｂ，１６Ｃに示されるように、Ｓ３－６において、対象物１０を容器１４０ｂの底面および側面に押し付けるため、ハンド２２０が、位置Ｐ００９から解放位置Ｐｒへ向けて斜め下方に移動するよう、制御される。当該制御において制御量が目標値に達したら、Ｓ３－７に移行する。

Ｓ３－７において、ロボット制御部１０７は、位置Ｐ００９から解放位置Ｐｒへ移動する途中の図１６Ｂ，１６Ｃに示される位置Ｐｃｂにおいて、言い換えると、解放位置Ｐｒの手前の位置Ｐｃｂに到達した時点で、ハンド２２０の制御モードを、位置制御とともに力制御を実行する第三制御モードから、力制御を実行する第一制御モードに切り替える。ロボット制御部１０７は、第一制御モードでの目標値を、図１６Ｃに示されるようなハンド２２０が把持した対象物１０の容器１４０ｂの底面および側面への押し付けが可能となるように設定する。具体的には、例えば、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、所定の押付力（例えば５Ｎに相当する大きさ）および容器１４０ｂの底面および側面に向かう図１６Ｃでは斜め下方向のベクトルの力目標値を設定し、押し付けない方向（斜め下方向の垂直方向）には力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値に０、または不感帯以下の値を設定したばね動作を実行し、Ｚ軸周りは力目標値を０とする積分動作を実行する。これにより、図１６Ｄに示されるように対象物１０が容器１４０ｂの側面と底面との間の隅に寄った解放位置Ｐｒに解放される。

次に、解放位置Ｐｒの周辺に容器１４０ｂの壁や障害物Ｏが多く存在する状況について説明する。ここで、仮に、図１６Ｅ～１６Ｈに示されるように、解放位置Ｐｒの両側に容器１４０ｂの側壁や障害物Ｏが存在している場合にあっては、カメラ３２０の情報などから、画像処理部１０２および統合部１０１が、周囲の状況を判断し、制御モードを切り替える区間（切替位置）を変更する。切り替えの判断は、例えば、ハンド２２０および把持した対象物１０の通過予定位置と、カメラ３２０から得られた距離画像における障害物Ｏの位置と、の比較から、両者の最短距離を計算し、当該最短距離が一定以上小さくなるなどの条件を与えることにより、実行することができる。図１６Ｅの例では、位置Ｐ００９は、図１６Ａとは異なる周囲の障害物Ｏから離れた位置に、設定されている。さらに、この条件では、図１６Ｂ，１６Ｃにおける斜め下方向の移動ができない。このような場合には、ロボット制御部１０７は、第一制御モードでの目標値を、図１６Ｆ，１６Ｇに示されるようなハンド２２０が把持した対象物１０の容器１４０ｂの底面に対する当該底面と垂直な方向への押し付けが可能となるように設定する。具体的には、例えば、並進成分（方向Ｘ，方向Ｙ，方向Ｚ）については、所定の押付力（例えば５Ｎに相当する大きさ）および容器１４０ｂの底面に向かう図１６Ｆ，１６Ｇでは下方向のベクトルの力目標値を設定し、押し付けない方向（方向Ｘ，方向Ｙ）には力目標値を０、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、Ｘ軸、Ｙ軸周りは力目標値に０、または不感帯以下の値を設定したばね動作を実行し、Ｚ軸周りは力目標値を０とする積分動作を実行する。また、この例では、容器１４０ｂの底面から上方に離れた位置が解放位置Ｐｒに設定される。この場合も、図１６Ｈに示されるように対象物１０が容器１４０ｂの底面上に載置される。

図１７Ａ～１７Ｄは、Ｓ３－６における位置Ｐ００９から解放位置Ｐｒへ向けて下方に移動する途中で対象物１０が障害物Ｏと干渉した場合の、ハンド２２０および対象物１０の状態を示す図である。このような事態は、例えば、物体ハンドリングシステム１が認識しているハンド２２０による対象物１０の把持位置または把持姿勢と、実際の把持位置または把持姿勢とが違う場合に、生じることがある。図１７Ｃに示されるように、位置Ｐ００９から解放位置Ｐｒへ行く途中、第三制御モードでの移動中に、対象物１０と障害物Ｏとが衝突している。この場合、衝突により、Ｘ軸、Ｙ軸回りにモーメントが発生する。ここで、第三制御モードでは、Ｘ軸およびＹ軸回りのモーメントは、目標値を０とするバネ動作に設定されているため、衝突により発生したモーメントは減殺され、ハンド２２０は、図１７Ｃに示されるように、ハンド２２０および対象物１０は傾く。さらに、この衝突により、方向Ｘおよび方向Ｙに力が生じるが、方向Ｘおよび方向Ｙについては、力目標値は０に設定されているため、衝突により受けた力は減殺され、図１７Ｄに示されるように、ハンド２２０および対象物１０は、障害物Ｏから離れるように図１７Ｄの左方へ移動する。その後、Ｘ軸およびＹ軸回りのモーメントは、バネ動作であるため、衝突による外力の入力が無くなると、ハンド２２０の姿勢は、当初の解放姿勢に戻る。また、ロボット制御部１０７は、方向Ｘおよび方向Ｙには、力の積分動作を実行するため、ハンド２２０は外力を受けない位置に平行移動し、当該位置を維持する。これにより、図１７Ｄに示されるように対象物１０が容器１４０ｂの側壁および障害物Ｏと干渉しない解放位置Ｐｒに解放される。このように、本実施形態では、物体ハンドリングシステム１は、各軸方向のパラメータにそれぞれ適切な値を設定することにより、容器１４０ｂ内の障害物Ｏ等の状況に応じて対象物１０を密着して配置することが可能となり、よりスペース効率の高い対象物１０の容器１４０ｂへの収納が実現されうる。

Ｓ３－８において、ロボット制御部１０７は、制御量が目標値に達したか否かを判断する。目標値に達していない場合、Ｓ３－９に移行する。制御量が目標値に達している場合には、Ｓ３－１０に移行する。

Ｓ３－９では、ロボット制御部１０７は、各パラメータの値がリミット（閾値）に達しているか否かを判定する。パラメータは、例えば、予め設定されている位置リミットや、所定時間を超えた処理を設定する時間リミット等である。リミットにより、マニピュレータ２００（ハンド２２０）が位置や姿勢の所定範囲を超えていないかを判定することができる。リミットに達した場合（Ｓ３－９でＹ）、一連の制御を終了する。リミットに達していない場合（Ｓ３－９でＮ）、Ｓ３－１０に移行する。

Ｓ３－１０では、ロボット制御部１０７は、対象物１０の解放をより確実に実行するため、ハンド２２０による解放状態を少なくとも所定時間以上維持する。Ｓ３－１０において、解放状態が所定時間以上継続できた場合には、解放動作が終了され、次の手順（例えば、ハンド２２０の位置Ｐ０１０への移動）に移行する。Ｓ３－１０において、解放状態が所定時間以上継続できなかった場合には、リカバリ動作等に移行する。

他方、Ｓ３－３において、接触するあるいは接触する可能性が高いと判断された場合（Ｓ３－３でＹ）、Ｓ３－１１において、ロボット制御部１０７は、位置Ｐ００８から、すなわち容器１４０ａ外から位置Ｐ００９へ、位置制御を実行する第二制御モードによらず、位置制御とともに力制御を実行する第三制御モードによって、ハンド２２０を移動させる。これにより、ハンド２２０が容器１４０ａや障害物Ｏと接触した場合にあっても、大きな接触力を発生させずに、接触点から逃げるような動作を実行することができる。ただし、第三制御モードによる移動距離が長くなると、所要時間が長くなる。よって、Ｓ３－１１では、Ｓ３－６よりも速度を高くしてもよい。Ｓ３－１１の後は、Ｓ３－５へ移行する。

以上のような解放動作時の制御により、対象物１０を把持したハンド２２０は、容器１４０ｂの壁や障害物Ｏとの干渉を回避しながら容器１４０ｂ外から解放位置Ｐｒへ移動することができる。また、力制御によって、対象物１０を解放位置Ｐｒの周辺の容器１４０ｂの壁や障害物Ｏに寄った状態（隣接した状態、密着した状態）で解放できる。これにより、解放位置Ｐｒ（容器１４０ｂ内）において対象物１０をより密集させることができる。

図１８は、種々の障害物Ｏに対応したハンド２２０の状態および移動方向を示す図である。図１８Ａには、対象物１０ではなくハンド２２０が障害物Ｏと干渉した場合が示されている。このような場合にあっても、ハンド２２０は、上述した第三制御モードあるいは第一制御モードで制御されることにより、図１７Ｃの場合と同様に、障害物Ｏを避けるように移動しながら解放位置Ｐｒへ移動することができる。図１８Ｂは、障害物Ｏが硬い場合を示している。この場合には、ロボット制御部１０７は、対象物１０を、障害物Ｏの側面および容器１４０ｂの底面に寄せた状態で、解放することができる。図１８Ｃ，１８Ｄの場合は、ロボット制御部１０７あるいは統合部１０１は、センサ３００の検出結果等に基づいて、対象物１０を障害物Ｏに押し付け難いと判断した場合、あるいは判断基準とするデータベースに、解放位置Ｐｒの周辺に位置された障害物Ｏの情報が登録されていなかった場合には、当該障害物Ｏから離れた解放位置Ｐｒにおいて、対象物１０を解放することができる。

また、ハンド２２０が仮置位置Ｐｔにある対象物１０を把持したり、ハンド２２０が仮置位置Ｐｔに対象物１０を解放したりする場合にあっても、上記実施形態と同様に、第一制御モード、第二制御モード、あるいは第三制御モードでハンド２２０を制御することができる。また、予め、仮置位置Ｐｔに容器の壁や障害物Ｏが存在していないことがわかっている場合や、画像処理部１０２、信号処理部１０３、あるいは統合部１０１が、センサ３００の検出結果に基づいて、仮置位置Ｐｔに容器の壁や障害物Ｏが存在していないことを判断した場合にあっては、ハンド２２０が位置制御（第二制御モード）によって移動する区間の長さを、把持位置Ｐｈや解放位置Ｐｒに移動する場合よりも長くすることができる。その場合、ハンド２２０が仮置位置Ｐｔへ近付く経路の移動時間をより短くすることができる。

上述した制御装置１００による演算処理や制御は、ソフトウエアによって実行されてもよいし、ハードウエアによって実行されてもよい。また、制御装置１００による演算処理や制御には、ソフトウエアによる演算処理や制御とハードウエアによる演算処理や制御とが含まれてもよい。ソフトウエアによる処理の場合にあっては、制御装置１００は、ＲＯＭや、ＨＤＤ、ＳＳＤ、フラッシュメモリ等の記録媒体（記憶媒体）に記憶されたプログラム（アプリケーション）を読み出して実行する。制御装置１００は、プログラムにしたがって作動することにより、制御装置１００に含まれる各部、すなわち、統合部１０１や、画像処理部１０２、信号処理部１０３、把持計画生成部１０４、解放計画生成部１０５、動作計画生成部１０６、ロボット制御部１０７、周辺機器・Ｉ／Ｏ制御部１０８、学習制御部１０９、エラー検出部１１０等として、機能する。この場合、プログラムには、上記各部に対応するモジュールが含まれる。

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

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

以上、説明したように、本実施形態では、ロボット制御部１０７（動作制御部）は、力（およびモーメント）を制御量とする第一制御モード、位置（および角度）を制御量とする第二制御モード、および力（およびモーメント）並びに位置（および角度）を制御量とする第三制御モードを切り替えて、ハンド２２０（マニピュレータ２００）を制御する。このような構成によれば、例えば、第一制御モードや第三制御モードにより、ハンド２２０が、対象物１０の把持面や、障害物Ｏ、容器１４０ｂの壁などに倣う制御や、所謂箱詰め制御が可能となる。また、干渉による衝撃力や押圧力を緩和することができ、ひいては対象物１０や、ハンド２２０、障害物Ｏ、容器１４０ａ，１４０ｂ等の保護性を高めることができる。また、第二制御モードや第三制御モードにより、より高速な移動が可能となる。このように、第一制御モードと当該第一制御モードとは異なる制御モードとを切り替えることにより、例えば、容器１４０ｂ内での対象物１０を含む取扱物品の集積度の向上や、保護性の向上、処理時間の短縮、等の種々の効果がより高いレベルで両立されやすい物体ハンドリングシステム１を得ることができる。

また、本実施形態では、ハンド２２０の移動経路は、ハンド２２０が容器１４０ａ，１４０ｂ内において第一制御モードで移動する第一区間（位置Ｐ００２または位置Ｐｃｂ～把持位置Ｐｈ、位置Ｐ００９または位置Ｐｃｂ～把持位置Ｐｈ）と、容器１４０ａ，１４０ｂ外において第二制御モードで移動する第二区間（位置Ｐ００３～位置Ｐ００８、位置０１０～位置Ｐ００１）と、を有する。このような構成によれば、例えば、ロボット制御部１０７は、対象物１０やハンド２２０がそれら以外の物体（例えば、障害物Ｏや容器１４０ａ，１４０ｂ）とより干渉しやすい容器１４０ａ，１４０ｂ内の第一区間では、力を制御する第一制御モードでハンド２２０を制御することにより、集積度の向上や保護性の向上を図ることができるとともに、対象物１０やハンド２２０がそれら以外の物体とより干渉し難い容器１４０ａ，１４０ｂ外の第二区間では、位置を制御する第二制御モードでハンド２２０を制御することにより、処理速度の向上を図ることができる。

また、本実施形態では、ハンド２２０の移動経路は、さらに、容器１４０ａ，１４０ｂ内において第三制御モードで移動する第三区間（位置Ｐ００２～位置Ｐｃａ、位置００９～位置Ｐｃｂ）を有する。このような構成によれば、例えば、ロボット制御部１０７は、容器１４０ａ，１４０ｂ内において、対象物１０やハンド２２０がそれら以外の物体と干渉する確率があるものの比較的（第一区間よりは）高くない第三区間では、位置および力を制御する第三制御モードでハンド２２０を制御することにより、容器１４０ａ，１４０ｂ内において、所要の保護性を確保しながら、処理速度の更なる向上を図ることができる。

また、本実施形態では、ロボット制御部１０７は、第二制御モードにおける移動速度を、場所により切り替えることができる。具体的には、容器１４０ａ，１４０ｂ内の第二制御モードによる第四区間（位置Ｐ００１～位置Ｐ００２、把持位置Ｐｈ～位置Ｐ００３、位置Ｐ００８～位置Ｐ００９、解放位置Ｐｒ～位置Ｐ０１０）における移動速度は、容器１４０ａ，１４０ｂ外の第二制御モードによる第二区間（位置Ｐ００３～位置Ｐ００８、位置０１０～位置Ｐ００１）における移動速度よりも低く設定されうる。このような構成によれば、例えば、所要の保護性を確保しながら、処理速度の更なる向上を図ることができる。

また、本実施形態では、重力補償部１０７ｄは、力センサ３１０の重力補償を、ハンド２２０に対する重力の作用状態が、把持姿勢あるいは解放姿勢（第一把持姿勢）と略等価となる姿勢（第二把持姿勢）で重力補償を実行する。このような構成によれば、例えば、ロボット制御部１０７は、力制御を実行する第一制御モードによるハンド２２０に作用する力（およびモーメント）をより高精度に検出することができ、ひいては、第一制御モードによる制御をより高精度に実行することができる。このような重力補償部１０７ｄにより得られる効果が得られるシステムは、上記物体ハンドリングシステム１には限定されない。

以上、本発明の実施形態を例示したが、上記実施形態は一例であって、発明の範囲を限定することは意図していない。上記実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、組み合わせ、変更を行うことができる。これら実施形態は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。また、各構成や形状等のスペック（構造や、種類、方向、形状、大きさ、長さ、幅、厚さ、高さ、数、配置、位置、材質等）は、適宜に変更して実施することができる。例えば、上記ハンド制御装置は、上述した一の容器内から別の容器内に対象物を搬送する物体ハンドリングシステム以外のハンドリングシステムにも適用可能である。

１０…対象物（物体）、１００…制御装置（ハンド制御装置）、１０３…信号処理部（検出値取得部）、１０４…把持計画生成部（計画生成部、把持姿勢決定部）、１０５…解放計画生成部（計画生成部、把持姿勢決定部）、１０６…動作計画生成部（計画生成部、把持姿勢決定部）、１０７…ロボット制御部（動作制御部）、１０７ｄ…重力補償部、１４０ａ，１４０ｂ…容器、Ｐｈ…把持位置（目標位置）、Ｐｒ…解放位置（目標位置）。

Claims (12)

1. 物体を把持するハンドの動作計画を生成する計画生成部と、
   前記ハンドに作用する力またはモーメントに基づいてハンドの動作を制御する第一制御モードと、前記第一制御モードとは異なる制御モードと、を切り替えて前記ハンドの動作を制御する、動作制御部と、
   前記ハンドに作用する力またはモーメントの検出値を取得する検出値取得部と、を備え、
   前記計画生成部は、前記第一制御モードにおいて前記ハンドが前記物体を把持する第一把持姿勢を決定し、
   前記第一把持姿勢における前記ハンドへの重力の作用状態と略等価な重力の作用状態となる第二把持姿勢で前記ハンドが前記物体を把持している状態において前記検出値取得部で取得された第一検出値に基づいて、前記検出値を補正する重力補償部をさらに備え、
   前記動作制御部は、補正された前記検出値に基づいて前記ハンドの動作を制御する、
   ハンド制御装置。
2. 前記第一制御モードにおいて、前記動作制御部は、目標位置へ移動するまでの間、前記ハンドの移動方向の反対方向に前記ハンドに作用する力が０より大きい力目標値を超えず、前記移動方向との直交方向に前記ハンドに作用する力が略０となり、前記移動方向の軸回りに前記ハンドに作用するモーメントが略０となり、かつ前記移動方向との直交方向の軸回りに前記ハンドに作用するモーメントがばね動作となるよう、前記ハンドの動作を制御する、請求項１に記載のハンド制御装置。
3. 前記目標位置は、前記ハンドが搬送する前記物体の把持位置である、請求項２に記載のハンド制御装置。
4. 前記目標位置は、前記ハンドが搬送した前記物体の解放位置である、請求項２に記載のハンド制御装置。
5. 前記動作制御部は、前記異なる制御モードとして前記ハンドの位置を制御する第二制御モードで前記ハンドの動作を制御する、請求項１～４のうちいずれか一つに記載のハンド制御装置。
6. 前記ハンドの移動経路が、容器内において前記動作制御部が前記第一制御モードで前記ハンドの動作を制御する第一区間と、前記容器外において前記動作制御部が前記第二制御モードで前記ハンドの動作を制御する第二区間と、を有した、請求項５に記載のハンド制御装置。
7. 前記動作制御部は、さらに、前記異なる制御モードとして前記ハンドの位置および前記ハンドに作用する力を制御する第三制御モードで、前記ハンドの動作を制御する、請求項５または６に記載のハンド制御装置。
8. 前記ハンドの移動経路が、容器内において前記動作制御部が前記第三制御モードで前記ハンドの動作を制御する第三区間を有した、請求項７に記載のハンド制御装置。
9. 物体を把持するハンドに作用する力またはモーメントの検出値を取得する検出値取得部と、
   前記ハンドの動作を制御する動作制御部と、
   前記ハンドが前記物体を搬送する所定区間において前記ハンドが前記物体を把持する第一把持姿勢を決定する把持姿勢決定部と、
   前記第一把持姿勢における前記ハンドへの重力の作用状態と略等価な重力の作用状態となる第二把持姿勢で前記ハンドが前記物体を把持している状態において前記検出値取得部で取得された第一検出値に基づいて、前記検出値を補正する重力補償部と、
   を備え、
   前記動作制御部は、補正された前記検出値に基づいて前記ハンドの動作を制御する、ハンド制御装置。
10. 前記ハンドに作用する力またはモーメントの検出値を取得する検出値取得部と、
    把持位置または解放位置の周辺状況を観測する観測部と、
    を備え、
    前記計画生成部は、前記観測部による前記把持位置または前記解放位置の観測結果にしたがい、前記第一制御モードと当該第一制御モードとは異なる制御モードとの切替位置を決定する、請求項１に記載のハンド制御装置。
11. 物体を把持するハンドの動作計画を生成する計画生成部と、
    前記ハンドに作用する力またはモーメントに基づいてハンドの動作を制御する第一制御モードと、前記第一制御モードとは異なる制御モードと、を切り替えて前記ハンドの動作を制御する、動作制御部と、
    前記ハンドに作用する力またはモーメントの検出値を取得する検出値取得部と、
    把持位置または解放位置の周辺状況を観測する観測部と、
    を備え、
    前記計画生成部は、前記観測部による前記把持位置または前記解放位置の観測結果にしたがい、前記第一制御モードにおいて、前記ハンドの移動方向の反対方向に前記ハンドに作用する力の目標値、前記移動方向との直交方向に前記ハンドに作用する力の目標値および動作モード、前記移動方向の軸回りに前記ハンドに作用するモーメントの目標値および動作モード、前記移動方向との直交方向の軸回りに前記ハンドに作用するモーメントの目標値および動作モードを決定し、
    前記動作制御部は、前記計画生成部において決定された前記目標値および前記動作モードにしたがって前記ハンドの動作を制御する、ハンド制御装置。
12. 物体を把持するハンドの動作計画を生成する計画生成部と、
    前記ハンドに作用する力またはモーメントに基づいてハンドの動作を制御する第一制御モードと、前記第一制御モードとは異なる制御モードと、を切り替えて前記ハンドの動作を制御する、動作制御部と、を備え、
    前記動作制御部は、前記異なる制御モードとして前記ハンドの位置を制御する第二制御モードで前記ハンドの動作を制御し、
    前記ハンドの移動経路が、容器内において前記動作制御部が前記第一制御モードで前記ハンドの動作を制御する第一区間と、前記容器外において前記動作制御部が前記第二制御モードで前記ハンドの動作を制御する第二区間と、を有した、
    ハンド制御装置。

Images