JP7562283B2 - ピッキング装置、制御装置、及びプログラム - Google Patents

Description

本発明の実施形態は、ピッキング装置、制御装置、及びプログラムに関する。

搬送の対象となる対象物をピッキングして把持し、搬送するピッキング装置において、搬送物の搬送中に搬送物やピッキング装置に衝突が生じると、物品を落としたり傷つけたりすることがある。このため、物品の搬送中に衝突が発生することを判定することが望まれるが、衝突を簡易に判定することが難しかった。

特許第５９０５５４９号公報

本発明が解決しようとする課題は、搬送対象物の搬送中に発生する衝突を簡易に検知することができるピッキング装置、制御装置、及びプログラムを提供することである。

実施形態のピッキング装置は、把持部と、アーム部と、検出部と、制御部と、を持つ。把持部は、搬送対象物をピッキングして把持する。アーム部は、前記把持部を移動させて前記把持部に前記搬送対象物を搬送させる。検出部は、前記アーム部に取り付けられ、前記把持部にかかる力を検出する。制御部は、前記把持部及び前記アーム部の動作を制御する。前記制御部は、算出部と、減算部と、を持つ。算出部は、前記搬送対象物の質量に応じて定められた係数を含む演算式を用いて、前記把持部が前記搬送対象物を把持して移動させる際に前記把持部にかかる重力及び慣性力を算出する。減算部は、前記検出部が検出した前記把持部にかかる力から、前記算出部が算出した前記重力及び前記慣性力を減算する。

実施形態のピッキング装置１の全体を示す図。 コントローラ２００の構成図。 重力補償パラメータテーブル２８２の一例を示す図。 慣性力補償パラメータテーブル２８４の一例を示す図。 コントローラ２００の処理の一例を示すフローチャート。 重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を作成する手順の一例を示すフローチャート。 第１軸１１１を極低速で往復動作させたときのＸ_Ｅ方向の第１力ｆｘの時間変化を最小二乗法でフィッティングした場合の一例を示すグラフ。 第１軸１１１を極低速で往復動作させたときのＺ_Ｅ方向の第１力ｆｘの時間変化を最小二乗法でフィッティングした場合の一例を示すグラフ。 第１軸１１１～第３軸１１３を往復動作させたときのＸ_Ｅ方向の加速度目標値と実加速度の一例を示すグラフ。 第１軸１１１～第３軸１１３を往復動作させたときのＺ_Ｅ方向の加速度目標値と実加速度の一例を示すグラフ。 図９に示す高加減速高速動作時の加速度のデータを（４）式に基づいてフィッティングした結果を示すグラフ。 図１０に示す高加減速高速動作時の加速度のデータを（５）式に基づいてフィッティングした結果を示すグラフ。 力センサ１５０が検出した力に対して、重力補償と慣性補償を施した際の第１力ｆｘの残差波形の一例を示すグラフ。 力センサ１５０が検出した力に対して、重力補償と慣性補償を施した際の第２力ｆｚの残差波形の一例を示すグラフ。

以下、実施形態のピッキング装置、制御装置、及びプログラムを、図面を参照して説明する。なお以下の説明では、同一または類似の機能を有する構成に同一の符号を付す。そして、それら構成の重複する説明は省略する場合がある。なお、本願でいう「ＸＸに基づく」とは、「少なくともＸＸに基づく」ことを意味し、ＸＸに加えて別の要素に基づく場合も含む。また、「ＸＸに基づく」とは、ＸＸを直接に用いる場合に限定されず、ＸＸに対して演算や加工が行われたものに基づく場合も含む。「ＸＸ」は、任意の要素（例えば、任意の情報）である。

図１は、実施形態のピッキング装置の全体を示す図である。ピッキング装置１は、ピッキングロボット１００と、コントローラ２００と、インターフェース３００と、を備える。ピッキングロボット１００は、ピッキング用の６軸ロボットアームを備える。ピッキング装置１は、例えば、物品用コンテナにおいて把持した物品を搬送して集荷用コンテナに詰める。ピッキング装置１は、これらの要素の他、各コンテナを引き込むベルトコンベアや物品を認識するためのカメラなどを備える。

ピッキングロボット１００は、アーム部１１０と、力センサ１５０と、吸着ハンド１６０と、を備える。ピッキングロボット１００は、例えば、物品用コンテナＣ１に収容された搬送対象物Ｐを吸着して、集荷用コンテナＣ２に搬送する。コントローラ２００は制御装置の一例であり、力センサ１５０は検出部の一例であり、吸着ハンド１６０は把持部の一例である。

アーム部１１０は、第１軸１１１～第６軸１１６の６つの軸を備える。第１軸１１１～第６軸１１６は、基台１１７と吸着ハンド１６０との間に配置される。第１軸～第６軸の呼称には、通常、基台１１７に近い側から順に若い番号を付すが、実施形態では、説明の便宜上、適宜順序を入れ替えて説明する。基台１１７には、第４軸１１４を介して胴部１１８が設けられ、胴部１１８には、第１軸１１１を介して第１アーム１２１の一端が接続される。

第１アーム１２１の他端には、第２軸１１２を介して第２アーム１２２の一端が接続され、第２アーム１２２の他端には、第５軸１１５を介して第３アーム１２３の一端が接続される。第２アーム１２２と第３アーム１２３は、同軸状に位置される。第３アーム１２３の他端には、第３軸１１３を介して手首部１２４の一端が接続され、手首部１２４の他端には、第６軸１１６が接続される。第６軸１１６には、力センサ１５０が取り付けられ、力センサ１５０には吸着ハンド１６０が取り付けられる。

第１軸１１１～第６軸１１６には、それぞれコントローラ２００に制御によって駆動するモータが設けられる。第４軸１１４は、基台１１７に対して胴部１１８を鉛直軸周りに回転させる。第１軸１１１は、胴部１１８に対して第１アーム１２１を水平軸周りに回転させる。第２軸１１２は、第１アーム１２１に対して第２アーム１２２を水平軸周りに回転させる。

第５軸１１５は、第２アーム１２２が延在する方向に沿った軸周りに第３アーム１２３を回転させる。第３軸１１３は、第３アーム１２３に対して手首部１２４を、第３アーム１２３及び手首部１２４がそれぞれ延在する方向に沿った２軸のそれぞれに対して直交する軸周りに回転させる。第６軸１１６は、手首部１２４が延在する方向に沿った軸周りに力センサ１５０及び吸着ハンド１６０を回転させる。

力センサ１５０は、吸着ハンド１６０にかかる力を検出する。力センサ１５０は、吸着ハンド１６０にかかる力について、ＸＹＺの各方向の成分に分けて検出する。力センサ１５０は、検出結果を検出信号としてコントローラ２００に出力する。吸着ハンド１６０は、コントローラ２００の制御に応じて、搬送対象物Ｐを把持したり把持した搬送対象物Ｐを解放したりする。吸着ハンド１６０は、搬送対象物Ｐを吸着するものに代えて、搬送対象物Ｐを挟持するもの等でもよい。

図２は、コントローラ２００の構成図である。コントローラ２００は、例えば、通信部２１０と、算出部２２０と、減算部２３０と、衝突検知部２４０と、動作制御部２５０と、記憶部２８０と、を備える。算出部２２０と、減算部２３０と、衝突検知部２４０と、動作制御部２５０と、は、例えば、ＣＰＵ（Central Processing Unit）（コンピュータ）等のハードウェアプロセッサがプログラム（ソフトウェア）を実行することにより実現される。また、これらの構成要素のうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、ＧＰＵ（Graphics Processing Unit）等のハードウェア（回路部；circuitryを含む）によって実現されてもよいし、ソフトウェアとハードウェアの協働によって実現されてもよい。プログラムは、予めコントローラ２００のＨＤＤやフラッシュメモリ等の記憶装置（非一過性の記憶媒体を備える記憶装置）に格納されていてもよいし、ＤＶＤやＣＤ－ＲＯＭ等の着脱可能な記憶媒体に格納されており、記憶媒体（非一過性の記憶媒体）がドライブ装置に装着されることでコントローラ２００のＨＤＤやフラッシュメモリにインストールされてもよい。記憶部２８０は、重力補償パラメータテーブル２８２と、慣性力補償パラメータテーブル２８４と、衝突閾値２８６と、を記憶する。算出部２２０と、減算部２３０と、衝突検知部２４０と、動作制御部２５０と、を備えるハードウェアプロセッサは、制御部の一例である。

通信部２１０は、例えば、ピッキングロボット１００やインターフェース３００等との間で情報や信号の入出力を行う。通信部２１０は、例えば、力センサ１５０が出力した検出信号を算出部２２０に通知する。通信部２１０は、例えば、動作制御部２５０により通知される動作制御信号をアーム部１１０のモータや吸着ハンド１６０に出力する。

算出部２２０は、例えば、取得部２２２と、重力補償部２２４と、慣性力補償部２２６と、を備える。取得部２２２は、力センサ１５０が出力した検出信号に基づいて、ピッキングロボット１００が搬送する搬送対象物Ｐの質量を算出して取得する。取得部２２２は、計測した搬送対象物Ｐの質量を示す質量情報を重力補償部２２４及び慣性力補償部２２６に通知する。ピッキングロボット１００は、力センサ１５０に代えてまたは加えて、搬送対象物Ｐの質量を計測する計測部を備え、取得部２２２は、計測部が出力する搬送対象物Ｐの質力を取得してもよい。

重力補償部２２４は、例えば、補償重力値を算出する補償重力算出式と、ピッキングロボット１００が搬送する搬送対象物Ｐの質量に応じた重力補償パラメータを選択する。重力補償部２２４は、選択した重力補償パラメータを利用して、力センサ１５０が検出した力に含まれる重力成分を算出する。重力補償部２２４は、選択部の一例である。重力補償部２２４は、算出した補償重力値を減算部２３０に通知する。補償重力算出式については、後に説明する。

図３は、重力補償パラメータテーブル２８２の一例を示す図である。重力補償パラメータは、例えば、搬送対象物Ｐの質量ごとにまとめられて重力補償パラメータテーブル２８２として記憶部２８０に記憶される。重力補償パラメータテーブル２８２には、０［ｋｇ］～１０［ｋｇ］までの１［ｋｇ］ごとの合計１１個の質量に応じた重力補償パラメータが記憶される。重力補償パラメータとしては、例えば、補償重力算出式に用いられる６つの重力補償パラメータｇ_xs、ｇ_xc、ｇ_xf、ｇ_zs、ｇ_zc、ｇ_zfが記憶される。重力補償パラメータは、重力を算出するための重力算出係数の一例である。０［ｋｇ］の重力補償パラメータは、吸着ハンド１６０が搬送対象物Ｐを把持しない空荷の状態の第１係数の一例であり、１［ｋｇ］～１０［ｋｇ］の重力補償パラメータは、吸着ハンド１６０に負荷がある場合の第２係数、及び吸着ハンド１６０に把持される搬送対象物Ｐの質量に応じてそれぞれ区分された区分対応係数の一例である。重力補償パラメータを決定し、重力補償パラメータテーブル２８２を作成する手順は、後に説明する。

慣性力補償部２２６は、例えば、補償慣性力値を算出する補償慣性力算出式と、ピッキングロボット１００が搬送する搬送対象物Ｐの質量に応じた慣性力補償パラメータを選択する。慣性力補償部２２６は、選択した慣性力補償パラメータを利用して、力センサ１５０が検出した力に含まれる慣性力成分を算出する。慣性力補償部２２６は、選択部の一例である。利用して、力センサ１５０が検出した力に含まれる慣性力成分を算出する。慣性力補償部２２６は、算出した補償慣性力値を減算部２３０に通知する。補償慣性力算出式については、後に説明する。

図４は、慣性力補償パラメータテーブル２８４の一例を示す図である。慣性力補償パラメータは、例えば、搬送対象物Ｐの質量ごとにまとめられて慣性力補償パラメータテーブル２８４として記憶部２８０に記憶される。慣性力補償パラメータテーブル２８４には、０［ｋｇ］～１０［ｋｇ］までの１［ｋｇ］ごとの合計１１個の質量に応じた慣性力補償パラメータが記憶される。慣性力補償パラメータとしては、例えば、補償慣性力算出式に用いられる３つの慣性力補償パラメータｍ_1x、ｍ_1z、ｍ_1o、ｍ_2x、ｍ_2z、ｍ_2oが記憶される。慣性力補償パラメータは、慣性力を算出するための慣性力算出係数の一例である。０［ｋｇ］の慣性力補償パラメータは、第１係数の一例であり、１［ｋｇ］～１０［ｋｇ］の慣性力補償パラメータは、吸着ハンド１６０に負荷がある場合の第２係数、及び区分対応係数の一例である。慣性力補償パラメータを決定し、慣性力補償パラメータテーブル２８４を作成する手順は、後に説明する。

減算部２３０は、力センサ１５０が出力した検出信号に基づく力から、重力補償部２２４が通知した補償重力値と、慣性力補償部２２６が通知した補償慣性力値と、を減算する。減算部２３０は、減算結果を衝突検知部２４０に通知する。

衝突検知部２４０は、減算部２３０が通知した減算結果と、記憶部２８０に記憶された衝突閾値２８６を比較した比較結果に基づいて、搬送対象物Ｐを搬送しているときに、ピッキングロボット１００に衝突が発生したことを検知する。「ピッキングロボット１００が衝突する」とは、例えば、ピッキングロボット１００のアーム部１１０、力センサ１５０、吸着ハンド１６０等が他の物体に衝突（接触）する場合や、ピッキングロボット１００が搬送する搬送対象物Ｐが、搬送の過程で接触する予定のない他の物体に衝突（接触）する場合を含む。衝突検知部２４０は、ピッキングロボット１００に衝突が発生したことを検知した場合に、衝突信号を動作制御部２５０に通知する。

動作制御部２５０は、オペレータがインターフェース３００を用いて入力した動作指令に応じて、ピッキングロボット１００のモータや吸着ハンド１６０を制御する。動作制御部２５０は、衝突検知部２４０が衝突信号を通知した場合に、ピッキングロボット１００のモータや吸着ハンド１６０の動作を停止させる。

インターフェース３００は、例えば、入力装置と表示装置を備える。入力装置は、例えば、オペレータ等のユーザ操作が可能であり、所定の情報を入力させるデバイス、例えばマウスである。操作装置は、マウス以外のデバイス、例えばキーボードやジョイスティック等でもよい。表示装置がタッチパネルであり、表示装置と入力装置が一体となっていてもよい。

次に、コントローラ２００の処理について説明する。図５は、コントローラ２００の処理の一例を示すフローチャートである。コントローラ２００は、例えば、オペレータによるインターフェース３００に対する入力操作に応じてピッキングロボット１００を作動させ（ステップＳ１０１）、搬送対象物Ｐを物品用コンテナＣ１から集荷用コンテナＣ２に搬送させる。ピッキングロボット１００が作動されると、力センサ１５０は、吸着ハンド１６０にかかる力を検出する。力センサ１５０は、検出結果に応じた検出信号をコントローラ２００に出力する。

続いて、通信部２１０は、力センサ１５０が出力した検出信号を入力し（ステップＳ１０３）、算出部２２０に通知する。算出部２２０における取得部２２２は、通知された検出信号に応じた検出結果に基づいて、吸着ハンド１６０が搬送する搬送対象物Ｐの質量を算出して取得する（ステップＳ１０５）。

続いて、重力補償部２２４及び慣性力補償部２２６は、取得部２２２が取得した搬送対象物Ｐの質量に基づいて、重力補償パラメータ及び慣性力補償パラメータを、重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４からそれぞれ選択する（ステップＳ１０７）。

続いて、減算部２３０は、力センサ１５０が出力した検出信号に応じた検出結果が示す質量から、重力補償パラメータ及び慣性力補償パラメータを減算する減算処理を行い（ステップＳ１０９）、減算値を算出する。

続いて、衝突検知部２４０は、減算部２３０が算出した減算値と、記憶部２８０に記憶された衝突閾値を比較し、減算値が衝突閾値を超えているか否かを判定する（ステップＳ１１１）。減算値が衝突閾値を超えていると判定した場合、衝突検知部２４０は、動作制御部２５０に衝突信号を出力し、動作制御部２５０は、ピッキングロボット１００の動作を停止させる（ステップＳ１１３）。コントローラ２００は、衝突検知部２４０が衝突を検知して動作制御部２５０がピッキングロボット１００の動作を停止させた際に、衝突が発生したことをインターフェース３００の表示制御装置に表示させる。

減算値が衝突閾値を超えていない（衝突閾値以下である）と衝突検知部２４０が判定した場合、動作制御部２５０は、搬送対象物Ｐが物品用コンテナＣ１から集荷用コンテナＣ２に搬送されて、搬送が完了したか否かを判定する（ステップＳ１１５）。搬送が完了していないと判定した場合、動作制御部２５０は、ステップＳ１０３の処理に戻る。搬送が完了したと判定した場合、コントローラ２００は、図５に示す処理を終了する。

次に、重力補償パラメータ及び慣性力補償パラメータを決定する手順について、衝突判定に利用する原理とともに説明する。重力補償パラメータ及び慣性力補償パラメータは、例えば、ピッキング装置１に重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を記憶させる前に、パラメータ推定装置（以下、「推定装置」という）を用いて、推定されて決定される。推定装置は、例えば、ピッキングロボット１００を実際に動作させて重力補償パラメータ及び慣性力補償パラメータを推定する。

実施形態では、６軸のロボットアームを備えるピッキングロボット１００のアーム部１１０を鉛直旋回３軸ロボットアームに見立てて説明を進める。実施形態では６次元のアーム部１１０を２次元の３軸ロボットアームに見立てて、二次元の回転行列を用いて説明を行うが、６次元のアーム部１１０についても、回転行列を二次元から三次元に変更するのみで、共通の原理でピッキングロボット１００の制御を行うことができる。

基本的な考え方として、まず、吸着ハンド１６０が空荷の状態のときに力センサ１５０にかかる重力と慣性力を完全に除去する。このために、オフライオンの最小二乗法に基づくフィッティング処理を行い、リアルタイムでの重力及び慣性力補償演算用のパラメータとして、重力補償パラメータ及び慣性力補償パラメータを推定して決定しておく。吸着ハンド１６０の質量が搬送対象物Ｐより大きい場合には、この場合の重力補償パラメータ及び慣性力補償パラメータを用いるのみでも衝突閾値を小さくすることができる。

次に、搬送対象物Ｐを把持した状態における重力と慣性力の差分を補償する原理について説明する。ここでの補償は、ロボットアームの動力学モデルは非線形であるが、質量や慣性を与える物理パラメータに関しては線形であり、慣性力や重力の補償について、重ね合わせの定理が成り立つことを利用するものである。

図６は、重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を作成する手順の一例を示すフローチャートである。まず、推定装置は、吸着ハンド１６０が空荷の状態（０［ｋｇ］の搬送対象物Ｐを搬送する状態）とする（ステップＳ２０１）。続いて、推定装置は、ピッキングロボット１００のアーム部１１０を極低速動作させて（ステップＳ２０３）、重力補償パラメータを推定する（ステップＳ２０５）。

アーム部１１０を極低速度で動作させると、力センサ１５０にかかる慣性力は、無視できる程度に小さくなるため、ピッキングロボット１００の動作に伴う力の影響として、重力の影響を取り出すことができる。説明に利用するベース座標系として、鉛直方向に沿ったＺ軸と水平方向に沿ったＸ軸を定義する。３軸のロボットアームに見立てたアーム部１１０の第１アーム１２１、第２アーム１２２、第３アーム１２３、及び手首部１２４がＺ軸に沿って同軸状に配置されるときの第１軸１１１の回転角度（以下、「第１回転角度」という）θ１、第２軸１１２の回転角度（以下、「第２回転角度」という）θ２、第３軸１１３の回転角度（以下、「第３回転角度」という）θ３をそれぞれ０°とする。第１回転角度θ１、第２回転角度θ２、及び第３回転角度θ３は、いずれも図１の方向に見て反時計回りの角度を示す。第１回転角度θ１、第２回転角度θ２、及び第３回転角度θ３は、角度データの一例である。

さらに、アーム部１１０先端の作業座標系（吸着ハンド１６０の作業座標系）として、先端方向をＸ_Ｅ軸、下向きをＺ_Ｅ軸と定義する。このときのアーム部１１０先端の作業座標系は、下記（１）式で表される。

上記（１）式から、力センサ１５０にかかる質量による重力は、ｃ₁₂₃，ｓ₁₂₃の関数となることは明らかである。力センサ１５０の座標系は、アーム部１１０先端の作業座標系と同一であり、力センサ１５０は、Ｘ_Ｅ方向にかかる第１力ｆｘ［Ｎ］と、Ｚ_Ｅ方向にかかる第２力ｆｚ［Ｎ］を検出する。したがって、重力補償パラメータを定義すると、Ｘ_Ｅ方向の第１力ｆｘ及びＺ_Ｅ方向の第２力ｆｚと、第１回転角度θ１～第３回転角度θ３との関係は、下記（２）式及び（３）式で表すことができる。
ｆｘ＝ｇ_xss₁₂₃＋ｇ_xcc₁₂₃＋ｇ_xf ・・・（２）
ｆｚ＝ｇ_zss₁₂₃＋ｇ_zcc₁₂₃＋ｇ_zf ・・・（３）

上記（２）式及び（３）式における右辺の第３項は、幾何学的誤差などを吸収するために設けたオフセットである。上記（２）式及び（３）式を用いて、ピッキングロボット１００の極低速時の第１回転角度～第３回転角度と、力センサ１５０が検出した力に基づくデータを最小二乗法でフィッティングすることにより、重力補償パラメータを算出することができる。重力補償パラメータｇ_xs、ｇ_xc、ｇ_xf、ｇ_zs、ｇ_zc、ｇ_zfは、吸着ハンド１６０を極低速動作させた場合の第１回転角度θ１、第２回転角度θ２、及び第３回転角度θ３に基づいて推定されて決定される。

図７は、第１軸１１１を極低速で往復動作させたときのＸ_Ｅ方向の第１力ｆｘの時間変化を最小二乗法でフィッティングした場合の一例を示すグラフ、図８は、そのＺ_Ｅ方向の第２力ｆｚの一例を示すグラフである。図７では、第１力ｆｘを実線Ｌ１１で示し、フィッティングさせたグラフを破線Ｌ２１で示す。図８では、第２力ｆｚを実線Ｌ１２で示し、フィッティングさせたグラフを破線Ｌ２２で示す。ここでは、第１軸１１１は、第１回転角度が０°から９０°を経て０°に戻るように往復動作させられている。図７及び図８に示す変化が得られた場合には、重力補償パラメータｇ_xs、ｇ_xc、ｇ_xf、ｇ_zs、ｇ_zc、ｇ_zfは、それぞれ以下の数値となる。

ｇ_xs＝－２．２８５６ｅ＋０１
ｇ_xc＝－５．３１０６ｅ－０１
ｇ_xf＝－４．０１４４ｅ－００
ｇ_zs＝４．５８６７ｅ－０１
ｇ_zc＝－２．２２８７ｅ＋０１
ｇ_zf＝－１．９２１２ｅ＋００

続いて、推定装置は、ピッキングロボット１００のアーム部１１０を高加減速動作させて（ステップＳ２０７）、慣性力補償パラメータを推定する（ステップＳ２０９）。ここでは、推定装置は、例えば３～４０ｍ／ｓ^２の加減速度かつ例えば１００％の高速でアーム部１１０を高加減速で、例えば、第１回転角度θ１＝第２回転角度θ２＝第３回転角度θ３＝０°の状態から、第１回転角度θ１＝第２回転角度θ２＝第３回転角度θ３＝９０°の状態を経て第１回転角度θ１＝第２回転角度θ２＝第３回転角度θ３＝０°とする往復動作を行わせる。

慣性力補償パラメータを推定する際に利用する加速度は、吸着ハンド１６０自体の重心位置の加速度に設定する。図９は、第１軸１１１～第３軸１１３を往復動作させたときの重心位置のＸ_Ｅ方向の加速度目標値と実加速度の一例を示すグラフ、図１０は、そのＺ_Ｅ方向の加速度目標値と実加速度の一例を示すグラフである。図９では、加速度目標値を破線Ｌ２３で示し、実加速度ａ_ｘを実線Ｌ１３で示す。図１０では、加速度目標値を破線Ｌ２４で示し、実加速度ａ_ｚを実線Ｌ１４で示す。この例では、実加速度が目標加速度によく追従しているので、加速度として、ノイズが少ない加速度目標値を用いて慣性力補償パラメータｍ_1x、ｍ_1z、ｍ_1o、ｍ_2x、ｍ_2z、ｍ_2oを推定する。

アーム部１１０先端の作業座標系でのＸ_Ｅ方向及びＺ_Ｅ方向の力と加速度の関係は、下記（４）式及び（５）式により表すことができる。図９及び図１０に示す高加減速高速動作時の加速度のデータを下記（４）式及び（５）式に基づいて最小二乗法によりフィッティングすることにより、慣性力補償パラメータを推定することができる。（４）式及び（５）式において、重心位置のＸ_Ｅ方向の加速度を第１加速度ａ_xとし、重心位置のＺ_Ｅ方向の加速度を第２加速度ａ_zとする。

ｆｘ＝ｍ_1xａ_x＋ｍ_1zａ_z＋ｍ_1o ・・・（４）
ｆｚ＝ｍ_2xａ_x＋ｍ_2zａ_z＋ｍ_2o ・・・（５）

図１１及び図１２は、図９及び図１０に示す高加減速高速動作時の加速度のデータを上記（４）式及び（５）式に基づいてフィッティングした結果を示すグラフである。図１１では、図９に示す加速度について、フィッティング前の加速度を実線Ｌ１５で示し、フィッティング後の加速度を破線Ｌ２５で示す。図１２では、図１０に示す加速度について、フィッティング前の加速度を実線Ｌ１６で示し、フィッティング後の加速度を破線Ｌ２６で示す。図１１及び図１２に示す変化が得られた場合には、慣性力補償パラメータｍ_1x、ｍ_1z、ｍ_1o、ｍ_2x、ｍ_2z、ｍ_2oは、それぞれ以下の数値となる。慣性力補償パラメータｍ_1x、ｍ_1z、ｍ_1o、ｍ_2x、ｍ_2z、ｍ_2oは、吸着ハンド１６０を高加減速動作させた場合のアーム部１１０の重心位置の加速度データに基づいて推定されて決定される。

ｍ_1x＝－２．０５９５ｅ－０１
ｍ_1z＝－２．２４４９ｅ＋００
ｍ_1o＝３．６７８１ｅ＋００
ｍ_2x＝２．３６８８ｅ＋００
ｍ_2z＝－２．１８２２ｅ－０１
ｍ_2o＝－３．５３１４ｅ－０１

吸着ハンド１６０の重心位置は、おおよその位置に設定すればよく、厳密な位置に設定しなくてもよい。吸着ハンド１６０の重心位置にある程度の誤差があったとしても、上記（４）式及び（５）式の枠組みの中で吸収することができる。
以上が、吸着ハンド１６０が空荷の状態（０［ｋｇ］の搬送対象物Ｐを搬送する状態）での重力補償パラメータ及び慣性力補償パラメータの推定手順である。

続いて、推定装置は、吸着ハンド１６０が搬送する搬送対象物Ｐの質量が１０［ｋｇ］になるまで（ステップＳ２１１）、何らかの方法で質量が１［ｋｇ］加算された搬送対象物Ｐについて、質量が１［ｋｇ］加算された（ステップＳ２１３）重力補償パラメータ及び慣性力補償パラメータの推定の処理（ステップＳ２０３～Ｓ２０９）を繰り返す。

上記の質量の１［ｋｇ］加算を繰り返し、搬送対象物Ｐの質量が１０［ｋｇ］に達すれば、推定装置は、ステップＳ２０５及びステップＳ２０９を経て推定した重力補償パラメータ及び慣性力補償パラメータに基づいて、重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を作成する（ステップＳ２１５）。こうして、推定装置は、図６に示す処理を終了する。

以下、ピッキング装置１におけるコントローラ２００による衝突検知の一例を示す。図１３は、力センサ１５０が検出した力に対して、重力補償と慣性力補償を施した際の第１力ｆｘの残差波形の一例を示すグラフ、図１４は、その第２力ｆｚの残差波形の一例を示すグラフである。ここで力センサ１５０が検出した力は、例えば、５［Ｎ］～１０［Ｎ］程度である。

図１３及び図１４から分かるように、第１力ｆｘ及び第２力ｆｚのいずれにおいても、重力補償及び慣性力補償をした後の力は５［Ｎ］～１０［Ｎ］の範囲に収まっていた。この結果、吸着ハンド１６０が空荷の際の衝突検知にあたって、重力や慣性力の影響を考慮する必要が無くなるので、衝突閾値２８６を大きく、例えば重力や慣性力の影響を考慮する場合の１／１０程度まで引き下げることができる。

例えば、吸着ハンド１６０と搬送対象物Ｐの質量が合計で１０［ｋｇ］である場合の付加を１０［ｍ／ｓ^２］（およそ１［Ｇ］）で加速すると、およそ１００［Ｎ］の慣性力が力センサ１５０により検出される。このため、そのまま衝突検知を行おうとすると、衝突検知のための衝突閾値を大きく設定する必要があり、検知精度の低下を招く恐れがある。さらに、力センサ１５０には重力もかかる。力センサ１５０によって検出された検出値から重力及び慣性力を除去して衝突検知を行うためにカルマンフィルタに基づく方法が提案されているが、演算量が多くなる上に未知質量の搬送対象物Ｐを把持した場合に、カルマンフィルタの過渡的な性能の頑健性が問題になる。この点、上記の実施形態では、予め定められた重力補償パラメータ及び慣性力補償パラメータを用いた演算処理を利用して衝突検知を行う。このため、吸着ハンド１６０が搬送対象物Ｐを搬送している場合にも同様に、衝突閾値２８６を引き下げることができ、検知精度の低下を抑制できる。さらに、コントローラ２００に組込むＣＰＵでリアルタイムに演算できる程度に簡易かつ頑健に衝突検知を行うことができる。

ピッキング装置１は、吸着ハンド１６０が搬送対象物Ｐを把持している搬送している状態では、吸着ハンド１６０が搬送対象物Ｐを把持した瞬間の前後における力センサ１５０の検出結果に応じた力の差分に基づいて、搬送対象物Ｐの質量を算出することができる。このため、ピッキング装置１は、重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を参照して搬送対象物Ｐの質量に応じた重力補償パラメータ及び慣性力補償パラメータを選択する。重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４は、それぞれ０［ｋｇ］～１０［ｋｇ］までの１［ｋｇ］刻みの質量について、予め、それぞれ６個のセットを１１通り作成された重力補償パラメータと慣性力補償パラメータを含む。このため、搬送対象物Ｐの質量が予め求められていない場合でも、ピッキングロボット１００の衝突を簡易に検知できる。１［ｋｇ］刻みの搬送対象物Ｐを把持する際の慣性力補償パラメータでは、加速度が作用する重心位置が変化することもあるが、質量の刻みと（４）式及び（５）式の枠組みの中で、これらの変化を吸収することができる。

上記の実施形態では、重力補償パラメータ及び慣性力補償パラメータを１［ｋｇ］ごとに推定して重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を作成したが、重力補償パラメータ及び慣性力補償パラメータを推定する単位を０．１［ｋｇ］ごと、０．５［ｋｇ］ごとなど細かく刻んでもよいし、２［ｋｇ］ごとなど大きくしてもよい。重力補償パラメータ及び慣性力補償パラメータを推定する質量の単位は、一定間隔でなくてもよいし、重力補償パラメータと慣性力補償パラメータで異なっていてもよい。重力補償パラメータ及び慣性力補償パラメータは、搬送対象物Ｐの質量等に応じて、ユーザが任意に決定できるようにしてもよい。上記の実施形態では、重力補償パラメータテーブル２８２及び慣性力補償パラメータテーブル２８４を参照して重力補償パラメータ及び慣性力補償パラメータを決定するが、予め重力補償パラメータ及び慣性力補償パラメータを規定した演算式を搬送対象物Ｐに質量に応じて決定しておいてもよい。実施形態では、制御部の一例である算出部２２０、減算部２３０、衝突検知部２４０、及び動作制御部２５０は、ピッキングロボット１００と別体のコントローラ２００に設けられるが、制御部は、ピッキングロボット１００に設けられてもよい。

以上説明した少なくともひとつの実施形態によれば、物体をピッキングして把持する把持部と、前記把持部を移動させて前記把持部に搬送対象物を搬送させるアーム部と、前記アーム部に取り付けられ、前記把持部にかかる力を検出する検出部と、前記把持部及び前記アーム部の動作を制御する制御部と、を備え、前記制御部は、前記搬送対象物の質量に応じて定められた係数を含む演算式を用いて、前記把持部が搬送対象物を把持して移動させる際に前記把持部にかかる重力及び慣性力を算出する算出部と、前記検出部が検出した前記把持部にかかる力から前記算出部が算出した前記質量及び前記慣性力を減算する減算部と、を備える、ピッキング装置であるので、搬送対象物の搬送中に発生する衝突を簡易に検知することができる。

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

１…ピッキング装置、１００…ピッキングロボット、１１０…アーム部、１１１…第１軸、１１２…第２軸、１１３…第３軸、１１４…第４軸、１１５…第５軸、１１６…第６軸、１２１…第１アーム、１２２…第２アーム、１２３…第３アーム、１２４…手首部、１５０…力センサ、１６０…吸着ハンド、２００…コントローラ、２１０…通信部、２２０…算出部、２２２…取得部、２２４…重力補償部、２２６…慣性力補償部、２３０…減算部、２４０…衝突検知部、２５０…動作制御部、２８０…記憶部、２８２…重力補償パラメータテーブル、２８４…慣性力補償パラメータテーブル、２８６…衝突閾値、３００…インターフェース

Claims (8)

1. 搬送対象物をピッキングして把持する把持部と、
   前記把持部を移動させて前記把持部に前記搬送対象物を搬送させるアーム部と、
   前記アーム部に取り付けられ、前記把持部にかかる力を検出する検出部と、
   前記把持部及び前記アーム部の動作を制御する制御部と、
   前記制御部は、
   最小二乗法に基づくフィッティング処理を行うことにより、重力補償演算用のパラメータ及び慣性力補償演算用のパラメータとしてそれぞれ搬送対象物の質量ごとに予め推定された重力補償パラメータ及び慣性力補償パラメータの中から、前記搬送対象物の質量に応じた係数として選択された前記重力補償パラメータ及び前記慣性力補償パラメータの一方を含むそれぞれの演算式を用いて、前記把持部が前記搬送対象物を把持して移動させる際に前記把持部にかかる重力及び慣性力を算出する算出部と、
   前記検出部が検出した前記把持部にかかる力から、前記算出部が算出した前記重力及び前記慣性力を減算する減算部と、
   前記減算部の減算結果と、予め定められた衝突閾値との比較結果に基づいて、衝突を検知する衝突検知部と、を備える、
   ピッキング装置。
2. 前記係数は、前記把持部が前記搬送対象物を把持しない空荷の場合の第１係数と、前記把持部に負荷がある場合の第２係数と、を備える、
   請求項１に記載のピッキング装置。
3. 前記係数は、前記把持部が前記搬送対象物を把持しない空荷の場合の第１係数と、前記把持部に把持される前記搬送対象物の質量に応じてそれぞれ区分された区分対応係数と、を備える、
   請求項１に記載のピッキング装置。
4. 前記搬送対象物の質量を取得する取得部と、
   前記取得部が取得した質量に応じた係数を選択する選択部と、を備える、
   請求項３に記載のピッキング装置。
5. 前記係数は、前記重力を算出するための重力算出係数と、前記慣性力を算出するための慣性力算出係数と、を含み、
   前記重力算出係数は、前記把持部を極低速動作させた場合の前記アーム部の角度データに基づいて決定される、
   請求項１に記載のピッキング装置。
6. 前記係数は、前記重力を算出するための重力算出係数と、前記慣性力を算出するための慣性力算出係数と、を含み、
   前記慣性力算出係数は、前記把持部を高加減速動作させた場合の前記アーム部の加速度データに基づいて決定される、
   請求項１に記載のピッキング装置。
7. 搬送対象物をピッキングして把持する把持部と、
   前記把持部を移動させて前記把持部に前記搬送対象物を搬送させるアーム部と、
   前記アーム部に取り付けられ、前記把持部にかかる力を検出する検出部と、を備えるピッキングロボットを制御する制御部と、
   前記搬送対象物の質量を取得する取得部と、を有し、
   前記制御部は、
   最小二乗法に基づくフィッティング処理を行うことにより、重力補償演算用のパラメータ及び慣性力補償演算用のパラメータとしてそれぞれ搬送対象物の質量ごとに予め推定された重力補償パラメータ及び慣性力補償パラメータの中から、前記搬送対象物の質量に応じた係数として選択された前記重力補償パラメータ及び前記慣性力補償パラメータの一方を含むそれぞれの演算式を用いて、前記把持部が前記搬送対象物を把持して移動させる際に前記把持部にかかる重力及び慣性力を算出する算出部と、
   前記検出部が検出した前記把持部にかかる力から、前記算出部が算出した前記重力及び前記慣性力を減算する減算部と、
   前記減算部の減算結果と、予め定められた衝突閾値との比較結果に基づいて、衝突を検知する衝突検知部と、を備える、
   制御装置。
8. 搬送対象物をピッキングして把持する把持部と、
   前記把持部を移動させて前記把持部に前記搬送対象物を搬送させるアーム部と、
   前記アーム部に取り付けられ、前記把持部にかかる力を検出する検出部と、を備えるピッキングロボットを制御し、
   前記搬送対象物の質量を取得する制御装置に、
   最小二乗法に基づくフィッティング処理を行うことにより、重力補償演算用のパラメータ及び慣性力補償演算用のパラメータとしてそれぞれ搬送対象物の質量ごとに予め推定された重力補償パラメータ及び慣性力補償パラメータの中から、前記搬送対象物の質量に応じた係数として選択された前記重力補償パラメータ及び前記慣性力補償パラメータの一方を含むそれぞれの演算式を用いて、前記把持部が前記搬送対象物を把持して移動させる際に前記把持部にかかる重力及び慣性力を算出させ、
   前記検出部が検出した前記把持部にかかる力から、前記重力及び前記慣性力を減算させ、
   前記減算の結果と、予め定められた衝突閾値との比較結果に基づいて、衝突を検知させる、
   プログラム。

Images