以下、本発明の例示的な実施形態が開示される。以下に示される実施形態の構成や制御(技術的特徴)、ならびに当該構成や制御によってもたらされる作用および結果(効果)は、一例である。
[システム全体構成]
図1は、物体ハンドリングシステム1の概略構成図である。物体ハンドリングシステム1は、マニピュレータ200、制御装置100、センサ300、および筐体400を、備えている。物体ハンドリングシステム1は、対象物10を把持位置Phで把持して搬送し、解放位置Prで解放する。このような動作は、対象物10の搬送、あるいは対象物10のピッキングと称されうる。制御装置100は、ハンド制御装置の一例である。対象物10は、搬送の対象としての物体の一例である。
マニピュレータ200は、アーム210と、ハンド220と、を備えている。アーム210は、例えば、複数(例えば6個)のサーボモータで回転駆動される回転部210a~210fを有した多関節ロボットである。アーム210は、これには限定されず、スカラロボットあるいは直動ロボットであってもよい。また、アーム210は、多関節ロボット、スカラロボット、および直動ロボットのうち少なくとも二つの組み合わせであってもよい。マニピュレータ200は、ロボットアームとも称されうる。
ハンド220は、一例としては、図1に示されるような吸着機構を有するが、これには限定されず、例えば、ジャミング機構、挟み込み機構、または多指機構を有してもよい。また、ハンド220は、吸着機構、ジャミング機構、挟み込み機構、および多指機構のうち少なくとも二つを有してもよい。なお、ハンド220は、エンドエフェクタとも称されうる。
図2A~2Dは、ハンド220の構成図である。図2A~2Dに示されるように、ハンド220は、それぞれ、力センサ310を有している。力センサ310の位置は、図2A~2Dには限定されない。また、ハンド220は、力センサ310以外の各種センサを有してもよい。力センサ310は、センサ300の一例である。
図2Aに示されるハンド220は、屈曲部220bを有している。ハンド220は、屈曲部220b以外にも、例えば、回転部や伸縮部のような可動部を有することができる。
図2A~2Cに示されるように、ハンド220は、吸着機構としての吸着パッド220aを有している。その場合、ハンド220は、例えば、吸着パッド220aの圧力センサを有してもよい。また、吸着パッド220aの数、配置、形状、大きさのようなスペックは、種々に変更することができる。
また、図2Dに示されるハンド220は、挟み込み機構あるいは多指機構としてのフィンガー220cを有している。
また、マニピュレータ200は、ハンド220を交換可能に構成されてもよい。その場合、ハンド220の交換には、ツールチェンジャーのような機構(不図示)が用いられてもよい。
センサ300は、力センサ310の他、回転部210a~210f(図1)に設けられるモータの、トルクセンサや、回転センサ、電流センサであってもよい。制御装置100は、ハンド220に作用する力を、力センサ310の検出値から得てもよいし、各モータの電流値や回転値(回転センサの検出値)から算出してもよい。
図3A~3Dは、図2A~2Dの各ハンド220が対象物10を把持した状態を示す図である。図3A~3D中の点Poは、ツールセンタポイントと称する。また、ツールセンタポイントPoを原点とし、ハンド220の把持基準軸Axと重なるZ軸と、当該Z軸と直交するX軸およびY軸と、を有する座標系を、手先座標と称する。なお、把持基準軸Axは、例えば、ハンド220を延び方向回りに回転させるハンド220に最も近い回転部(例えば、図1中の回転部210f)の中心軸であり、ハンド220が屈曲部220b(図3A)を有する場合にあっては、ハンド220の先端部220dの重心(または対称中心)を通り当該先端部220dの延び方向に沿う軸、あるいは屈曲部220bの伸長状態においてハンド220に最も近い回転部(例えば、図1中の回転部210f)の中心軸と重なる先端部220dの軸である。ハンド220は、例えば、把持基準軸AxがツールセンタポイントPoを通り、かつ当該ツールセンタポイントPoがハンド220による対象物10の接触領域の重心または中心となるよう、構成されるが、これには限定されない。なお、ツールセンタポイントPoは、コンプライアンスセンターとも称されうる。
また、図1に示されるように、物体ハンドリングシステム1は、複数のカメラ320を備えている。カメラ320は、センサ300の一例であり、画像センサとも称されうる。カメラ320は、例えば、RGB画像カメラや、距離画像カメラ、レーザーレンジファインダ、laser imaging detection and ranging(LiDAR)など、画像情報や距離情報(3次元情報)を取得可能なセンサである。カメラ320は、観測部の一例である。
物体ハンドリングシステム1は、カメラ320として、例えば、把持用のカメラ321、把持状態確認用あるいはキャリブレーション用のカメラ322、解放用のカメラ323、および一時載置用のカメラ324を、備えている。
把持用のカメラ321は、把持(搬出)する対象物10が存在している把持位置Phにおいて、対象物10およびその周辺領域を撮影かつ検出する。当該把持位置Phにおいて、対象物10は、例えばコンテナやパレットのような把持用の容器140a内に収容されている。その場合、把持用のカメラ321は、容器140a内の一部または全部を撮影かつ検出する。把持位置Phは、対象物10の移動開始位置や出発位置とも称されうる。なお、図1では、容器140aは、ベルトコンベアのような搬送機構上に載置されているが、これには限定されない。
キャリブレーション用のカメラ322は、把持用の容器140a外であるとともに把持位置Phよりも上方に設定された対象物10のキャリブレーション位置において、対象物10を撮影かつ検出する。
解放用のカメラ323は、対象物10を解放(搬入)する解放位置Prにおいて、当該解放位置Prおよびその周辺領域を撮影かつ検出する。解放位置Prにおいて、対象物10は、例えばコンテナやパレットのような解放用の容器140b内に収容される。その場合、解放用のカメラ323は、容器140b内の一部または全部を撮影かつ検出する。解放位置Prは、対象物10の移動終了位置や到着位置とも称されうる。なお、図1では、容器140bは、ベルトコンベアのような搬送機構上に載置されているが、これには限定されない。
仮置き用のカメラ324は、把持位置Phと解放位置Prとの間で対象物10を一時的に置いておく仮置位置Ptおよびその周辺領域を撮影かつ検出する。仮置位置Ptにおいて、対象物10は、例えばテーブルやステージのような仮置き面150c上に置かれる。その場合、仮置き用のカメラ324は、仮置き面150cの一部または全部を撮影かつ検出する。仮置位置Ptは、対象物10の途中位置や経由位置とも称されうる。なお、仮置位置Ptは、載置面で無くてもよい。
筐体400内には、例えば、モータのような電動アクチュエータを駆動させるための電源装置、流体アクチュエータを駆動させるためのボンベ、タンク、およびコンプレッサー、各種安全機構など、物体ハンドリングシステム1の種々の部品や機器類が収容されうる。なお、筐体400内には、制御装置100が収容されてもよい。
[制御装置]
図4は、制御装置100のブロック図である。制御装置100は、例えば、統合部101、画像処理部102、信号処理部103、把持計画生成部104、解放計画生成部105、動作計画生成部106、ロボット制御部107、周辺機器・I/O制御部108(I/O:インプットアウトプット)、学習制御部109、エラー検出部110、および内部DB120を有している。
統合部101は、外部I/F130(I/F:インタフェース)からのユーザー入力情報や、物体ハンドリングシステム1の状態、センサ300による検出値に基づいて、物体ハンドリングシステム1の作業計画の生成や、運用、管理を実行する。
画像処理部102は、カメラ320から得られた画像や情報(検出値)を処理し、動作計画や、動作制御、エラー検出、学習等に必要な情報を生成する。
信号処理部103は、センサ300から得られた情報(検出値)を処理し、動作計画や、動作制御、エラー検出、等に必要な情報を生成する。信号処理部103は、検出値取得部の一例である。
把持計画生成部104は、マニピュレータ200による対象物10の把持方法や、把持位置Phや把持姿勢、必要な個所が周囲環境に干渉することなく把持位置Phおよび把持姿勢に移動可能な経路および経由点を算出する処理を、実行する。把持計画生成部104は、計画生成部の一例であり、把持姿勢決定部の一例でもある。また、把持姿勢は、第一把持姿勢の一例である。ここで、必要な個所とは、例えば、マニピュレータ200の全体やアーム210等の一部である。一方で、本実施形態の物体ハンドリングシステム1では、力の制御が可能であるため、吸着パッドや弾性関節部などある程度の柔軟性を持つ部分に関しては、多少の接触を許容してもよい。
解放計画生成部105は、マニピュレータ200が把持した対象物10を容器140bに箱詰めしたり設置したりする場所における例えば押し付けのような設置方法や、把持方法、解放方法、解放位置Prや解放姿勢、必要な個所が周囲環境に干渉することなく解放位置Prおよび解放姿勢に移動可能な経路および経由点を算出する処理を、実行する。解放計画生成部105は、計画生成部の一例である。また、解放姿勢は、解放する直前までの把持姿勢と略同じであるから、解放計画生成部105も、把持姿勢決定部の一例であると言うことができる。また、解放姿勢は、第一把持姿勢の一例であると言うことができる。
動作計画生成部106は、統合部101からの指示に従って、マニピュレータ200が現在位置から移動可能な経路(経由点)を経由して把持位置Phや、仮置位置Pt、解放位置Pr等に移動するよう、動作方法や、動作速度、動作経路等のロボット動作情報の算出を実行する。動作計画生成部106は、計画生成部の一例である。
ロボット制御部107は、把持計画生成部104、解放計画生成部105、あるいは動作計画生成部106で生成されたロボット動作情報および統合部101からの各種の動作切り替え指示等に従って、マニピュレータ200を含む物体ハンドリングシステム1を制御する。また、ロボット制御部107は、統合部101から取得した各種姿勢情報や軌道計画情報等に従い、マニピュレータ200を制御する。ロボット制御部107は、動作制御部の一例である。
周辺機器・I/O制御部108は、各種搬送機器の制御や、安全ドアなどの周辺機器500の制御、各種センサ情報の取得、照明の制御等のための、インプットアウトプット(I/O)制御を実行する。
学習制御部109は、マニピュレータ200の振動抑制など動作精度向上のためのロボットモデル学習や、対象物10の把持性能向上のための把持制御パラメータ学習、把持データベース学習、作業計画の実施性能を向上のためのエラー検知学習、等の学習機能を制御する。本実施形態では、状況に応じて、後に詳しく述べる力制御において、パラメータの最適な値を選択する。これらの値は人間が経験値から設定してもよいが、機械学習の結果を適用することで、より省力化しながら、より効率の高い処理を実現することができる。
エラー検出部110は、物体ハンドリングシステム1の状態、作業計画の実施状態、駆動制御状態、対象物10の把持、搬送の状態等を観測し、エラーを検知する。このエラー検知は、例えば、力センサ310や力センサ310の出力を手先座標に変換した値、それらの値をローパスフィルタに通したものを観測し、既定の値を超えたらエラーと判断することで実現できる。この結果、処理中の作業を中断し、その後リカバリ動作に移るなどの処理が可能となる。
内部DB120(DB:データベース)は、例えば、ロボットデータベース、ハンドデータベース、物品データベース、把持データベース、および環境データベース(いずれも不図示)を含んでいる。
ロボットデータベース(ロボットDB)には、例えば、物体ハンドリングシステム1の構造、各部の寸法や重量、慣性モーメント、各駆動部の動作範囲や速度、およびトルク性能が、保存されている。
ハンドデータベース(ハンド220DB)には、例えば、ハンド220の機能、およびハンド220の把持の特性に関する情報が、保存されている。
物品データベース(物品DB)には、例えば、対象物10の名称、識別番号、カテゴリ、全面の画像情報、CADモデル情報、重量情報、および把持時の特性情報(柔らかい、壊れやすい、形が変わる等)が、保存されている。
把持データベース(把持DB)には、例えば、対象物10について、ハンド220の把持方法ごとに、把持可能位置、把持可能姿勢、把持のしやすさなどのスコア情報、把持時の押し込み可能量、把持判定のための判定閾値、およびエラー検知のための判定閾値が、保存されている。ここで、把持方法は、例えば、吸着方式、並行2指方式、平行4指方式、または多関節方式である。
環境データベース(環境DB)には、例えば、物体ハンドリングシステム1が対応している作業台情報、および物体ハンドリングシステム1の動作範囲や周囲の障害物Oを示すような周囲環境情報が、保存されている。
また、外部I/F130は、統合部101(制御装置100)と外部機器(不図示)との間のデータの送信および受信を実行する。
[ピッキング処理の手順]
図5は、制御装置100が実行する対象物10を把持し、移動して、解放する手順を示すフローチャートである。
まず、S1-1において、統合部101は、外部I/F130を経て、外部機器から対象物10の搬送指示(ピッキング指示)を受け取る。
S1-2において、統合部101は、カメラ321の撮影した画像や、光電センサやマイクロスイッチのような他のセンサ300の検出値に基づいて、対象物10または当該対象物10を収容した容器140aが把持位置Phに到達したことを検出する。
S1-3において、統合部101は、対象物10の把持位置Phおよびその周辺、例えば対象物10が収容されている容器140a内を、撮像するよう、カメラ321を制御する。
S1-4において、画像処理部102は、カメラ321が撮影した画像から、対象物10の有無の判定、対象物10の把持可能面の識別を実行する。さらに、画像処理部102は、対象物10の把持可能面の形状や大きさ、3次元空間内での位置や姿勢等の把持面情報を算出する。
S1-5において、把持計画生成部104は、把持面情報に基づいて、マニピュレータ200が対象物10の把持可能面を把持するための複数の把持情報を算出する。把持情報には、把持位置情報や把持姿勢情報が含まれる。ここで、把持計画生成部104は、容器140a内の対象物10以外の物品(周辺部品)などの把持位置Phの周辺情報や、把持面情報や物品DBの情報に基づいて、例えば、後述の力制御を実行する際のハンド220の移動方向や、押付力の大きさ、移動許容範囲のような付加情報を算出し、これを把持情報に付加する。
S1-6において、統合部101は、把持情報に基づいて、把持対象とする対象物10、その把持位置Ph、および把持姿勢を、選択あるいは決定する。そして、動作計画生成部106は、マニピュレータ200の現在位置から、把持位置Phおよび把持姿勢に至るロボット動作情報を生成する。ロボット動作情報の生成については、例えば、マニピュレータ200と対象物10以外の物体との干渉あるいは衝突の虞が無いあるいは確率が比較的低い領域では、高速な動作が選択される。他方、例えば、ハンド220が容器140aに近付いたり容器140a内に入ったりするなど、ハンド220または対象物10と当該対象物10以外の物体との干渉あるいは衝突の虞があるあるいは確率が比較的高い領域では、速度を抑えた動作が選択される。さらに、動作計画生成部106は、この領域でハンド220に発生する力の目標値を決定する。
S1-7において、ロボット制御部107は、生成されたロボット動作情報に従って動作し、把持位置Phおよび把持姿勢で対象物10を把持するよう、マニピュレータ200およびハンド220を制御する。対象物10の把持位置Phの周辺領域では、力制御のみによるハンド220の押付動作が実行され、吸着パッド220aやフィンガー220cが対象物10に十分に接触した状態で、吸着や挟み込み等の把持動作が実行される。把持位置Phの周辺領域では、ハンド220が、対象物10以外の物体のような障害物Oに当たった場合には、与えられた動作方式や力の目標値に従って、ハンド220が障害物Oから逃げる動作等が実行される。また、対象物10の把持面が比較的大きな傾きを持っているような場合にあっても、与えられた押付方法や押付力により、ハンド220を対象物10の把持面に倣わせる動作が実行される。
統合部101またはロボット制御部107は、マニピュレータ200やハンド220の動作や把持の状態を監視している。S1-8において、把持の成功が確認された場合には(S1-8でY)、処理はS1-10に移行し、把持の失敗が確認された場合には(S1-8でN)、処理はS1-9に移行する。
S1-9では、統合部101は、マニピュレータ200の退避動作や、把持の失敗を示す情報を物品DBに登録する登録処理などのリトライ準備動作を実行する。S1-9の後、処理はS1-3に移行する。
S1-10において、ロボット制御部107は、ハンド220による対象物10の把持状態が撮影可能な位置に移動するよう、マニピュレータ200を制御する。また、統合部101は、当該位置で、対象物10を把持したハンド220を撮影するよう、カメラ322を制御する。
S1-11において、画像処理部102は、カメラ322が撮影した画像から、例えば、ハンド220に対する対象物10の相対位置、ハンド220に対する対象物10の相対姿勢、および対象物10の状態や形状のような、対象物10情報を生成する。
S1-12において、統合部101は、対象物10の解放位置Prおよびその周辺、例えば把持されている対象物10が解放され収容される容器140b内を、撮影するよう、カメラ323を制御する。また、力センサ310のリセット等の重力補償動作が実施される。ここで、既に次の解放姿勢が決定している場合には、ロボット制御部107は、ハンド220が、解放姿勢と略同じかあるいは近いキャリブレーション姿勢となるよう、マニピュレータ200およびハンド220を制御し、この状態で、重力補償部107dは、重力補償を実行する。これにより、より精度の高い重力補償動作を実施することができる。この際、合わせて力センサ310による対象物10の重量計測を実施しても良い。
S1-13において、画像処理部102は、カメラ323が撮影した画像から、解放位置Prおよびその周辺における、対象物10以外の物体の有無の判定、当該物体の大きさや、位置、当該物体の表面の位置等の、周辺情報を生成する。解放位置Prの周辺情報は、障害物情報とも称されうる。
S1-14において、解放計画生成部105は、対象物10情報、把持DBの情報、周辺情報等に基づいて、マニピュレータ200が把持している対象物10の解放情報を生成する。解放情報には、解放位置情報や、解放姿勢情報、経由点位置情報、経由点姿勢情報等が含まれる。ここで、解放計画生成部105は、力制御を伴うハンド220の押付方向や押付力の大きさ、力制御の移動許容範囲等の付加情報を算出し、これを解放情報に付加する。
S1-15において、統合部101は、解放情報に基づいて、把持している対象物10の解放位置Prおよび解放姿勢を、決定する。そして、動作計画生成部106は、マニピュレータ200の現在位置から、解放位置Prおよび解放姿勢に至るロボット動作情報を生成する。この場合も、ロボット動作情報の生成については、例えば、マニピュレータ200と対象物10以外の物体との干渉あるいは衝突の虞が無いあるいは確率が比較的低い領域では、高速な動作が選択される。他方、例えば、ハンド220が容器140bに近付いたり容器140b内に入ったりするなど、ハンド220または対象物10と当該対象物10以外の物体との干渉あるいは衝突の虞があるあるいは確率が比較的高い領域では、速度を抑えた動作が選択される。さらに、動作計画生成部106は、この領域でハンド220に発生する力の目標値を決定する。
S1-16において、ロボット制御部107は、生成されたロボット動作情報に従って動作し、解放位置Prへ対象物10を移動させ当該解放位置Prで対象物10を解放するよう、マニピュレータ200およびハンド220を制御する。対象物10の解放位置Prの周辺領域では、対象物10を容器140bにより高密度に配置するために、容器140bの壁や、既に置かれている十分な大きさの物品がある場合には、ハンド220により当該壁や物品の被押付面に対して対象物10を押し付ける動作が、実行される。また、被押付面が比較的大きな傾きを持っているような場合にあっても、与えられた押付方法や押付力により、対象物10を被押付面に倣わせて、密着した状態で解放する動作が実行される。
S1-17では、対象物10の解放後、ロボット制御部107は、容器140bから脱出するようハンド220を制御し、待機姿勢をとるようマニピュレータ200を制御する。
S1-18において、重力補償部107dは、力センサ310の検出値のリセット(この状態での検出値を0とする)のような、重力補償を実行する。ここで、次の対象物10の把持姿勢が既に決定している場合には、ロボット制御部107は、ハンド220が、把持姿勢と略同じかあるいは近いキャリブレーション姿勢となるよう、マニピュレータ200およびハンド220を制御し、この状態で、重力補償部107dは、重力補償を実行する。
S1-19において、統合部101は、外部I/F130を経て、外部機器から次の対象物10の搬送指示(ピッキング指示)を受け取った場合には、S1-2へ移行し、次の対象物10に対する一連の制御を開始する(S1-19でY)。次の対象物10が無い場合には、一連の制御を終了する(S1-19でN)。
[ハンドの移動経路]
図6は、ピッキング動作中におけるハンド220のツールセンタポイントPcの経路を示す。
位置P000は、初期位置、もしくは待機位置である。ピッキング処理が実行されていない場合や、次の指示を待機する際には、ハンド220は、この位置P000で待機する。また、位置P000において重力補償を実行することにより、他の位置で改めて重力補償を実行する場合に比べてピッキングの処理時間を短縮することが可能である。
位置P001は、容器140aの外側に位置されている。位置P001において、ハンド220の姿勢は、把持姿勢または把持姿勢に近い姿勢に設定される。例えば、屈曲部220bを持つようなハンド220では、この位置P001において屈曲部220bの傾きを把持位置Phでの把持姿勢と同じ傾きに変更することができる。これにより、容器140a内でハンド220の姿勢を変化させる場合に比べて、より衝突の危険を低減することができる。また、位置P001では、力センサ310の重力補償が実行されてもよい。これにより、後述の位置P002および把持位置Phの周辺でのハンド220の対象物10への押し付け、すなわちタッチ動作を実行する際に、より精度の高い力制御を実現することができる。なお、ハンド220は、位置P000から位置P001へは、例えば、位置(および姿勢)を制御量とする制御モード(第二制御モード)によって移動する。第二制御モードは、位置制御とも称されうる。また、姿勢は、例えば、把持基準軸Ax回りの角度あるいは回転角度とも称されうる。
位置P002は把持位置Phの直前の位置である。位置P002は、一例としては、把持位置Phおよび把持姿勢から、ハンド220の押付方向の逆側に数cm離れた位置である。位置P002でのハンド220の姿勢は把持位置Phでの姿勢とほぼ同じであってもよい。なお、ハンド220は、位置P001から位置P002へは、例えば、移動速度が比較的低い状態での位置を制御量とする制御(第二制御モード)によって移動してもよいし、力による退避動作が可能となるよう、位置に加えて力(およびモーメント)を制御量とする制御(第三制御モード)によって移動してもよい。第三制御モードは、位置・力制御とも称されうる。また、モーメントは、例えば、把持基準軸Ax回りの回転モーメントとも称されうる。
把持位置Phは、指定された(演算された、推定された、あるいは決定された)把持位置Phである。把持位置Phの周辺(近傍)において、ハンド220は、例えば、力制御によって制御される。これにより、対象物10との接触力を制御することができ、より確実性の高い把持が実現されやすい。なお、ハンド220は、位置P002から把持位置Phへは、例えば、位置および力を制御量とする制御(第三制御モード)か、あるいは力(およびモーメント)のみを制御量とする制御(第一制御モード)で移動する。この場合、位置P002から位置P002と把持位置Phとの間の位置Pcaまでの区間を第三制御モードにより制御し、位置Pcaから把持位置Phまでを第一制御モードにより制御してもよい。第一制御モードは、力制御モードとも称されうる。把持位置Phは、目標位置の一例である。また、位置Pcaは切替位置とも称されうる。
把持位置Ph’は、実際にハンド220による対象物10の把持が実行された位置、または実行されたと判定された位置である。力制御が行われた場合において、例えば、対象物10が実際には把持位置Phよりも手前側にあった場合には、ハンド220は対象物10との接触力の増大に応じて、把持位置Phよりも手前で停止する。このような場合、把持位置Ph’と把持位置Phとは互いに異なることになる。また、対象物10の把持面の実際の傾きが、指定された把持位置Phにおける把持面の傾きとは異なっていた場合、ハンド220は、力制御による倣い動作によって、実際の把持面に倣うことができる。また、ハンド220が把持位置Phに到達した場合にあっても、対象物10が推定された位置からずれていたような場合、ハンド220と対象物10との接触が発生せず、指定された力に達していない場合には、ハンド220は力制御によってさらに移動し、移動許容リミットに達するか、把持位置Phからずれていた把持面に接触して、接触力が指定された力に達した時点で停止する。このような場合にも、把持位置Phと把持位置Ph’とは異なる位置となる。また、ハンド220が位置P002と把持位置Phとの間で、例えば障害物Oのような対象物10とは異なる物体と接触し、指定された力目標を超えないように逃げるような動作をした場合にあっても、把持位置Phと把持位置Ph’とは異なる位置となる。
位置P003は容器140aの外側に位置される。位置P003において、ハンド220の姿勢は、把持位置Phまたは把持位置Ph’での把持姿勢と略同じに設定される。把持位置Ph’と位置P003との間では、対象物10やハンド220は、他の位置に比べて、周囲の物体に接触しやすい。このため、ハンド220は、位置P003へは、なるべく姿勢を変化させずに、容器140a内から脱出する。この場合、力制御はなるべく抑制され、位置制御によって、ハンド220は移動する。
位置P004では、ハンド220の姿勢は、重力に対して対象物10の把持をなるべく維持しやすい姿勢に設定される。対象物10の把持を維持しやすい姿勢とは、例えば、吸着方式の場合には吸着パッド220aが下向きになる姿勢であり、挟み込み方式の場合にはフィンガー220cが横方向に対して少し下側を向く姿勢である。
位置P005および位置P006への移動区間において、ハンド220は、把持状態の撮影に適した撮影姿勢に設定される。撮影姿勢は、言い換えると、把持した対象物10を計測するためのカメラ322の撮像精度が高くなる姿勢である。この区間では撮影のための正確な位置情報が必要であるため、力制御をおこなわず、位置制御を実行する。また、この移動区間において、ハンド220の姿勢を変化させながら、力センサ310の重力補償を実行することができる。これにより、より精度の高い重力補償を実現できる。
位置P006と位置P007は待機位置である。容器140bの撮像や、解放計画の作成、解放に伴う動作情報の決定などが完了するまで、ハンド220は、位置P006または位置P007で待機する。また、このような待機位置において力センサ310の重力補償を実行することにより、他の位置で改めて重力補償を実行する場合に比べてピッキングの処理時間を短縮することが可能である。さらに、位置P006または位置P007において、解放姿勢、すなわち解放する直前の把持姿勢と等価な重力が作用するキャリブレーション姿勢で重力補償が実行されることにより、解放時の力制御をより高精度に実行することが可能となる。解放姿勢(解放する直前の把持姿勢)は、第一把持姿勢の一例であり、キャリブレーション姿勢は、第二把持姿勢の一例である。
位置P008は容器140bから直上に外れた位置である。位置P008において、ハンド220の姿勢は、解放姿勢または解放姿勢に近い姿勢に設定される。例えば、屈曲部220bを持つようなハンド220では、この位置P008において屈曲部220bの傾きを解放位置Prでの解放姿勢と同じ傾きに変更することができる。これにより、容器140b内でハンド220の姿勢を変化させる場合に比べて、より衝突の危険を低減することができる。なお、ハンド220は、位置P006から位置P008へは、例えば、位置を制御量とする制御(第二制御モード)によって移動する。
位置P009は解放位置Pr直前の位置である。位置P009は、一例としては、解放位置Prおよび解放姿勢から、ハンド220の押付方向の逆側に数cm~十数cm離れた位置である。位置P009でのハンド220の姿勢は、解放位置Prでの姿勢とほぼ同じであってもよい。なお、ハンド220は、位置P008から位置P009へは、例えば、移動速度が比較的低い状態での位置を制御量とする制御(第二制御モード)によって移動してもよいし、力による退避動作が可能となるよう、位置に加えて力を制御量とする制御(第三制御モード)によって移動してもよい。
解放位置Prは、指定された(演算された、推定された、あるいは決定された)解放位置Prである。解放位置Prの周辺(近傍)において、ハンド220は、例えば、力制御によって制御される。これにより、既に置かれた物品など周囲の環境との接触力を制御することができ、より確実性の高い解放が実現されやすい。なお、ハンド220は、位置P009から解放位置Prへの移動は、例えば、位置および力を制御量とする制御(第三制御モード)か、あるいは力のみを制御量とする制御(第一制御モード)で移動する。この場合、位置P009から当該位置P009と解放位置Prとの間の位置Pcbまでの区間を第三制御モードにより制御し、位置Pcbから解放位置Prまでを第一制御モードにより制御してもよい。解放位置Prは、目標位置の一例である。また、位置Pcaは切替位置とも称されうる。
解放位置Pr’は、実際にハンド220による対象物10の解放が実行された位置、または実行されたと判定された位置である。力による制御が行われた場合において、例えば、壁や既に置かれている物品のようなストッパが実際には解放位置Prよりも手前側にあった場合には、ハンド220は障害物O(周辺物体、隣接物体、先行物体)との接触力の増大に応じて、解放位置Prよりも手前で停止する。このような場合、解放位置Pr’と解放位置Prとは互いに異なることになる。また、解放位置Pr’近傍のストッパの被押付面の実際の傾きが、指定された解放位置Prにおける被押付面の傾きとは異なっていた場合、ハンド220は、力制御による倣い動作によって、実際の被押付面に倣うことができる。また、ハンド220が解放位置Prに到達した場合にあっても、ストッパが推定された位置からずれていたような場合、ハンド220とストッパとの接触が発生せず、指定された力に達していない場合には、ハンド220は力制御によってさらに移動し、移動許容リミットに達するか、解放位置Prからずれていた被押付面に接触して、接触力が指定された力に達した時点で停止する。このような場合にも、解放位置Prと解放位置Pr’とは異なる位置となる。また、ハンド220が位置P009と解放位置Prとの間で、例えば障害物Oのような物体と接触し、指定された力目標を超えないように逃げるような動作をした場合にあっても、解放位置Prと解放位置Pr’とは異なる位置となる。
位置P010は容器140bの外側に位置される。位置P010において、ハンド220の姿勢は、解放位置Prまたは解放位置Pr’での解放姿勢と略同じに設定される。解放位置Prと解放位置Pr’との間では、対象物10やハンド220は、他の位置に比べて、周囲の物体に接触しやすい。このため、ハンド220は、位置P010へは、なるべく姿勢を変化させずに、容器140b内から脱出する。この場合、位置制御によって、ハンド220はより高速に移動する。
また、ハンド220が位置P005または位置P006から解放位置Pr’を経由した位置P000までの区間においては、カメラ321により、ハンド220や対象物10が障害となることなく、容器140a内の撮影が可能であり、ハンド220が位置P011から把持位置Ph’を経由した位置P007までの区間においては、カメラ321により、ハンド220や対象物10が障害となることなく、容器140b内の撮影が可能である。また、本実施形態では、図6に示されるハンド220の経路の任意の位置において、力センサ310の検出値に基づいて、ハンド220と周囲環境との衝突の有無を検知することができる。
また、重力補償部107dは、ハンド220の経路内の、実際に押付を実行する解放位置Prに対して位置制御による移動区間(位置P005から位置P009)を挟んだ異なる位置P005,P006において、力センサ310の重力補償を実行することができる。これにより、経路から外れた別の位置で力センサ310の重力補償を実行する場合に比べて、ピッキングの処理時間を短縮することが可能である。
このように、本実施形態の物体ハンドリングシステム1では、ピッキング処理おける把持や箱詰め動作の際に、経路の状況に応じて、位置制御や、力制御、および位置制御および力制御を並行して実行するハイブリッド制御を切り替えることにより、より効率良くピッキング処理を実行することができる。また、種々の状況に応じて、より適切な位置や、時刻、ハンド220の姿勢において、力センサ310の重力補償動作を実行することにより、より精度の高い力制御動作を実現することができる。
図6に例示される経路では、例えば、容器140a内の位置P002または位置Pcaから把持位置Phまでの区間および容器140b内の位置P009または位置Pcbから解放位置Prまでの区間が、第一制御モードによる第一区間の一例であり、容器140a,140b外の位置P003から位置P008までの区間および位置P010から位置P001までの区間が、第二制御モードによる第二区間の一例である。また、容器140a内の位置P002から位置Pcaまでの区間および容器140b内の位置P009から位置Pcbまでの区間が、第三制御モードによる第三区間の一例である。また、位置P001から位置P002までの区間、把持位置Phから位置P003までの区間、位置P008から位置P009までの区間、および解放位置Prから位置P010までの区間の、容器140a,140b内を通る第四区間にあっては、位置制御を含む第二制御モードまたは第三制御モードによりハンド220の制御が実行されるが、これら区間におけるハンド220の移動速度は、容器140a,140b外の第二制御モードによる第二区間でのハンド220の移動速度よりも低く設定される。
[力制御]
図7Aは、力制御による対象物10の解放位置Prへの移動を示す斜視図であって、対象物10を解放位置Prの周辺に存在する部品(障害物O)に押し付ける場合を示す図、図7Bは、力制御による対象物10の解放位置Prへの移動を示す側面図であって、対象物10を解放位置Prの周辺に存在する部品(障害物O)の斜面に沿って押し付ける場合を示す図である。
ここでは、特にハンド220により対象物10を容器140b内に隙間なく詰める場合、所謂箱詰めの場合における、ロボット制御部107の力制御系(以下、単に力制御系と称する)の構成および動作について説明する。力制御系は、ロボット制御部107の位置制御系(以下、単に位置制御系と称する)に対し、マニピュレータ200の手首部(具体的には回転部210fとハンド220との間)に配置された力センサ310の検出値をフィードバックし、リアルタイムで位置補正を実行する。力センサ310は、例えば、6軸力覚センサであり、3方向の力と3方向を軸とする回転モーメントを検出する。力制御系は、具体的には、箱詰め動作において、対象物10を密着させる面に対して当該対象物10を押し付けることが可能となる長めの位置目標値軌道を生成しておき、対象物10が被押付面に接触したときに倣うように、言い換えると力を逃がすように、制御する。倣う方向としては、並進の3自由度と鉛直軸回りの姿勢の1自由度としておけば、図7A,7Bの密着動作に対応することができる。姿勢の残りの2自由度については、後述するバネ動作として対象物10を把持するハンド220の姿勢が変化したり対象物10がずれたりしないようにする。このように、本実施形態の物体ハンドリングシステム1は、容器140a,140bのような、ハンド220の挿入先(箱詰め先)の状態によって各軸の力制御の方式を切り替えることにより、効果的な把持や箱詰めを実現することができる。
図8は、ロボット制御部107のブロック図である。本実施形態では、一例として、ロボット制御部107は、位置制御系をベースとした力制御系のブロック構成を有している。力センサ310が手首部にあるため、力制御系は手先座標系でフィードバックループを構成し、マニピュレータ200のベース座標系への座標変換をリアルタイムに実行しながら、関節座標系での位置制御を実行する。ロボット制御部107は、手先座標力制御部107a、逆運動学演算部107b、関節座標位置制御部107c、および重力補償部107d、を有している。手先座標力制御部107aは手先位置の目標値、すなわちマニピュレータ200(ハンド220)のツールセンタポイントPoの位置および姿勢の目標値と力制御に係るパラメータおよび力センサ310の検出値に基づく力情報を受け取り、手先位置の指令値を逆運動学演算部107bに出力する。逆運動学演算部107bは、一般に逆運動学演算(インバースキネマティクス)と呼ばれる演算を行い、ツールセンタポイントPoの位置および姿勢の情報から、当該位置および姿勢を実現するマニピュレータ200の各関節軸の回転位置を算出する。関節座標位置制御部107cは、逆運動学演算部107bから出力された各関節軸の回転位置指令値と、マニピュレータ200の各関節軸の関節位置情報を受け取り、関節位置指令値を出力する。関節座標位置制御部107cは、関節位置を所定の目標値に維持するようフィードバック制御を実行する。重力補償部107dは、ハンド220が所定の位置および姿勢にある状態で、対象物10の重力補償のため、力センサ310のキャリブレーションを実行する。キャリブレーションが実行される位置は、ハンド220の経路の途中に組み込まれる。また、キャリブレーションは、ハンド220が対象物10を把持した状態で、対象物10を把持する直前の姿勢と略同じ姿勢、または対象物10を解放する直前の姿勢と略同じ姿勢において、実行される。
図9は、手先座標力制御部107aのブロック図である。手先座標力制御部107aは、手先座標系における力・モーメント目標値と検出値との差を力フィードバック制御部107a2で演算して微小変位・回転角(後述)を算出し、位置目標値をリアルタイム(サンプル周期は数ms毎)で修正することにより、外力・モーメントに応じてハンド220または対象物10を被押付面に倣わせる。ツールセンタポイントPoは、力・モーメント制御(力制御)の作用点であり、手先座標系の原点であって、上述したように、吸着方式の場合、吸着パッド220aの中心に設定される。ツールセンタポイントPoでは、並進の力が加わると並進動作だけが生じ、モーメントが加わると回転動作だけが生じる。ここで、力・モーメントの検出値を力センサ310の座標系から手先座標系へ変換する必要がある。手先座標力制御部107aは、管理部107a1、力フィードバック制御部107a2、変換部107a3、および手先位置制御部107a4を有している。管理部107a1は、入力された力制御モードの指令に従って、力フィードバック制御部107a2のモード切り替パラメータや変換切り替えパラメータを設定する。さらに、各軸の力・モーメントの目標値を設定する。ここで、力フィードバック制御部107a2のモード切替とは、各軸の力制御のオンオフや、積分動作、ばね動作などの切り替え、経路の状況による制御ゲインの切り替え等を意味する。これは、力制御の動作モードの切り替えの一例である。また、変換切り替えパラメータは、ハンド220の状態やハンド220による対象物10の把持の状態に応じて、ツールセンタポイントPoの位置を切り替える際に用いられる。力フィードバック制御部107a2は、力センサ310の座標系から手先座標系に座標変換された力・モーメントの検出値である力情報と力・モーメントの目標値の差分信号を入力として、ツールセンタポイントPoの微小変位・回転角指令値を出力する。変換部107a3は、力センサ310からの3軸の力と3軸回りのモーメントである力情報をツールセンタポイントPoでの力情報に変換する処理を実行する。この際、力センサ310の設置位置とツールセンタポイントPoの幾何的な位置関係から、座標変換により変換処理を実行することができる。ここで、例えば屈曲部220bなどの稼働により、力センサ310とツールセンタポイントPoとの相対姿勢および相対位置が変化した場合は、これを考慮した変換を実行する。また、ハンド220が対象物10を把持した場合には、対象物10の重心位置をツールセンタポイントPoとしてもよいし、動作中に画像認識等により、把持された対象物10が何かに接触するようなことが推定される場合には、その接触推定点をツールセンタポイントPoとしても良い。このようなツールセンタポイントPoの切り替えは、変換切り替えパラメータによって行われる。
力フィードバック制御部107a2は、3軸方向の力と、3軸方向のモーメントの6個の制御器で構成されている。制御器は、同じ構造を有し、次の式(1)で表される。
・・・ (1)
ここに、K
Gは、力制御比例ゲイン、K
Aは、力制御積分ゲイン、Mは、モデルマス定数、Dは、モデルダンピング定数、Kは、モデルバネ定数である。式(1)は、バネ・マス・ダンパモデルを表す2次フィルタに比例積分(PI)制御器が直列に接続された伝達関数(ディジタルでの実装ではサンプル周期毎の矩形積分)である。ここで、K
G=0とすると、Gf(s)全体が0となる。この場合は、手先座標力制御部107aの力制御寄与分は無くなり、位置制御になる。このように、力フィードバック制御部107a2では、位置制御のみの制御と力制御の切換がスムースに行える。また、K
G≠0、K
A=0とすると、外力に対するバネ動作が実現でき、K
G≠0、K
A≠0とすると、外力に応じて被押付面に倣う積分動作が実現できる。
ここで、バネ動作とは、仮想的にバネがあるような姿勢・位置の復元を伴う動作である。例えば、力の目標値を0とした力の制御中にある軸方向に外力が加わると、力フィードバック制御部107a2は力センサ310に検知された外力を0にするように、指令値を出力する。この結果、マニピュレータ200はある軸の外力と反対方向に位置を移動する。ここで、バネ動作時には、外力がなくなった場合には元の位置に戻る。このような動作はバネ動作と称される。
また、積分動作とは外力に従って移動し、被押付面に倣う動作である。例えば、力の目標値を0とした力の制御中にある軸方向に外力が加わると、力制御器は力センサ310に検知された外力を0にするように、指令値を出力する。この結果、マニピュレータ200はある軸の外力と反対方向に位置を移動する。ここで、積分動作時には、マニピュレータ200は外力がなくなった後も、外力によって最終的に移動した位置にとどまる。このような動作は積分動作と称される。
力フィードバック制御部107a2は、2~3次のローパスフィルタと見なすことができ、力センサ信号のノイズ除去も同時に行える。力センサ信号のAD入力において、アンチエリアシングフィルタとして簡易なアナログローパスフィルタを入れておけば十分である。
本実施形態の物体ハンドリングシステムでは例えば、図7A,7Bに示されるような押し付けの動作において、力フィードバック制御部107a2の設定を、並進の3自由度と鉛直軸回りを積分動作、姿勢の残りの2自由度をバネ動作とする。このように、押付方向や回転方向に対して適切な力制御モードを設定することで、より確実に密着した箱詰め動作を実現することができる。
積分動作ではドリフトを防ぐために、あらかじめ力センサ310のキャリブレーションの一種である重力補償が必要である。これは、力センサ310の先にある質量によって発生する力の成分をキャンセルし、衝突時や押し付け時における外力をより高精度に検出するための処理である。
図10は、重力補償部107dの構成図である。図10に示されるように、重力補償部107dは、ローパスフィルタ107d1(LPF)、管理部107d2、メモリ107d3、座標変換部107d4を有している。ローパスフィルタ107d1は、例えば、重力補償値を得るために、次の式(2)で表せる力センサ信号6軸分に、遅い時定数T
f0、1秒のオーダ)の1次ローパスフィルタを力フィードバック制御部107a2と並行して動作させておく。
・・・ (2)
管理部107d2は、統合部101から、ロボット姿勢情報、対象物情報、および管理情報を受け、重力補償動作を実行する。ロボット姿勢情報は、ハンド220を含むマニピュレータ200の姿勢を示す情報である。対象物情報は、対象物10の重量や、大きさ、把持状態等の情報である。管理情報は、例えば、補償実行指示や、力制御開始指示等である。統合部101は、上述した位置P006や、位置P007、位置P000のような、ハンド220が重力補償値を取得する位置に到来した時点で、管理部107d2に、管理情報としての補償実行指示を出す。管理部107d2は、補償実行指示を取得すると、その時点でのロボット姿勢情報や、対象物情報とともに、ローパスフィルタ107d1を通過した後の6軸各軸の力情報をメモリ107d3に保持する。この時、メモリ107d3がマニピュレータ200の複数の姿勢に対応したロボット姿勢情報や複数の対象物情報を保持しておくことにより、座標変換部107d4における重力補償値の算出の精度をより高めることができる。各軸のメモリ情報はピッキングの動作中以外でも、事前に蓄積することも可能である。管理部107d2は、管理情報としての力制御開始指示を受け取ると、座標変換部107d4に、ロボット姿勢情報、対象物情報、補償値出力開始指示を出す。座標変換部107d4は、補償値開始指示に対応して、重力補償値を出力する。ここで、座標変換部107d4はメモリ107d3に保持されたロボット姿勢情報や対象物情報毎に保持された力情報を用い、管理部107d2から入力された動作中のロボット姿勢情報、対象物情報に従って、重力補償値を算出し、出力する。一例としては、座標変換部107d4は、保持された力情報と動作中のロボット姿勢情報が同じか近い値の場合の力情報を、重力補償値(オフセット)として出力し、力センサ310の計測値(生値)から引いておけば良い。メモリ107d3内に現在の姿勢に近いロボット姿勢情報に対応する重力補償値が保持されていない場合には、他のロボット姿勢情報に対応する重力補償値から補完処理を行うなどすることにより、重力補償値を算出する。なお、ローパスフィルタ107d1の出力値は、力・モーメント目標値達成の閾値判定にも使える。このような簡易的な重力補償は演算が高速に可能なため、高速な処理が必要なピッキング処理に用いる重力補償として適している。
ただし、力制御時の姿勢が大きく変化すると、補償値の誤差が大きくなる。特に、重力方向と力センサ310の力検出軸のなす角が変化すると誤差が大きくなる。そこで、本実施形態の物体ハンドリングシステムで1は、力制御を実行する行程と同じか、より近い姿勢をとった状態で、重力補償を実行する。少なくとも、重力方向と力センサ310の力検出軸のなす角が略一致するような姿勢をとった状態、例えば、対象物10の押し付け時の力センサ310のZ軸の向きが、重力方向と並行である場合には、重力補償時も同様に力センサ310のZ軸の向きが、重力方向と並行とする。この際、Z軸回りの回転位置、すなわちX軸やY軸の方向は異なる状態でも構わない。これらの方向は力制御実施時に、姿勢を考慮した演算を実行することで、安定した力制御を可能としている。
図9において、力フィードバック制御部107a2の出力を微小変位・回転角[ΔX,ΔY,ΔZ,ΔA,ΔB,ΔC]と見なすと、これは、同次変換行列としては、次の式(3)のように表せる。
・・・ (3)
で表される。手先位置制御部107a4では、この行列をベース座標系で表された手先の(ノミナルの)位置目標値軌道[X,Y,Z,A,B,C]から導出した同次変換行列Tに右から掛けて、T(Δ+I)を計算することによって、修正された位置目標値軌道のベース座標系での同次変換行列をリアルタイムで得ることができる。さらに、図8の逆運動学演算部107bにおける逆座標変換によって各関節の位置制御系への目標値が算出される。
ここで、上述したような「密着させる面に対してハンド220または対象物10を押し付ける(長めの)位置目標値軌道を生成しておき、対象物10が被押付面に接触したときに当該被押付面に倣うように力制御系を機能させる」だけでは、密着が完了するとは限らない。そこで、位置目標値が終了してから、力制御器だけを機能させて密着させる次のようなシーケンスが必要となる。(1)~(5)により、対象物10の容器140a,140bの隅部への密着が完了する。
(1)対象物を吸着し、容器140a,140bの上方へ移動
(2)力センサ310の現在値から重力補償値を決定
(3)並進X,Y,Zの3方向、垂直軸(例えばX軸)回りを積分動作とし、姿勢方向(例えば、Y軸回り方向、Z軸回り方向)をバネ動作として、力目標値=0(ゼロ)で、容器140a,140bの隅付近のX,Y,Z座標を目標値に設定して直線補間で移動
(4)位置目標値軌道が終了したら、さらに、押付力目標値を設定して待機
(5)位置制御モードに戻す
(4)では、(ノミナルの)直線補間の目標値が終了した後に、改めてX,Y(平面)方向の力目標値を設定し、例えば、容器140a,140bの壁にはね返されても、力制御だけで密着動作をさせることをねらっている。また、Z(鉛直)方向の力目標値とC(鉛直軸)回りのモーメント目標値は0(ゼロ)のままとしている。前者は、重力を利用すればZ方向の力制御は不要であること、後者は、X,方向Yの力目標値があるので、C回りにはモーメントの目標値をゼロとする制御を実行すれば十分という技術的思想に基づいている。
また、本実施形態の物体ハンドリングシステム1では、図8,10に示されるようなロボット制御部107において、衝突検知等のエラー検知を実行することができる。例えば、ロボット制御部107に入力される力情報を図示しないローパスフィルタに通し、エラー検出部110(図4)にてモニタリングを実行する。ここで、位置制御中に衝突等が発生し、力センサ310やハンド220が壊れるような力が発生した場合や、力制御中であっても、力をキャンセルするマニピュレータ200の移動速度(速度限界、上限速度)を超え、力センサ310やハンド220が壊れるような力が発生した場合には、マニピュレータ200の動作を急停止する。また、図9の変換部107a3の出力を活用し、手先に発生する力をモニターすることで、力センサ310やハンド220が壊れないような力の範囲であっても、手先に把持した物品が壊れるような範囲の力が発生した際に、マニピュレータ200の動作を緊急停止することができる。このように力制御と力検出によるマニピュレータ200の動作の切り替えを併用することにより、より安定的に把持、箱詰め動作を実現することができる。
[把持動作]
次に、物体ハンドリングシステム1が、対象物10を把持する際に実施する把持動作の手順について説明する。ロボット制御部107は、把持動作時には、力制御によりハンド220を対象物10に優しく押し付ける。
図11は、ハンド220が対象物10を把持する状態を示す斜視図である。図11に示されるように、ロボット制御部107は、対象物10の把持面に対して鉛直方向にハンド220を近付けて比較的軽く押し付ける。これは、タッチ動作とも称されうる。タッチ動作においては、並進成分(方向X,方向Y,方向Z)については、所定の押付力に相当する大きさかつ図11中太矢印で示す把持面との直交方向(方向Z)のベクトルの力目標値を設定し、押し付けない方向、言い換えると押付方向との直交方向(方向X,方向Y)には力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値を0、または不感帯以下の値に設定したばね動作を実行し、Z軸周りは力目標値を0に設定した積分動作を実行する。ここで不感帯とは、実際には力センサ310で力が検出されていたとしても、フィードバック制御を行わない力の範囲である。不感帯は、例えば、押付力のような制御力以下に設定される。
図12は、把持動作時におけるロボット制御部107によるマニピュレータ200の制御手順を示すフローチャートである。図13A~13Gは、把持動作における各位置でのハンド220の状態または移動経路を示す側面図である。
S2-1において、ロボット制御部107は、統合部101等から把持動作の開始指示を受けると、位置制御を実行する第二制御モードにより、ハンド220を位置P001までより高速に移動させる。
S2-2において、ロボット制御部107は、位置P001において、ハンド220の姿勢を、把持位置Phでの姿勢と略同じ姿勢か近い姿勢に設定する。
S2-3において、画像処理部102および統合部101は、カメラ321が撮影した画像から、把持位置Phに到達するまでの間に、ハンド220が対象物10以外の障害物Oと接触するか否かを判断する。接触するあるいは接触する可能性が高いと判断された場合(S2-3でY)、S2-11に移行する。接触しないあるいは接触する可能性が低いと判断された場合(S2-3でN)、S2-4に移行する。
S2-4において、ロボット制御部107は、図13Aに示される位置P001から位置P002までの区間において、ハンド220を、位置制御を実行する第二制御モードにより、例えば容器140a,140b外の区間のような他の位置制御による区間よりも遅い速度で、移動させる。このように、位置制御において移動速度がより低い区間を設定することにより、衝突検知のための力の閾値をより低く設定することができ、当該閾値が高い場合に比べて、把持動作をより確実に実行することができるとともに、障害物Oと干渉した場合に当該障害物Oから受ける力をより小さくすることができる。
S2-5において、ハンド220は、図13Aに示される位置P002に到達する。
S2-6において、ロボット制御部107は、ハンド220の制御モードを、位置制御を実行する第二制御モードから、位置制御とともに力制御を実行する第三制御モードに切り替える。第三制御モードでは、並進成分(方向X,方向Y,方向Z)については、力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値を0、または不感帯以下の値を設定したばね動作を実行し、Z軸周りは力目標値を0、または不感帯以下の値に設定した積分動作を実行する。当該制御において制御量が目標値に達したら、S2-7に移行する。
S2-7において、ロボット制御部107は、位置P002から把持位置Phへ移動する途中の図13Bに示される位置Pcaにおいて、言い換えると、把持位置Phの手前の位置Pcaに到達した時点で、ハンド220の制御モードを、位置制御とともに力制御を実行する第三制御モードから、力制御を実行する第一制御モードに切り替える。ロボット制御部107は、第一制御モードでの目標値を、ハンド220による対象物10の把持面(接触面)に対する垂直方向への押し付けが可能となるように設定する。具体的には、例えば、並進成分(方向X,方向Y,方向Z)については、所定の押付力(例えば5Nに相当する大きさ)および把持面に対する垂直方向(方向Z)のベクトルの力目標値を設定し、押し付けない方向(方向X,方向Y)には力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値に0、または不感帯以下の値を設定したばね動作を実行し、Z軸周りは力目標値に0の積分動作を実行する。
ここで、仮に、位置制御モードでは、制御上は、ハンド220は目標位置(この場合は把持位置Ph)に到達していたとしても、実際には、ハンド220が目標位置の手前で物に接触し、そこで止まってしまうような場合もある。この点、本実施形態によれば、ロボット制御部107は、ハンド220を、位置P002から把持位置Phへ、位置制御とともに力制御を実行する第三制御モードまたは力制御を実行する第一制御モードによって移動させるため、ハンド220の対象物10に対する押圧力が所期の値に設定されやすくなるとともに維持されやすくなる。また、本実施形態では、第一制御モードでは、X軸、Y軸回りのモーメントは、目標値=0のバネ動作に設定されているため、対象物10の把持面が傾いていた場合においても、ハンド220に発生するモーメントが減殺され、ハンド220が把持面に対して直交し、言い換えるとハンド220の吸着パッド220aが把持面に倣い、これにより、ハンド220による対象物10のより確実な把持状態が得られやすくなる。
S2-8において、ロボット制御部107は、制御量が目標値に達したか否かを判断する。目標値に達していない場合、S2-9に移行する。制御量が目標値に達している場合には、S2-10に移行する。
S2-9では、ロボット制御部107は、各パラメータの値がリミット(閾値)に達しているか否かを判定する。パラメータは、例えば、予め設定されている位置リミットや、所定時間を超えた処理を設定する時間リミット等である。リミットにより、マニピュレータ200(ハンド220)が位置や姿勢の所定範囲を超えていないかを判定することができる。リミットに達した場合(S2-9でY)、一連の制御を終了する。リミットに達していない場合(S2-9でN)、S2-10に移行する。
S2-10では、ロボット制御部107は、図13Cに示される把持位置Phにおける対象物10の把持をより確実に実行するため、ハンド220による把持状態を少なくとも所定時間以上維持する。S2-10において、把持状態が所定時間以上継続できた場合には、把持動作が終了され、次の手順(例えば、ハンド220の位置P003への移動)に移行する。S2-10において、把持状態が所定時間以上継続できなかった場合には、リカバリ動作等に移行する。
他方、S2-3において、接触するあるいは接触する可能性が高いと判断された場合(S2-3でY)、S2-11において、ロボット制御部107は、位置P001から、すなわち容器140a外から位置P005へ、位置制御を実行する第二制御モードによらず、位置制御とともに力制御を実行する第三制御モードによって、ハンド220を移動させる。これにより、ハンド220が障害物Oと接触した場合にあっても、大きな接触力を発生させずに、接触点から逃げるような動作を実行することができる。ただし、第三制御モードによる移動距離が長くなると、所要時間が長くなる。よって、S2-11では、S2-6よりも速度を高くしてもよい。S2-11の後は、S2-5へ移行する。
このような把持動作時の制御により、対象物10がばらばらに積まれているような場合であっても、力制御によって、ハンド220を、対象物10の把持面に、その垂直方向に所定の大きさの力で押し付けることができる。よって、例えば、ハンド220の押圧や衝突による対象物10の破損や、ハンド220と対象物10とが接触していないことによる把持の失敗等を、回避できる。
[解放動作]
次に、物体ハンドリングシステム1が、対象物10を解放する際に実施する解放動作の手順について説明する。ロボット制御部107は、解放動作時には、力制御により対象物10を解放位置Prの周辺の壁や物体に優しく押し付けるタッチ動作を実行する。
図14A,14Bは、ハンド220による解放動作の一例を示す斜視図である。図14Aは、解放動作においてハンド220の回転を伴わない場合の例であり、図14Bは、解放動作においてハンド220の回転を伴う場合の例である。解放動作においては、並進成分(方向X,方向Y,方向Z)については、所定の押付力に相当する大きさかつ図14A,14B中の太矢印で示す押付方向のベクトルの力目標値を設定し、押し付けない方向には力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値を0、または不感帯以下の値に設定したばね動作を実行し、Z軸周りは力目標値を0に設定した積分動作を実行する。図14Aの例では、押付方向は、障害物Oと容器140bの壁140b1との間の隅部に挿入する方向であり、ロボット制御部107は、対象物10を、当該障害物Oおよび壁140b1に押し付ける。この場合、各方向の押付力の大きさを仮に5[N]とした場合、(X,Y,Z,A,B,C)方向の力目標値は(5[N],-5[N],-5[N],0[Nm],0[Nm],0[Nm])を設定する。これにより、対象物10は容器140bおよび既に置いてある障害物Oに密着し、より集積度の高い箱詰めが実現されうる。
図14Bに示されるように、解放動作においてハンド220の回転を伴う場合にあっても、図14Aの場合と同様の制御が実行される。この場合も、各方向の押付力の大きさを仮に5[N]とした場合、(X,Y,Z,A,B,C)方向の力目標値は(5[N],-5[N],-5[N],0[Nm],0[Nm],0[Nm])に設定されている。ただし、図14Bでは、ロボット制御部107は、対象物10を、障害物Oの側面O1に斜めに押し付ける。すると、対象物10およびハンド220は、障害物Oとの接触点を中心とするZ軸回りのモーメントが生じる。この場合、Z軸回りのモーメントの目標値は0[Nm]であるため、発生するモーメントを打ち消すように、ハンド220は回転し、既に置いてある障害物Oに倣うように、すなわち障害物Oの側面O1と平行な状態に、対象物10を配置することができる。このように、本実施形態では、物体ハンドリングシステム1は、各軸方向のパラメータにそれぞれ適切な値を設定することにより、容器140b内の障害物O等の状況に応じて対象物10を密着して配置することが可能となり、よりスペース効率の高い対象物10の容器140bへの収納が実現されうる。
図15は、ロボット制御部107によるマニピュレータ200の制御手順を示すブローチャートである。図16A~16Hは、解放動作における各位置でのハンド220または対象物10の状態または移動経路を示す図である。
S3-1において、ロボット制御部107は、位置制御を実行する第二制御モードにより、ハンド220を位置P008までより高速に移動させる。
S3-2において、ロボット制御部107は、位置P008において、ハンド220の姿勢を、把持位置Phでの姿勢と略同じ姿勢か近い姿勢に設定する。
S3-3において、画像処理部102および統合部101は、カメラ322が撮影した画像から、解放位置Prに到達するまでの間に、ハンド220が対象物10以外の障害物Oと接触するか否かを判断する。接触するあるいは接触する可能性が高いと判断された場合(S3-3でY)、S3-11に移行する。接触しないあるいは接触する可能性が低いと判断された場合(S3-3でN)、S3-4に移行する。
S3-4において、ロボット制御部107は、図16Aに示される位置P008から位置P009までの区間において、ハンド220を、位置制御を実行する第二制御モードにより、例えば容器140a,140b外の区間のような他の位置制御による区間よりも遅い速度で、移動させる。このように、位置制御において移動速度がより低い区間を設定することにより、衝突検知のための力の閾値をより低く設定することができ、当該閾値が高い場合に比べて、把持動作をより確実に実行することができるとともに、障害物Oと干渉した場合に当該障害物Oから受ける力をより小さくすることができる。
S3-5において、対象物10を把持したハンド220は、図16A,16Bに示される位置P009に到達する。
S3-6において、ロボット制御部107は、ハンド220の制御モードを、位置制御を実行する第二制御モードから、位置制御とともに力制御を実行する第三制御モードに切り替える。この場合、図16B,16Cに示されるように、S3-6において、対象物10を容器140bの底面および側面に押し付けるため、ハンド220が、位置P009から解放位置Prへ向けて斜め下方に移動するよう、制御される。当該制御において制御量が目標値に達したら、S3-7に移行する。
S3-7において、ロボット制御部107は、位置P009から解放位置Prへ移動する途中の図16B,16Cに示される位置Pcbにおいて、言い換えると、解放位置Prの手前の位置Pcbに到達した時点で、ハンド220の制御モードを、位置制御とともに力制御を実行する第三制御モードから、力制御を実行する第一制御モードに切り替える。ロボット制御部107は、第一制御モードでの目標値を、図16Cに示されるようなハンド220が把持した対象物10の容器140bの底面および側面への押し付けが可能となるように設定する。具体的には、例えば、並進成分(方向X,方向Y,方向Z)については、所定の押付力(例えば5Nに相当する大きさ)および容器140bの底面および側面に向かう図16Cでは斜め下方向のベクトルの力目標値を設定し、押し付けない方向(斜め下方向の垂直方向)には力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値に0、または不感帯以下の値を設定したばね動作を実行し、Z軸周りは力目標値を0とする積分動作を実行する。これにより、図16Dに示されるように対象物10が容器140bの側面と底面との間の隅に寄った解放位置Prに解放される。
次に、解放位置Prの周辺に容器140bの壁や障害物Oが多く存在する状況について説明する。ここで、仮に、図16E~16Hに示されるように、解放位置Prの両側に容器140bの側壁や障害物Oが存在している場合にあっては、カメラ320の情報などから、画像処理部102および統合部101が、周囲の状況を判断し、制御モードを切り替える区間(切替位置)を変更する。切り替えの判断は、例えば、ハンド220および把持した対象物10の通過予定位置と、カメラ320から得られた距離画像における障害物Oの位置と、の比較から、両者の最短距離を計算し、当該最短距離が一定以上小さくなるなどの条件を与えることにより、実行することができる。図16Eの例では、位置P009は、図16Aとは異なる周囲の障害物Oから離れた位置に、設定されている。さらに、この条件では、図16B,16Cにおける斜め下方向の移動ができない。このような場合には、ロボット制御部107は、第一制御モードでの目標値を、図16F,16Gに示されるようなハンド220が把持した対象物10の容器140bの底面に対する当該底面と垂直な方向への押し付けが可能となるように設定する。具体的には、例えば、並進成分(方向X,方向Y,方向Z)については、所定の押付力(例えば5Nに相当する大きさ)および容器140bの底面に向かう図16F,16Gでは下方向のベクトルの力目標値を設定し、押し付けない方向(方向X,方向Y)には力目標値を0、または不感帯以下の値に設定した積分動作を実行する。モーメント成分については、X軸、Y軸周りは力目標値に0、または不感帯以下の値を設定したばね動作を実行し、Z軸周りは力目標値を0とする積分動作を実行する。また、この例では、容器140bの底面から上方に離れた位置が解放位置Prに設定される。この場合も、図16Hに示されるように対象物10が容器140bの底面上に載置される。
図17A~17Dは、S3-6における位置P009から解放位置Prへ向けて下方に移動する途中で対象物10が障害物Oと干渉した場合の、ハンド220および対象物10の状態を示す図である。このような事態は、例えば、物体ハンドリングシステム1が認識しているハンド220による対象物10の把持位置または把持姿勢と、実際の把持位置または把持姿勢とが違う場合に、生じることがある。図17Cに示されるように、位置P009から解放位置Prへ行く途中、第三制御モードでの移動中に、対象物10と障害物Oとが衝突している。この場合、衝突により、X軸、Y軸回りにモーメントが発生する。ここで、第三制御モードでは、X軸およびY軸回りのモーメントは、目標値を0とするバネ動作に設定されているため、衝突により発生したモーメントは減殺され、ハンド220は、図17Cに示されるように、ハンド220および対象物10は傾く。さらに、この衝突により、方向Xおよび方向Yに力が生じるが、方向Xおよび方向Yについては、力目標値は0に設定されているため、衝突により受けた力は減殺され、図17Dに示されるように、ハンド220および対象物10は、障害物Oから離れるように図17Dの左方へ移動する。その後、X軸およびY軸回りのモーメントは、バネ動作であるため、衝突による外力の入力が無くなると、ハンド220の姿勢は、当初の解放姿勢に戻る。また、ロボット制御部107は、方向Xおよび方向Yには、力の積分動作を実行するため、ハンド220は外力を受けない位置に平行移動し、当該位置を維持する。これにより、図17Dに示されるように対象物10が容器140bの側壁および障害物Oと干渉しない解放位置Prに解放される。このように、本実施形態では、物体ハンドリングシステム1は、各軸方向のパラメータにそれぞれ適切な値を設定することにより、容器140b内の障害物O等の状況に応じて対象物10を密着して配置することが可能となり、よりスペース効率の高い対象物10の容器140bへの収納が実現されうる。
S3-8において、ロボット制御部107は、制御量が目標値に達したか否かを判断する。目標値に達していない場合、S3-9に移行する。制御量が目標値に達している場合には、S3-10に移行する。
S3-9では、ロボット制御部107は、各パラメータの値がリミット(閾値)に達しているか否かを判定する。パラメータは、例えば、予め設定されている位置リミットや、所定時間を超えた処理を設定する時間リミット等である。リミットにより、マニピュレータ200(ハンド220)が位置や姿勢の所定範囲を超えていないかを判定することができる。リミットに達した場合(S3-9でY)、一連の制御を終了する。リミットに達していない場合(S3-9でN)、S3-10に移行する。
S3-10では、ロボット制御部107は、対象物10の解放をより確実に実行するため、ハンド220による解放状態を少なくとも所定時間以上維持する。S3-10において、解放状態が所定時間以上継続できた場合には、解放動作が終了され、次の手順(例えば、ハンド220の位置P010への移動)に移行する。S3-10において、解放状態が所定時間以上継続できなかった場合には、リカバリ動作等に移行する。
他方、S3-3において、接触するあるいは接触する可能性が高いと判断された場合(S3-3でY)、S3-11において、ロボット制御部107は、位置P008から、すなわち容器140a外から位置P009へ、位置制御を実行する第二制御モードによらず、位置制御とともに力制御を実行する第三制御モードによって、ハンド220を移動させる。これにより、ハンド220が容器140aや障害物Oと接触した場合にあっても、大きな接触力を発生させずに、接触点から逃げるような動作を実行することができる。ただし、第三制御モードによる移動距離が長くなると、所要時間が長くなる。よって、S3-11では、S3-6よりも速度を高くしてもよい。S3-11の後は、S3-5へ移行する。
以上のような解放動作時の制御により、対象物10を把持したハンド220は、容器140bの壁や障害物Oとの干渉を回避しながら容器140b外から解放位置Prへ移動することができる。また、力制御によって、対象物10を解放位置Prの周辺の容器140bの壁や障害物Oに寄った状態(隣接した状態、密着した状態)で解放できる。これにより、解放位置Pr(容器140b内)において対象物10をより密集させることができる。
図18は、種々の障害物Oに対応したハンド220の状態および移動方向を示す図である。図18Aには、対象物10ではなくハンド220が障害物Oと干渉した場合が示されている。このような場合にあっても、ハンド220は、上述した第三制御モードあるいは第一制御モードで制御されることにより、図17Cの場合と同様に、障害物Oを避けるように移動しながら解放位置Prへ移動することができる。図18Bは、障害物Oが硬い場合を示している。この場合には、ロボット制御部107は、対象物10を、障害物Oの側面および容器140bの底面に寄せた状態で、解放することができる。図18C,18Dの場合は、ロボット制御部107あるいは統合部101は、センサ300の検出結果等に基づいて、対象物10を障害物Oに押し付け難いと判断した場合、あるいは判断基準とするデータベースに、解放位置Prの周辺に位置された障害物Oの情報が登録されていなかった場合には、当該障害物Oから離れた解放位置Prにおいて、対象物10を解放することができる。
また、ハンド220が仮置位置Ptにある対象物10を把持したり、ハンド220が仮置位置Ptに対象物10を解放したりする場合にあっても、上記実施形態と同様に、第一制御モード、第二制御モード、あるいは第三制御モードでハンド220を制御することができる。また、予め、仮置位置Ptに容器の壁や障害物Oが存在していないことがわかっている場合や、画像処理部102、信号処理部103、あるいは統合部101が、センサ300の検出結果に基づいて、仮置位置Ptに容器の壁や障害物Oが存在していないことを判断した場合にあっては、ハンド220が位置制御(第二制御モード)によって移動する区間の長さを、把持位置Phや解放位置Prに移動する場合よりも長くすることができる。その場合、ハンド220が仮置位置Ptへ近付く経路の移動時間をより短くすることができる。
上述した制御装置100による演算処理や制御は、ソフトウエアによって実行されてもよいし、ハードウエアによって実行されてもよい。また、制御装置100による演算処理や制御には、ソフトウエアによる演算処理や制御とハードウエアによる演算処理や制御とが含まれてもよい。ソフトウエアによる処理の場合にあっては、制御装置100は、ROMや、HDD、SSD、フラッシュメモリ等の記録媒体(記憶媒体)に記憶されたプログラム(アプリケーション)を読み出して実行する。制御装置100は、プログラムにしたがって作動することにより、制御装置100に含まれる各部、すなわち、統合部101や、画像処理部102、信号処理部103、把持計画生成部104、解放計画生成部105、動作計画生成部106、ロボット制御部107、周辺機器・I/O制御部108、学習制御部109、エラー検出部110等として、機能する。この場合、プログラムには、上記各部に対応するモジュールが含まれる。
プログラムは、それぞれインストール可能な形式または実行可能な形式のファイルでCD-ROMや、FD、CD-R、DVD、USBメモリ等の、コンピュータで読み取り可能な記録媒体に記録されて提供されうる。また、プログラムは、通信ネットワークに接続されたコンピュータの記憶部に記憶され、ネットワーク経由でダウンロードされることによって導入されうる。また、プログラムは、ROM等に予め組み込まれてもよい。
また、制御装置100の少なくとも一部がハードウエアによって構成される場合、当該制御装置100には、例えば、field programmable gate array(FPGA)や、application specific integrated circuit(ASIC)等が含まれうる。
以上、説明したように、本実施形態では、ロボット制御部107(動作制御部)は、力(およびモーメント)を制御量とする第一制御モード、位置(および角度)を制御量とする第二制御モード、および力(およびモーメント)並びに位置(および角度)を制御量とする第三制御モードを切り替えて、ハンド220(マニピュレータ200)を制御する。このような構成によれば、例えば、第一制御モードや第三制御モードにより、ハンド220が、対象物10の把持面や、障害物O、容器140bの壁などに倣う制御や、所謂箱詰め制御が可能となる。また、干渉による衝撃力や押圧力を緩和することができ、ひいては対象物10や、ハンド220、障害物O、容器140a,140b等の保護性を高めることができる。また、第二制御モードや第三制御モードにより、より高速な移動が可能となる。このように、第一制御モードと当該第一制御モードとは異なる制御モードとを切り替えることにより、例えば、容器140b内での対象物10を含む取扱物品の集積度の向上や、保護性の向上、処理時間の短縮、等の種々の効果がより高いレベルで両立されやすい物体ハンドリングシステム1を得ることができる。
また、本実施形態では、ハンド220の移動経路は、ハンド220が容器140a,140b内において第一制御モードで移動する第一区間(位置P002または位置Pcb~把持位置Ph、位置P009または位置Pcb~把持位置Ph)と、容器140a,140b外において第二制御モードで移動する第二区間(位置P003~位置P008、位置010~位置P001)と、を有する。このような構成によれば、例えば、ロボット制御部107は、対象物10やハンド220がそれら以外の物体(例えば、障害物Oや容器140a,140b)とより干渉しやすい容器140a,140b内の第一区間では、力を制御する第一制御モードでハンド220を制御することにより、集積度の向上や保護性の向上を図ることができるとともに、対象物10やハンド220がそれら以外の物体とより干渉し難い容器140a,140b外の第二区間では、位置を制御する第二制御モードでハンド220を制御することにより、処理速度の向上を図ることができる。
また、本実施形態では、ハンド220の移動経路は、さらに、容器140a,140b内において第三制御モードで移動する第三区間(位置P002~位置Pca、位置009~位置Pcb)を有する。このような構成によれば、例えば、ロボット制御部107は、容器140a,140b内において、対象物10やハンド220がそれら以外の物体と干渉する確率があるものの比較的(第一区間よりは)高くない第三区間では、位置および力を制御する第三制御モードでハンド220を制御することにより、容器140a,140b内において、所要の保護性を確保しながら、処理速度の更なる向上を図ることができる。
また、本実施形態では、ロボット制御部107は、第二制御モードにおける移動速度を、場所により切り替えることができる。具体的には、容器140a,140b内の第二制御モードによる第四区間(位置P001~位置P002、把持位置Ph~位置P003、位置P008~位置P009、解放位置Pr~位置P010)における移動速度は、容器140a,140b外の第二制御モードによる第二区間(位置P003~位置P008、位置010~位置P001)における移動速度よりも低く設定されうる。このような構成によれば、例えば、所要の保護性を確保しながら、処理速度の更なる向上を図ることができる。
また、本実施形態では、重力補償部107dは、力センサ310の重力補償を、ハンド220に対する重力の作用状態が、把持姿勢あるいは解放姿勢(第一把持姿勢)と略等価となる姿勢(第二把持姿勢)で重力補償を実行する。このような構成によれば、例えば、ロボット制御部107は、力制御を実行する第一制御モードによるハンド220に作用する力(およびモーメント)をより高精度に検出することができ、ひいては、第一制御モードによる制御をより高精度に実行することができる。このような重力補償部107dにより得られる効果が得られるシステムは、上記物体ハンドリングシステム1には限定されない。
以上、本発明の実施形態を例示したが、上記実施形態は一例であって、発明の範囲を限定することは意図していない。上記実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、組み合わせ、変更を行うことができる。これら実施形態は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。また、各構成や形状等のスペック(構造や、種類、方向、形状、大きさ、長さ、幅、厚さ、高さ、数、配置、位置、材質等)は、適宜に変更して実施することができる。例えば、上記ハンド制御装置は、上述した一の容器内から別の容器内に対象物を搬送する物体ハンドリングシステム以外のハンドリングシステムにも適用可能である。