ワーク質量測定機能を備えたロボット
【課題】ハンドリング動作時であってもワークの質量を正確に取得する。
【解決手段】把持されたワークの質量を測定するワーク質量測定機能を備えたロボット（１）は、ロボット（１）の機構部の先端部（２）に作用する力を測定する力測定部（５）と、ロボット（１）が移動しているときに力測定部（５）により得られる力の情報に基づいて、ロボット（１）に把持されたワークの質量を推定する質量推定部（１１）とを含む。力測定部（５）は、ワークを把持しているときに測定した力測定値とワークを把持していないときに測定した力測定値とに基づいてワークの質量を推定するのが好ましい。
【解決手段】把持されたワークの質量を測定するワーク質量測定機能を備えたロボット（１）は、ロボット（１）の機構部の先端部（２）に作用する力を測定する力測定部（５）と、ロボット（１）が移動しているときに力測定部（５）により得られる力の情報に基づいて、ロボット（１）に把持されたワークの質量を推定する質量推定部（１１）とを含む。力測定部（５）は、ワークを把持しているときに測定した力測定値とワークを把持していないときに測定した力測定値とに基づいてワークの質量を推定するのが好ましい。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、力測定部を備えたロボットにより把持したワークの質量を測定する機能を備えたロボットに関する。
【背景技術】
【０００２】
生産中の部品や製品の不良品検査を行うために、力測定部を備えたロボットを用いてワークの質量を測定することが行われている。具体的には、ロボット手首部に力測定部が備えられたロボットのハンドがワークを把持する。そして、このときの力測定部の出力値からワークの質量を測定している。
【０００３】
このような質量の測定は、例えば、一つのみのワークを把持すべきであるにも関わらず、搬送中に複数のワークを把持したか否かを確認するときに行われる。あるいは、そのような質量の測定は、単一のワークの質量が所定の範囲内にあるか否かを通じてワークの良否を判定するときにも行われる。
【０００４】
特許文献１には、質量の測定値に基づいてワークの良品検査、部品装着検査、種類検査を行い、それにより、ワークを仕分けすることが開示されている。さらに、特許文献１においては、ワークの質量を測定するために、ワークの重量が力測定部に垂直にかかるようにロボット動作をプログラミングし、そのときの力測定部の出力と、所定の基準質量との間の差を求めている。
【０００５】
また、特許文献２は、ロボットハンドの上方および下方にそれぞれ一対の力測定部（ロードセル）を備えた構成を開示している。そして、ワークを上方および下方から把持したときの力測定部のそれぞれの出力からワークの重量を測定している。特許文献２においては、ハンドがワークを把持している状態で且つ一対の荷重センサが鉛直方向にある状態でワークを静止させ、下側のロードセルの重量値から上側のロードセルの重量値を減じることによりワークの質量を求めている。
【０００６】
ところで、図７（ａ）および図７（ｂ）は従来技術におけるロボットの側面図である。これら図面に示されるロボットは、垂直多関節ロボットであり、ロボットのアームの先端部２にはロボットハンド３が取付けられている。そして、アームの先端部２とハンド３との間には力センサ５が配置されている。
【０００７】
はじめに、図７（ａ）に示されるように、ワークＷを把持しない状態でロボット１を静止させ、力センサ５の出力を記録する。次いで、図７（ｂ）に示されるように、ハンド３がワークＷを把持する動作のみを行って（ロボット１の位置姿勢を変えることなしに）、静止状態で、力センサ５の出力を記録する。そして、力センサ５の二つの出力の偏差に基づいてワークＷの質量を求めている。
【０００８】
ここで、力センサ５が一軸のロードセルである場合には、図７（ａ）および図７（ｂ）に示されるように、力センサ５の計測時に力センサ５は鉛直方向下向きであることが要求される。ただし、力センサ５が三軸以上の力センサである場合には、ワークＷを把持していないときおよびワークを把持しているときの位置姿勢が同じであれば、力センサ５が鉛直方向下向き以外の姿勢であってもよい。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開平６−３１６６７号公報
【特許文献２】特開平５−５７６６７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
前述したように、ワークＷの質量を測定するときには、ロボット１が静止している必要がある。その理由は、ロボット１の動作時にはハンド３及びワークＷの重力だけでなく、ロボット１の加減速によって生じる力、遠心力、コリオリ力等の慣性力も力センサ５によって検出されるためである。そして、そのような重力および慣性力が作用しないようにロボット１を静止させると、ロボット１の減速時間、静止時間、再加速時間が必要となる。それゆえ、ロボット１のサイクルタイムがかなり増大することになる。
【００１１】
さらに、特許文献１および特許文献２においても、ワークの測定時には、ワークの重量が垂直に力測定部にかかるようにロボット１の姿勢を変更する必要がある。
【００１２】
本発明はこのような事情に鑑みてなされたものであり、ハンドリング動作時であってもワークの質量を正確に取得することのできる、ワーク質量測定機能を備えたロボットを提供することを目的とする。
【課題を解決するための手段】
【００１３】
前述した目的を達成するために１番目の発明によれば、把持されたワークの質量を測定するワーク質量測定機能を備えたロボットにおいて、前記ロボットの機構部の先端部に作用する力を測定する力測定部と、前記ロボットが移動しているときに前記力測定部により得られる力に基づいて、前記ロボットに把持されたワークの質量を推定する質量推定部と、を備えるロボットが提供される。
【００１４】
２番目の発明によれば、１番目の発明において、前記力測定部は、前記ワークを把持しているときに測定した力測定値と前記ワークを把持していないときに測定した力測定値とに基づいて前記ワークの質量を推定する。
【００１５】
３番目の発明によれば、１番目または２番目の発明において、前記ロボットが移動しているときに、前記機構部の先端部の姿勢を変化させないようにした。
【００１６】
４番目の発明によれば、１番目から３番目のいずれかの発明において、前記ロボットが移動しているときの複数の時刻において前記質量推定部が推定した前記ワークの複数の質量の平均値を質量推定値とする。
【００１７】
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は前記ロボットの機構部先端部に作用する加速度を測定または推定する加速度取得部を含んでおり、前記ロボットが移動しているときに前記力測定部により測定される力および前記加速度取得部により取得される加速度に基づいて、前記質量推定部は前記ロボットが把持したワークの質量を推定する。
【００１８】
６番目の発明によれば、５番目の発明において、前記加速度取得部は、動力学方程式により加速度を計算する。
【００１９】
７番目の発明によれば、５番目の発明において、前記加速度取得部は、前記ロボットに取付けられた加速度センサにより測定された加速度を取得する。
【００２０】
８番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定部は複数の方向において前記力を測定し、前記加速度取得部は前記複数の方向において前記加速度を取得し、前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記質量の平均値を質量推定値とする。
【００２１】
９番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定部は複数の方向において前記力を測定し、前記加速度取得部は前記複数の方向において前記加速度を取得し、前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記方向のうち、前記加速度の絶対値が最大である方向に対応する前記質量を質量推定値とする。
【００２２】
１０番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は、前記ロボットの機構部の先端部に作用する重力方向に対して垂直な平面上を前記機構部の先端部が移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした。
【００２３】
１１番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は、前記ロボットの機構部の先端部が一定速度で直線軌道を移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした。
【００２４】
１２番目の発明によれば、１０番目または１１番目の発明において、さらに、前記ロボットの機構部の先端部の姿勢を取得する姿勢取得部を具備し、前記質量推定部は、前記力測定部により測定された一方向以上の力と、前記姿勢取得部により取得された前記姿勢とに基づいて、前記ロボットが把持したワークの質量を推定する。
【発明の効果】
【００２５】
１番目の発明においては、ロボットを停止させることなしにワークの質量を算出することができる。従って、質量の測定に伴ってロボット作業のサイクルタイムが延びることはない。また、単一の力測定部で足りるので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることもできる。
２番目の発明においては、ハンドの質量が未知であっても、ワークの質量を求めることができる。
３番目の発明においては、ワークの質量を求めるに際し、外乱による影響を軽減することができる。
４番目の発明においては、ワークの質量をより正確に算出することができる。
５番目の発明においては、加速度を用いることにより、ワークの質量をより正確に算出することができる。
６番目の発明においては、加速度センサが不要であるので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用をさらに抑えられる。
７番目の発明においては、加速度をより正確かつ直接的に求められる。
８番目の発明においては、複数の方向、例えばＸＹＺの三方向での平均値を用いているので、ワークの質量をより正確に算出することができる。
９番目の発明においては、加速度の絶対値が大きい方向について質量を求めるので、ノイズなどの誤差要因の影響を軽減することができる。
１０番目の発明においては、重力方向に慣性力が作用するのを排除できる。
１１番目の発明においては、ロボットの加減速に必要な力、遠心力、コリオリ力などの慣性力を排除できる。
１２番目の発明においては、ロボットの姿勢を考慮することにより、ハンドを真下に向けるなどの特殊な姿勢でロボットを動作させる必要がない。
【図面の簡単な説明】
【００２６】
【図１】本発明に基づくロボットの側面図である。
【図２】（ａ）本発明の実施例１等に基づくロボットの動作を示す第一の図である。（ｂ）本発明の実施例１等に基づくロボットの動作を示す第二の図である。
【図３】本発明の実施例２等に基づくロボットの動作を示す図である。
【図４】ロボットの側面図である。
【図５】（ａ）本発明の実施例５等に基づくロボットの動作を示す第一の図である。（ｂ）本発明の実施例５等に基づくロボットの動作を示す第二の図である。
【図６】本発明の実施例９に基づくロボットの側面図である。
【図７】（ａ）従来技術におけるロボットの側面図である。（ｂ）従来技術におけるロボットの他の側面図である。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボットの側面図である。図１においてロボット１は垂直多関節ロボットまたは公知のロボットマニュピレータであり、そのアームの先端部２にはワークＷを把持するハンド３が取付けられている。なお、ハンド３はロボット１のアームの先端部２に取付けられている必要は必ずしもない。
【００２８】
ハンド３は、ワークＷの形状、材質、質量等に応じて適宜選択される。ハンド３は、吸引式、チャック式のいずれであってもよい。さらに、アームの先端部２とハンド３との間には力センサ５が配置されている。
【００２９】
図１においては、ロボット１の動作範囲内に二つのテーブルＴ１、Ｔ２が配置されていて、一方のテーブルＴ１にワークＷが載置されている。また、図１に示されるように、ロボット１全体を制御するロボット制御装置９がロボット１に接続されている。なお、力センサ５もロボット制御装置９に接続されるものとする。
【００３０】
ロボット制御装置９はデジタルコンピュータであり、ワークＷの質量を推定する質量推定部１１と、ロボットアームの先端部２に作用する加速度を算出する加速度取得部１２とを備えている。なお、加速度取得部１２の代わりに、加速度センサ６（後述する）がハンド３に取付けられていてもよい。さらに、図１に示されるように、ロボット制御装置９はロボットアームの先端部２の姿勢を取得する姿勢取得部１３を含んでいる。
【００３１】
さらに、図１においては、ロボット１のベースに固定された基準座標系Σ１と、力センサ５に固定された力センサ座標系Σ２とが示されている。基準座標系Σ１の軸方向と重力方向との関係は予めロボット制御装置９の記憶部（図示しない）に記憶しておく。なお、この記憶部には各種のデータが記憶されるものとする。
【００３２】
通常は基準座標系Σ１のＺ方向と重力方向とが一致するように設定しているが、他の方向に設定してもよい。また、図１および後述する図面において、重力は各図面の下方に向かって作用するものとする。
【００３３】
ロボットアームの先端部２が動作するにつれて基準座標系Σ１に対する力センサ座標系Σ２の位置／姿勢は変化する。或る時刻における力センサ座標系Σ２の位置／姿勢は、その瞬間のロボット１の各軸の角度やロボットリンク長等を使って公知の順運動学計算により算出することができる。
【００３４】
図２（ａ）および図２（ｂ）は、それぞれ本発明の実施例１等に基づくロボットの動作を示す第一および第二の図である。図２（ａ）においては、ロボット１がワークＷを把持していない状態で、テーブルＴ２からテーブルＴ１に向かって先端部２を或る軌道上に動作させている。このとき、力センサ５には、ハンド３の重力およびハンド３が運動することにより生ずる慣性力などが作用する。
【００３５】
また、図２（ｂ）においては、ロボット１がワークＷを把持している状態で、テーブルＴ１からテーブルＴ２に向かって先端部２を或る軌道上に動作させている。このとき、力センサ５には、ハンド３とワークＷの重力およびハンド３とワークＷが運動することにより生ずる慣性力などが作用する。
【００３６】
実施例１
実施例１においては、ハンド３の質量は別途計測することにより既知である。また、力センサ５として三軸以上の力センサを使用しており、そのため力センサ５は並進三成分の力を検出できるものとする。
【００３７】
図２（ｂ）においては、ロボット１のハンド３によりワークＷを把持しつつ、ロボット１およびワークＷが周辺機器などに接触することなしに動作する。このとき、図２（ｂ）に示されるようにロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（１）で表される。なお、手首を変化させない理由は、ロボットの振動などの外乱の影響を少なくするためである。
【００３８】
Ｆ＝（Ｍｈ＋Ｍｗ）・Ｒ・｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝ （１）
ここで、Ｆは力センサ５により検出された力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル、
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル、
ｄ2ｘ／ｄｔ2はｘの２階微分、
Ｃ（ｘ、ｄｘ／ｄｔ）はハンド３とワークＷに作用する遠心力やコリオリ力等の慣性力を表すベクトルであり、ｘおよびｄｘ／ｄｔの関数である。
【００３９】
式（１）はロボット１が、周辺機器などに接触していない限りにおいて任意の時刻について成立する。ロボット１の動作時の或る時刻における力ベクトルＦを記録する。そして、質量推定部１１が同じ時刻におけるｘをロボットリンク長やロボット１の各軸の角度などの情報から順運動学計算により算出する。次いで、質量推定部１１は、算出されたｘを用いて、ｇ、ｄ2ｘ／ｄｔ2およびＣ（ｘ、ｄｘ／ｄｔ）も算出する。なお、後述する演算も主に質量推定部１１が行うものとする。
【００４０】
式（１）の各項は三成分のベクトルなので三つの式から成り、それぞれからワーク質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００４１】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下の方法により、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、以下の式（２）よりＭｗを容易に且つ正確に求めることができる。
Ｍｗ＝ （Ｍｗｘ＋Ｍｗｙ＋Ｍｗｚ）／３ （２）
【００４２】
あるいは、ベクトル｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝の三成分のうち絶対値が最大の方向の質量を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００４３】
以上のようにして、或る一つの時刻における式（１）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、ロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のバラツキ、ロボット自体の揺れによる振動等の外乱による影響を軽減し、信頼性の高い質量Ｍｗを求めることができる。
【００４４】
実施例２
図３は本発明の実施例２等に基づくロボットの動作を示す図である。図から分かるように、実施例２は、加速度センサ６がハンド３の基部に取付けられている点で実施例１とは異なる。また、実施例２においても、力センサ５として三軸以上の力センサを使用しており、そのため力センサ５は並進三成分の力を検出できるものとする。前述した実施例１ではベクトル｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝を計算により求めていたが、実施例２では加速度センサ６の出力を利用して上記ベクトルを求める。
【００４５】
図２（ｂ）に示されるようにロボット１がワークＷを把持している状態で、テーブルＴ１からテーブルＴ２に向かって先端部２を或る軌道上に動作させる。このときの力センサ５の出力は以下の式（３）で表すことができる。
【００４６】
Ｆ＝（Ｍｈ＋Ｍｗ）・Ａ （３）
ここで、Ｆは力センサ５により検出された力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換したもの、である。
【００４７】
実施例１の場合と同様に、ロボット１の動作時において或る時刻における力ベクトルＦを記録する。このとき、或る時刻における加速度センサ６の出力ベクトルＡも同様に記録する。そして、ハンド３の質量Ｍｈは既知であるので、式（３）を用いることによりワークＷの質量Ｍｗを算出することができる。
【００４８】
式（３）の各項は三成分のベクトルなので三つの式から成り、それぞれからワーク質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００４９】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、前述した式（２）よりワークＷの質量Ｍｗを容易に求めることができる。
【００５０】
あるいは、ベクトルＡの三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、Ａ＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００５１】
以上のようにして、或る一つの時刻における式（３）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。また、実施例２においては、加速度センサ６を採用しているので、加速度をより正確かつ直接的に求められるのが分かるであろう。
【００５２】
実施例３
実施例３において使用される力センサ５は三軸以上の力センサであり、並進三成分の力を検出できるものとする。実施例３は後述する二つのステップにより測定するという点で、前述した実施例１とは異なる。すなわち、実施例３においては、ハンド３の質量も計算するので、ハンド３の質量を事前に測定する必要がない。
【００５３】
はじめに第一のステップを説明する。図２（ａ）に示されるように、ロボット１のハンド３がワークＷを把持していない状態で、周辺機器に接触することなしに、ロボット１のハンド３を空中に移動させる。ロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（４）で表される。
【００５４】
Ｆ１＝Ｍｈ・Ｒ・（ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）） （４）
ここで、Ｆ１は力センサ５で検知した力ベクトル（検出値）、
Ｍｈはハンドの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル
ｄ2ｘ／ｄｔ2はｘの２階微分
Ｃ（ｘ、ｄｘ／ｄｔ）はツールにかかる遠心力やコリオリ力による加速度ベクトルであり、ｘ、ｄｘ／ｄｔの関数である。
【００５５】
式（４）はロボット１が周辺機器などに接触しなければ任意の時刻において成立し、ロボット１が動作中のある時刻における力ベクトルＦ１を記録する。同じ時刻における位置ベクトルｘをロボットリンク長やロボットの各軸角度などの情報から順運動学計算により演算する。そして、位置ベクトルｘを用いて、さらにｇ、ｄ2ｘ／ｄｔ2、Ｃ（ｘ、ｄｘ／ｄｔ）も導出する。この場合には、加速度センサ６を設ける必要がないので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることが可能となる。
【００５６】
次いで、上記の情報から式（４）よりハンド３の質量Ｍｈを計算する。或る１つの時刻において式（４）からハンド３の質量Ｍｈを求めることに加えて、複数の時刻において式（４）からハンド３の質量Ｍｈを求めその平均値を演算してもよい。これにより、ロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減し、信頼性の高い質量Ｍｈを求めることができる。
【００５７】
次いで、第二のステップを説明する。図２（ｂ）に示されるように、ロボット１のハンド３がワークＷを把持した状態で、周辺機器に接触することなしに、ロボット１のハンド３を空中に移動させる。ロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（５）で表される。
【００５８】
Ｆ２＝（Ｍｈ＋Ｍｗ）・Ｒ・（ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）） （５）
ここで、Ｆ２は力センサで検知した力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル、
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル、
ｄ2ｘ／ｄｔ2はｘの２階微分、
Ｃ（ｘ、ｄｘ／ｄｔ）はツールとワークにかかる遠心力やコリオリ力をあらわすベクトルでｘ、ｄｘ／ｄｔの関数である。
【００５９】
前述した第一のステップと同様に或る時刻におけるｘ、ｇ、ｄ2ｘ／ｄｔ2、Ｃ（ｘ、ｄｘ／ｄｔ）を計算する。ハンド３の質量Ｍｈは第一のステップにおいて既に定まっている。従って、式（５）を使ってワークＷの質量Ｍｗを導出する。
【００６０】
式（５）の各項は三成分のベクトルなので三つの式から成り、それぞれから質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００６１】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、前述した式（２）よりＭｗを容易に求めることができる。
【００６２】
あるいは、ベクトル（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ）の三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ）＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００６３】
以上のようにして、或る一つの時刻における式（５）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。
【００６４】
実施例４
図３を参照して前述したように三軸の加速度センサ６をハンド３に取付ける。前述した実施例３においてはロボットリンク長やロボットの各軸角度などから加速度（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ））を求めたが、実施例４においては加速度センサ６の出力を使用する。
【００６５】
実施例４においても実施例３と同様に三軸以上の力センサ５を使用し、並進三成分の力が検出できるものとする。実施例４は後述する二つのステップにより測定するという点で、前述した実施例２とは異なる。すなわち、実施例４においては、ハンド３の質量を計算する。
【００６６】
はじめに第一のステップを説明する。図２（ａ）に示されるように、ロボット１のハンド３がワークＷを把持していない状態で、周辺機器などに接触することなしにハンド３が空中を移動する。その際の力センサ５の出力は以下の式（６）で表される。
【００６７】
Ｆ１＝Ｍｈ・Ａ （６）
ここで、Ｆ１は力センサ５で検知した力ベクトル（検出値）
Ｍｈはハンド３の質量
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換した値である。
【００６８】
式（６）はロボット１が周辺機器などに接触していなければ任意の時刻について成立し、或る時刻における力センサ５の力ベクトルＦ１と加速度センサ６に関する値Ａとを記録する。そして、これらの情報を用いて式（６）からハンド３の質量Ｍｈを算出する。
【００６９】
或る一つの時刻において式（６）からハンド３の質量Ｍｈを求めることに加えて、複数の時刻においてハンド３の質量Ｍｈを求め、その平均値を算出してもよい。そうすることでロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００７０】
次いで、第二のステップを説明する。図２（ｂ）に示されるように、ロボット１でワークＷを把持した状態で周辺機器などに接触させることなしハンド３を空中に動作させる。その際の力センサ５の出力は以下の式（７）で表すことができる。
【００７１】
Ｆ２＝（Ｍｈ＋Ｍｗ）・Ａ （７）
ここで、Ｆ２は力センサ５で検知した力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換した値である。
【００７２】
第一のステップの場合と同様に、或る時刻における力センサ５の力ベクトルＦ２と加速度センサ６に関する値Ａとを記録する。ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（７）を使用すれば、ワークＷの質量Ｍｗを求めることができる。
【００７３】
式（７）の各項は三成分のベクトルなので三つの式から成り、それぞれから質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００７４】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚの平均値を算出することであり、前述した式（２）よりＭｗを容易に求めることができる。
【００７５】
あるいは、ベクトルＡの三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、Ａ＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００７６】
以上のようにして、或る一つの時刻における式（７）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。
【００７７】
後述する実施例５〜実施例８においては、力測定部として、一軸以上のロードセルまたは力センサのいずれを採用してもよい。一軸ロードセルを使用する場合、ロードセルが力を測定できる１方向のことを以下では測定方向と呼ぶ。ロボット手首が鉛直下向き姿勢のときに測定方向と重力方向が一致するようにロードセルをロボット手首に取付ける（ロボットの側面図である図４を参照されたい）。
【００７８】
ロボットのアームの先端部２が動作するにつれて基準座標系Σ１に対する力センサ座標系Σ２またはロードセル５の位置／姿勢は変化する。姿勢取得部１３は、ある時刻における力センサ座標系Σ２またはロードセル５の位置／姿勢を、その瞬間のロボットの各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。
【００７９】
図５（ａ）は本発明の実施例５等に基づくロボットの動作を示す第一の図である。図５（ａ）においては、ロボット１がワークＷを把持していない状態で、重力方向に対して垂直な平面上をロボットのアームの先端部２が動作している様子を示す。力センサ５にはハンド３の重力およびハンド３が運動することにより生ずる慣性力などが作用する。しかしながら、重力方向については、慣性力の成分は作用しないので、重力のみが作用する。
【００８０】
図５（ｂ）は本発明の実施例５等に基づくロボットの動作を示す第二の図である。図５（ｂ）においては、ロボット１がワークＷを把持している状態で、重力方向に対して垂直な平面上をロボットのアームの先端部２が動作している様子を示す。力センサ５にはハンド３とワークＷの重力およびハンド３とワークＷが運動することにより生ずる慣性力などが作用する。しかしながら、重力方向については、慣性力の成分は作用しないので、重力のみが作用する。
【００８１】
実施例５
実施例５ではハンド３の質量はあらかじめ測定して既知とし、力測定部５として１軸以上のロードセル５を使用する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つ、ロボットの手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（８）が成立する。
【００８２】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ・ｃｏｓθ （８）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度、である（例えば図４における角度θは０°である）。
【００８３】
式（８）においては、重力方向に対して垂直な平面上を動作している限り任意の時刻において成立する。そして、そのときの或る時刻における力Ｆを記録する。
【００８４】
姿勢取得部１３は、基準座標系Σ１に対するロードセル５の位置／姿勢を、その瞬間のロボット１の各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。基準座標系Σ１に対する重力方向もわかっているのでその瞬間におけるθも計算できる。そして、ハンド３の質量Ｍｈは既知であるので、式（８）からワークの質量Ｍｗを算出できる。この場合には、重力方向に慣性力が作用するのを排除できるので、ワークＷの質量はより正確になる。
【００８５】
或る一つの時刻において式（８）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００８６】
実施例６
実施例６においてはハンド３の質量はあらかじめ測定して既知とし、力測定部５として三軸以上の力センサを使用する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（９）が成立する。
【００８７】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ （９）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度である。
【００８８】
前述した式（８）は計測方向に関する式であるが、式（９）は重力方向についての式である。式（９）は重力方向に対して垂直な平面上を動作している限り任意の時刻において成立し、或る時刻における力センサ５の出力を記録する。
【００８９】
力センサ５の出力は力センサ座標系Σ２における値でＸ、Ｙ、Ｚの三成分ある。上述のように、基準座標系Σ１に対する力センサ座標系Σ２、重力方向ともにわかっているのでその瞬間における、力センサ５の出力の重力方向成分Ｆも計算することができる。そして、ハンド３の質量Ｍｈは既知としているので、式（９）からＭｗを計算する。
【００９０】
或る一つの時刻において式（９）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００９１】
実施例７
実施例７においては、力測定部として一軸以上のロードセル５を使用する。実施例７は後述する二つのステップにより測定するという点で、前述した実施例５とは異なる。すなわち、実施例７においては、ハンド３の質量を計算する。
【００９２】
はじめに第一のステップを説明する。図４（ａ）に示されるように、ロボット１がワークＷを把持していない状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上に動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。このとき、力について以下の式（１０）が成立する。
【００９３】
Ｆ＝Ｍｈ・ｇ・ｃｏｓθ （１０）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度である。
【００９４】
式（１０）は重力方向に対して垂直な平面上を動作している限り任意の時刻において成立する。そして、そのときの或る時刻における力Ｆを記録する。
【００９５】
姿勢取得部１３は、基準座標系Σ１に対するロードセル５の位置／姿勢を、その瞬間のロボット１の各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。基準座標系Σ１に対する重力方向もわかっているのでその瞬間における角度θも計算できる。これらの情報を使って式（１０）からハンド３の質量Ｍｈを計算する。
【００９６】
或る一つの時刻において式（１０）よりハンド３の質量Ｍｈを求めることができる。複数の時刻におけるハンド３の質量Ｍｈを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００９７】
次いで、第二のステップを説明する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（１１）が成立する。
【００９８】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ・ｃｏｓθ （１１）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
Ｍｗはワーク５の質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度である。
【００９９】
前述した第一のステップの場合と同様に、或る時刻における力Ｆを記録し、角度θを計算する。そして、ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（１１）からワークＷの質量Ｍｗを計算する。
【０１００】
或る一つの時刻において式（１１）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１０１】
実施例８
実施例８においては、力測定部５として三軸以上の力センサ５を使用する。実施例８は後述する二つのステップにより測定するという点で、前述した実施例６とは異なる。すなわち、実施例８においては、ハンド３の質量を計算する。
【０１０２】
はじめに、第一のステップについて説明する。図４（ａ）に示されるように、ロボット１がワークＷを把持していない状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上に動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。このとき、力について以下の式（１２）が成立する。
【０１０３】
Ｆ＝Ｍｈ・ｇ （１２）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ｇは重力加速度である。
【０１０４】
前述した式（８）は計測方向についての式であるが、式（１２）は重力方向の式である。式（１２）は、重力方向に対して垂直な平面上を動作している限り任意の時刻において成立し、そのときの或る時刻における力センサ５の出力を記録する。
【０１０５】
力センサ５の出力は力センサ座標系Σ２における値でＸ、Ｙ、Ｚの三成分ある。上述のように、基準座標系Σ１に対する力センサ座標系Σ２、重力方向ともにわかっているのでその瞬間における、力センサ５の力の重力方向成分Ｆも計算することができる。これらの情報を使って式（１２）からハンド３の質量Ｍｈを計算する。
【０１０６】
或る一つの時刻において式（１２）よりハンド３の質量Ｍｈを求めることができる。複数の時刻におけるハンド３の質量Ｍｈを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１０７】
次いで、第二のステップについて説明する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（１３）が成立する。
【０１０８】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ （１３）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度である。
【０１０９】
前述した第一のステップの場合と同様に、或る時刻における力の成分Ｆを計算する。ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（１３）を用いてワークＷの質量Ｍｗを計算することができる。
【０１１０】
或る一つの時刻において式（１３）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１１１】
実施例９
図６は本発明の実施例９に基づくロボットの側面図である。実施例５〜実施例８においては、アームの先端部２に作用する重力方向に対して垂直な平面上をロボットアーム先端部２が移動していた。これに対し、実施例９では、図６に示されるように、ロボットアームの先端部を一定速度で直線軌道上を移動させる。
【０１１２】
図６から分かるように、この直線軌道は必ずしも重力方向に対して垂直な平面上になくてもよい。一定速度かつ直線軌道に限定することで加減速に必要な力、遠心力、コリオリ力などの慣性力が力測定部５に作用しなくなる。従って、より正確にワークＷの質量Ｍｈを求められるのが分かるであろう。なお、ワークＷの質量Ｍｗの計算方法は実施例５〜実施例８と同様であるので説明を省略する。
【０１１３】
このように、本発明においては、ロボット１を停止させることなしにワークＷの質量を算出することができる。このため、ロボット１の減速時間、静止時間、再加速時間が不要となる。従って、質量の測定に伴ってロボット作業のサイクルタイムが延びることはない。
【０１１４】
さらに、ロボット１が任意の起動で動作しても、動力学項の影響を受けること無しに、ワークＷの質量を正確に取得することができる。また、本発明では複数の力測定部は必要でなく、単一の力測定部で足りる。このため、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることが可能となる。なお、異なる実施例であっても、同様の構成であれば同様の効果が得られるのは当業者であれば明らかであろう。
【符号の説明】
【０１１５】
１ ロボット
２ アームの先端部（機構部の先端部）
３ ハンド
５ 力測定部、力センサ、ロードセル
６ 加速度センサ
９ ロボット制御装置
１１ 質量推定部
１２ 加速度取得部
１３ 姿勢取得部
Ｗ ワーク
【技術分野】
【０００１】
本発明は、力測定部を備えたロボットにより把持したワークの質量を測定する機能を備えたロボットに関する。
【背景技術】
【０００２】
生産中の部品や製品の不良品検査を行うために、力測定部を備えたロボットを用いてワークの質量を測定することが行われている。具体的には、ロボット手首部に力測定部が備えられたロボットのハンドがワークを把持する。そして、このときの力測定部の出力値からワークの質量を測定している。
【０００３】
このような質量の測定は、例えば、一つのみのワークを把持すべきであるにも関わらず、搬送中に複数のワークを把持したか否かを確認するときに行われる。あるいは、そのような質量の測定は、単一のワークの質量が所定の範囲内にあるか否かを通じてワークの良否を判定するときにも行われる。
【０００４】
特許文献１には、質量の測定値に基づいてワークの良品検査、部品装着検査、種類検査を行い、それにより、ワークを仕分けすることが開示されている。さらに、特許文献１においては、ワークの質量を測定するために、ワークの重量が力測定部に垂直にかかるようにロボット動作をプログラミングし、そのときの力測定部の出力と、所定の基準質量との間の差を求めている。
【０００５】
また、特許文献２は、ロボットハンドの上方および下方にそれぞれ一対の力測定部（ロードセル）を備えた構成を開示している。そして、ワークを上方および下方から把持したときの力測定部のそれぞれの出力からワークの重量を測定している。特許文献２においては、ハンドがワークを把持している状態で且つ一対の荷重センサが鉛直方向にある状態でワークを静止させ、下側のロードセルの重量値から上側のロードセルの重量値を減じることによりワークの質量を求めている。
【０００６】
ところで、図７（ａ）および図７（ｂ）は従来技術におけるロボットの側面図である。これら図面に示されるロボットは、垂直多関節ロボットであり、ロボットのアームの先端部２にはロボットハンド３が取付けられている。そして、アームの先端部２とハンド３との間には力センサ５が配置されている。
【０００７】
はじめに、図７（ａ）に示されるように、ワークＷを把持しない状態でロボット１を静止させ、力センサ５の出力を記録する。次いで、図７（ｂ）に示されるように、ハンド３がワークＷを把持する動作のみを行って（ロボット１の位置姿勢を変えることなしに）、静止状態で、力センサ５の出力を記録する。そして、力センサ５の二つの出力の偏差に基づいてワークＷの質量を求めている。
【０００８】
ここで、力センサ５が一軸のロードセルである場合には、図７（ａ）および図７（ｂ）に示されるように、力センサ５の計測時に力センサ５は鉛直方向下向きであることが要求される。ただし、力センサ５が三軸以上の力センサである場合には、ワークＷを把持していないときおよびワークを把持しているときの位置姿勢が同じであれば、力センサ５が鉛直方向下向き以外の姿勢であってもよい。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開平６−３１６６７号公報
【特許文献２】特開平５−５７６６７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
前述したように、ワークＷの質量を測定するときには、ロボット１が静止している必要がある。その理由は、ロボット１の動作時にはハンド３及びワークＷの重力だけでなく、ロボット１の加減速によって生じる力、遠心力、コリオリ力等の慣性力も力センサ５によって検出されるためである。そして、そのような重力および慣性力が作用しないようにロボット１を静止させると、ロボット１の減速時間、静止時間、再加速時間が必要となる。それゆえ、ロボット１のサイクルタイムがかなり増大することになる。
【００１１】
さらに、特許文献１および特許文献２においても、ワークの測定時には、ワークの重量が垂直に力測定部にかかるようにロボット１の姿勢を変更する必要がある。
【００１２】
本発明はこのような事情に鑑みてなされたものであり、ハンドリング動作時であってもワークの質量を正確に取得することのできる、ワーク質量測定機能を備えたロボットを提供することを目的とする。
【課題を解決するための手段】
【００１３】
前述した目的を達成するために１番目の発明によれば、把持されたワークの質量を測定するワーク質量測定機能を備えたロボットにおいて、前記ロボットの機構部の先端部に作用する力を測定する力測定部と、前記ロボットが移動しているときに前記力測定部により得られる力に基づいて、前記ロボットに把持されたワークの質量を推定する質量推定部と、を備えるロボットが提供される。
【００１４】
２番目の発明によれば、１番目の発明において、前記力測定部は、前記ワークを把持しているときに測定した力測定値と前記ワークを把持していないときに測定した力測定値とに基づいて前記ワークの質量を推定する。
【００１５】
３番目の発明によれば、１番目または２番目の発明において、前記ロボットが移動しているときに、前記機構部の先端部の姿勢を変化させないようにした。
【００１６】
４番目の発明によれば、１番目から３番目のいずれかの発明において、前記ロボットが移動しているときの複数の時刻において前記質量推定部が推定した前記ワークの複数の質量の平均値を質量推定値とする。
【００１７】
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は前記ロボットの機構部先端部に作用する加速度を測定または推定する加速度取得部を含んでおり、前記ロボットが移動しているときに前記力測定部により測定される力および前記加速度取得部により取得される加速度に基づいて、前記質量推定部は前記ロボットが把持したワークの質量を推定する。
【００１８】
６番目の発明によれば、５番目の発明において、前記加速度取得部は、動力学方程式により加速度を計算する。
【００１９】
７番目の発明によれば、５番目の発明において、前記加速度取得部は、前記ロボットに取付けられた加速度センサにより測定された加速度を取得する。
【００２０】
８番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定部は複数の方向において前記力を測定し、前記加速度取得部は前記複数の方向において前記加速度を取得し、前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記質量の平均値を質量推定値とする。
【００２１】
９番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定部は複数の方向において前記力を測定し、前記加速度取得部は前記複数の方向において前記加速度を取得し、前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記方向のうち、前記加速度の絶対値が最大である方向に対応する前記質量を質量推定値とする。
【００２２】
１０番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は、前記ロボットの機構部の先端部に作用する重力方向に対して垂直な平面上を前記機構部の先端部が移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした。
【００２３】
１１番目の発明によれば、１番目から４番目のいずれかの発明において、前記質量推定部は、前記ロボットの機構部の先端部が一定速度で直線軌道を移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした。
【００２４】
１２番目の発明によれば、１０番目または１１番目の発明において、さらに、前記ロボットの機構部の先端部の姿勢を取得する姿勢取得部を具備し、前記質量推定部は、前記力測定部により測定された一方向以上の力と、前記姿勢取得部により取得された前記姿勢とに基づいて、前記ロボットが把持したワークの質量を推定する。
【発明の効果】
【００２５】
１番目の発明においては、ロボットを停止させることなしにワークの質量を算出することができる。従って、質量の測定に伴ってロボット作業のサイクルタイムが延びることはない。また、単一の力測定部で足りるので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることもできる。
２番目の発明においては、ハンドの質量が未知であっても、ワークの質量を求めることができる。
３番目の発明においては、ワークの質量を求めるに際し、外乱による影響を軽減することができる。
４番目の発明においては、ワークの質量をより正確に算出することができる。
５番目の発明においては、加速度を用いることにより、ワークの質量をより正確に算出することができる。
６番目の発明においては、加速度センサが不要であるので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用をさらに抑えられる。
７番目の発明においては、加速度をより正確かつ直接的に求められる。
８番目の発明においては、複数の方向、例えばＸＹＺの三方向での平均値を用いているので、ワークの質量をより正確に算出することができる。
９番目の発明においては、加速度の絶対値が大きい方向について質量を求めるので、ノイズなどの誤差要因の影響を軽減することができる。
１０番目の発明においては、重力方向に慣性力が作用するのを排除できる。
１１番目の発明においては、ロボットの加減速に必要な力、遠心力、コリオリ力などの慣性力を排除できる。
１２番目の発明においては、ロボットの姿勢を考慮することにより、ハンドを真下に向けるなどの特殊な姿勢でロボットを動作させる必要がない。
【図面の簡単な説明】
【００２６】
【図１】本発明に基づくロボットの側面図である。
【図２】（ａ）本発明の実施例１等に基づくロボットの動作を示す第一の図である。（ｂ）本発明の実施例１等に基づくロボットの動作を示す第二の図である。
【図３】本発明の実施例２等に基づくロボットの動作を示す図である。
【図４】ロボットの側面図である。
【図５】（ａ）本発明の実施例５等に基づくロボットの動作を示す第一の図である。（ｂ）本発明の実施例５等に基づくロボットの動作を示す第二の図である。
【図６】本発明の実施例９に基づくロボットの側面図である。
【図７】（ａ）従来技術におけるロボットの側面図である。（ｂ）従来技術におけるロボットの他の側面図である。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボットの側面図である。図１においてロボット１は垂直多関節ロボットまたは公知のロボットマニュピレータであり、そのアームの先端部２にはワークＷを把持するハンド３が取付けられている。なお、ハンド３はロボット１のアームの先端部２に取付けられている必要は必ずしもない。
【００２８】
ハンド３は、ワークＷの形状、材質、質量等に応じて適宜選択される。ハンド３は、吸引式、チャック式のいずれであってもよい。さらに、アームの先端部２とハンド３との間には力センサ５が配置されている。
【００２９】
図１においては、ロボット１の動作範囲内に二つのテーブルＴ１、Ｔ２が配置されていて、一方のテーブルＴ１にワークＷが載置されている。また、図１に示されるように、ロボット１全体を制御するロボット制御装置９がロボット１に接続されている。なお、力センサ５もロボット制御装置９に接続されるものとする。
【００３０】
ロボット制御装置９はデジタルコンピュータであり、ワークＷの質量を推定する質量推定部１１と、ロボットアームの先端部２に作用する加速度を算出する加速度取得部１２とを備えている。なお、加速度取得部１２の代わりに、加速度センサ６（後述する）がハンド３に取付けられていてもよい。さらに、図１に示されるように、ロボット制御装置９はロボットアームの先端部２の姿勢を取得する姿勢取得部１３を含んでいる。
【００３１】
さらに、図１においては、ロボット１のベースに固定された基準座標系Σ１と、力センサ５に固定された力センサ座標系Σ２とが示されている。基準座標系Σ１の軸方向と重力方向との関係は予めロボット制御装置９の記憶部（図示しない）に記憶しておく。なお、この記憶部には各種のデータが記憶されるものとする。
【００３２】
通常は基準座標系Σ１のＺ方向と重力方向とが一致するように設定しているが、他の方向に設定してもよい。また、図１および後述する図面において、重力は各図面の下方に向かって作用するものとする。
【００３３】
ロボットアームの先端部２が動作するにつれて基準座標系Σ１に対する力センサ座標系Σ２の位置／姿勢は変化する。或る時刻における力センサ座標系Σ２の位置／姿勢は、その瞬間のロボット１の各軸の角度やロボットリンク長等を使って公知の順運動学計算により算出することができる。
【００３４】
図２（ａ）および図２（ｂ）は、それぞれ本発明の実施例１等に基づくロボットの動作を示す第一および第二の図である。図２（ａ）においては、ロボット１がワークＷを把持していない状態で、テーブルＴ２からテーブルＴ１に向かって先端部２を或る軌道上に動作させている。このとき、力センサ５には、ハンド３の重力およびハンド３が運動することにより生ずる慣性力などが作用する。
【００３５】
また、図２（ｂ）においては、ロボット１がワークＷを把持している状態で、テーブルＴ１からテーブルＴ２に向かって先端部２を或る軌道上に動作させている。このとき、力センサ５には、ハンド３とワークＷの重力およびハンド３とワークＷが運動することにより生ずる慣性力などが作用する。
【００３６】
実施例１
実施例１においては、ハンド３の質量は別途計測することにより既知である。また、力センサ５として三軸以上の力センサを使用しており、そのため力センサ５は並進三成分の力を検出できるものとする。
【００３７】
図２（ｂ）においては、ロボット１のハンド３によりワークＷを把持しつつ、ロボット１およびワークＷが周辺機器などに接触することなしに動作する。このとき、図２（ｂ）に示されるようにロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（１）で表される。なお、手首を変化させない理由は、ロボットの振動などの外乱の影響を少なくするためである。
【００３８】
Ｆ＝（Ｍｈ＋Ｍｗ）・Ｒ・｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝ （１）
ここで、Ｆは力センサ５により検出された力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル、
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル、
ｄ2ｘ／ｄｔ2はｘの２階微分、
Ｃ（ｘ、ｄｘ／ｄｔ）はハンド３とワークＷに作用する遠心力やコリオリ力等の慣性力を表すベクトルであり、ｘおよびｄｘ／ｄｔの関数である。
【００３９】
式（１）はロボット１が、周辺機器などに接触していない限りにおいて任意の時刻について成立する。ロボット１の動作時の或る時刻における力ベクトルＦを記録する。そして、質量推定部１１が同じ時刻におけるｘをロボットリンク長やロボット１の各軸の角度などの情報から順運動学計算により算出する。次いで、質量推定部１１は、算出されたｘを用いて、ｇ、ｄ2ｘ／ｄｔ2およびＣ（ｘ、ｄｘ／ｄｔ）も算出する。なお、後述する演算も主に質量推定部１１が行うものとする。
【００４０】
式（１）の各項は三成分のベクトルなので三つの式から成り、それぞれからワーク質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００４１】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下の方法により、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、以下の式（２）よりＭｗを容易に且つ正確に求めることができる。
Ｍｗ＝ （Ｍｗｘ＋Ｍｗｙ＋Ｍｗｚ）／３ （２）
【００４２】
あるいは、ベクトル｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝の三成分のうち絶対値が最大の方向の質量を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００４３】
以上のようにして、或る一つの時刻における式（１）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、ロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のバラツキ、ロボット自体の揺れによる振動等の外乱による影響を軽減し、信頼性の高い質量Ｍｗを求めることができる。
【００４４】
実施例２
図３は本発明の実施例２等に基づくロボットの動作を示す図である。図から分かるように、実施例２は、加速度センサ６がハンド３の基部に取付けられている点で実施例１とは異なる。また、実施例２においても、力センサ５として三軸以上の力センサを使用しており、そのため力センサ５は並進三成分の力を検出できるものとする。前述した実施例１ではベクトル｛ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）｝を計算により求めていたが、実施例２では加速度センサ６の出力を利用して上記ベクトルを求める。
【００４５】
図２（ｂ）に示されるようにロボット１がワークＷを把持している状態で、テーブルＴ１からテーブルＴ２に向かって先端部２を或る軌道上に動作させる。このときの力センサ５の出力は以下の式（３）で表すことができる。
【００４６】
Ｆ＝（Ｍｈ＋Ｍｗ）・Ａ （３）
ここで、Ｆは力センサ５により検出された力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換したもの、である。
【００４７】
実施例１の場合と同様に、ロボット１の動作時において或る時刻における力ベクトルＦを記録する。このとき、或る時刻における加速度センサ６の出力ベクトルＡも同様に記録する。そして、ハンド３の質量Ｍｈは既知であるので、式（３）を用いることによりワークＷの質量Ｍｗを算出することができる。
【００４８】
式（３）の各項は三成分のベクトルなので三つの式から成り、それぞれからワーク質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００４９】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、前述した式（２）よりワークＷの質量Ｍｗを容易に求めることができる。
【００５０】
あるいは、ベクトルＡの三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、Ａ＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００５１】
以上のようにして、或る一つの時刻における式（３）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。また、実施例２においては、加速度センサ６を採用しているので、加速度をより正確かつ直接的に求められるのが分かるであろう。
【００５２】
実施例３
実施例３において使用される力センサ５は三軸以上の力センサであり、並進三成分の力を検出できるものとする。実施例３は後述する二つのステップにより測定するという点で、前述した実施例１とは異なる。すなわち、実施例３においては、ハンド３の質量も計算するので、ハンド３の質量を事前に測定する必要がない。
【００５３】
はじめに第一のステップを説明する。図２（ａ）に示されるように、ロボット１のハンド３がワークＷを把持していない状態で、周辺機器に接触することなしに、ロボット１のハンド３を空中に移動させる。ロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（４）で表される。
【００５４】
Ｆ１＝Ｍｈ・Ｒ・（ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）） （４）
ここで、Ｆ１は力センサ５で検知した力ベクトル（検出値）、
Ｍｈはハンドの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル
ｄ2ｘ／ｄｔ2はｘの２階微分
Ｃ（ｘ、ｄｘ／ｄｔ）はツールにかかる遠心力やコリオリ力による加速度ベクトルであり、ｘ、ｄｘ／ｄｔの関数である。
【００５５】
式（４）はロボット１が周辺機器などに接触しなければ任意の時刻において成立し、ロボット１が動作中のある時刻における力ベクトルＦ１を記録する。同じ時刻における位置ベクトルｘをロボットリンク長やロボットの各軸角度などの情報から順運動学計算により演算する。そして、位置ベクトルｘを用いて、さらにｇ、ｄ2ｘ／ｄｔ2、Ｃ（ｘ、ｄｘ／ｄｔ）も導出する。この場合には、加速度センサ６を設ける必要がないので、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることが可能となる。
【００５６】
次いで、上記の情報から式（４）よりハンド３の質量Ｍｈを計算する。或る１つの時刻において式（４）からハンド３の質量Ｍｈを求めることに加えて、複数の時刻において式（４）からハンド３の質量Ｍｈを求めその平均値を演算してもよい。これにより、ロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減し、信頼性の高い質量Ｍｈを求めることができる。
【００５７】
次いで、第二のステップを説明する。図２（ｂ）に示されるように、ロボット１のハンド３がワークＷを把持した状態で、周辺機器に接触することなしに、ロボット１のハンド３を空中に移動させる。ロボット１の手首の姿勢が変化しないようにロボット１を動作させたときの力センサ５の出力は以下の式（５）で表される。
【００５８】
Ｆ２＝（Ｍｈ＋Ｍｗ）・Ｒ・（ｇ＋ｄ2ｘ／ｄｔ2＋Ｃ（ｘ、ｄｘ／ｄｔ）） （５）
ここで、Ｆ２は力センサで検知した力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ｒは基準座標系Σ１から力センサ座標系Σ２への変換行列、
ｇは基準座標系Σ１から見た重力加速度ベクトル、
ｘは基準座標系Σ１から見た力センサ座標系Σ２原点の位置ベクトル、
ｄ2ｘ／ｄｔ2はｘの２階微分、
Ｃ（ｘ、ｄｘ／ｄｔ）はツールとワークにかかる遠心力やコリオリ力をあらわすベクトルでｘ、ｄｘ／ｄｔの関数である。
【００５９】
前述した第一のステップと同様に或る時刻におけるｘ、ｇ、ｄ2ｘ／ｄｔ2、Ｃ（ｘ、ｄｘ／ｄｔ）を計算する。ハンド３の質量Ｍｈは第一のステップにおいて既に定まっている。従って、式（５）を使ってワークＷの質量Ｍｗを導出する。
【００６０】
式（５）の各項は三成分のベクトルなので三つの式から成り、それぞれから質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００６１】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚ平均値を算出することであり、前述した式（２）よりＭｗを容易に求めることができる。
【００６２】
あるいは、ベクトル（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ）の三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ）＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００６３】
以上のようにして、或る一つの時刻における式（５）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。
【００６４】
実施例４
図３を参照して前述したように三軸の加速度センサ６をハンド３に取付ける。前述した実施例３においてはロボットリンク長やロボットの各軸角度などから加速度（ｇ−ｄ2ｘ／ｄｔ2−Ｃ（ｘ、ｄｘ／ｄｔ））を求めたが、実施例４においては加速度センサ６の出力を使用する。
【００６５】
実施例４においても実施例３と同様に三軸以上の力センサ５を使用し、並進三成分の力が検出できるものとする。実施例４は後述する二つのステップにより測定するという点で、前述した実施例２とは異なる。すなわち、実施例４においては、ハンド３の質量を計算する。
【００６６】
はじめに第一のステップを説明する。図２（ａ）に示されるように、ロボット１のハンド３がワークＷを把持していない状態で、周辺機器などに接触することなしにハンド３が空中を移動する。その際の力センサ５の出力は以下の式（６）で表される。
【００６７】
Ｆ１＝Ｍｈ・Ａ （６）
ここで、Ｆ１は力センサ５で検知した力ベクトル（検出値）
Ｍｈはハンド３の質量
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換した値である。
【００６８】
式（６）はロボット１が周辺機器などに接触していなければ任意の時刻について成立し、或る時刻における力センサ５の力ベクトルＦ１と加速度センサ６に関する値Ａとを記録する。そして、これらの情報を用いて式（６）からハンド３の質量Ｍｈを算出する。
【００６９】
或る一つの時刻において式（６）からハンド３の質量Ｍｈを求めることに加えて、複数の時刻においてハンド３の質量Ｍｈを求め、その平均値を算出してもよい。そうすることでロボットリンク長の個体差やリンクの撓みなどによるモデル化誤差、力センサの出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００７０】
次いで、第二のステップを説明する。図２（ｂ）に示されるように、ロボット１でワークＷを把持した状態で周辺機器などに接触させることなしハンド３を空中に動作させる。その際の力センサ５の出力は以下の式（７）で表すことができる。
【００７１】
Ｆ２＝（Ｍｈ＋Ｍｗ）・Ａ （７）
ここで、Ｆ２は力センサ５で検知した力ベクトル（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
Ａは加速度センサ６の出力ベクトルを力センサ座標系Σ２での値に変換した値である。
【００７２】
第一のステップの場合と同様に、或る時刻における力センサ５の力ベクトルＦ２と加速度センサ６に関する値Ａとを記録する。ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（７）を使用すれば、ワークＷの質量Ｍｗを求めることができる。
【００７３】
式（７）の各項は三成分のベクトルなので三つの式から成り、それぞれから質量Ｍｗを求めることができる。理想的には全て同じ値になるが、実際の計算では多少異なる結果になることが多い。
【００７４】
三つの式から得られる質量をそれぞれＭｗｘ、Ｍｗｙ、Ｍｗｚとすると、以下のようにして、Ｍｗを算出することが可能である。第一の方法はＭｗｘ、Ｍｗｙ、Ｍｗｚの平均値を算出することであり、前述した式（２）よりＭｗを容易に求めることができる。
【００７５】
あるいは、ベクトルＡの三成分のうち絶対値が最大の方向の結果を採用してもよい。つまり、加速度の絶対値が大きい方向は、ノイズなどの誤差要因の影響は相対的に小さくなるのでその結果だけを利用する。すなわち、Ａ＝（ａｘ、ａｙ、ａｚ）と仮定すると、｜ａｘ｜が最大ならＭｗｘ、｜ａｙ｜が最大ならＭｗｙ、｜ａｚ｜が最大ならＭｗｚをＭｗとして採用する。
【００７６】
以上のようにして、或る一つの時刻における式（７）だけでワークＷの質量Ｍｗを算出できる。ただし、複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。これにより、様々な誤差の影響を軽減できるのが分かるであろう。
【００７７】
後述する実施例５〜実施例８においては、力測定部として、一軸以上のロードセルまたは力センサのいずれを採用してもよい。一軸ロードセルを使用する場合、ロードセルが力を測定できる１方向のことを以下では測定方向と呼ぶ。ロボット手首が鉛直下向き姿勢のときに測定方向と重力方向が一致するようにロードセルをロボット手首に取付ける（ロボットの側面図である図４を参照されたい）。
【００７８】
ロボットのアームの先端部２が動作するにつれて基準座標系Σ１に対する力センサ座標系Σ２またはロードセル５の位置／姿勢は変化する。姿勢取得部１３は、ある時刻における力センサ座標系Σ２またはロードセル５の位置／姿勢を、その瞬間のロボットの各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。
【００７９】
図５（ａ）は本発明の実施例５等に基づくロボットの動作を示す第一の図である。図５（ａ）においては、ロボット１がワークＷを把持していない状態で、重力方向に対して垂直な平面上をロボットのアームの先端部２が動作している様子を示す。力センサ５にはハンド３の重力およびハンド３が運動することにより生ずる慣性力などが作用する。しかしながら、重力方向については、慣性力の成分は作用しないので、重力のみが作用する。
【００８０】
図５（ｂ）は本発明の実施例５等に基づくロボットの動作を示す第二の図である。図５（ｂ）においては、ロボット１がワークＷを把持している状態で、重力方向に対して垂直な平面上をロボットのアームの先端部２が動作している様子を示す。力センサ５にはハンド３とワークＷの重力およびハンド３とワークＷが運動することにより生ずる慣性力などが作用する。しかしながら、重力方向については、慣性力の成分は作用しないので、重力のみが作用する。
【００８１】
実施例５
実施例５ではハンド３の質量はあらかじめ測定して既知とし、力測定部５として１軸以上のロードセル５を使用する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つ、ロボットの手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（８）が成立する。
【００８２】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ・ｃｏｓθ （８）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度、である（例えば図４における角度θは０°である）。
【００８３】
式（８）においては、重力方向に対して垂直な平面上を動作している限り任意の時刻において成立する。そして、そのときの或る時刻における力Ｆを記録する。
【００８４】
姿勢取得部１３は、基準座標系Σ１に対するロードセル５の位置／姿勢を、その瞬間のロボット１の各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。基準座標系Σ１に対する重力方向もわかっているのでその瞬間におけるθも計算できる。そして、ハンド３の質量Ｍｈは既知であるので、式（８）からワークの質量Ｍｗを算出できる。この場合には、重力方向に慣性力が作用するのを排除できるので、ワークＷの質量はより正確になる。
【００８５】
或る一つの時刻において式（８）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００８６】
実施例６
実施例６においてはハンド３の質量はあらかじめ測定して既知とし、力測定部５として三軸以上の力センサを使用する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（９）が成立する。
【００８７】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ （９）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度である。
【００８８】
前述した式（８）は計測方向に関する式であるが、式（９）は重力方向についての式である。式（９）は重力方向に対して垂直な平面上を動作している限り任意の時刻において成立し、或る時刻における力センサ５の出力を記録する。
【００８９】
力センサ５の出力は力センサ座標系Σ２における値でＸ、Ｙ、Ｚの三成分ある。上述のように、基準座標系Σ１に対する力センサ座標系Σ２、重力方向ともにわかっているのでその瞬間における、力センサ５の出力の重力方向成分Ｆも計算することができる。そして、ハンド３の質量Ｍｈは既知としているので、式（９）からＭｗを計算する。
【００９０】
或る一つの時刻において式（９）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００９１】
実施例７
実施例７においては、力測定部として一軸以上のロードセル５を使用する。実施例７は後述する二つのステップにより測定するという点で、前述した実施例５とは異なる。すなわち、実施例７においては、ハンド３の質量を計算する。
【００９２】
はじめに第一のステップを説明する。図４（ａ）に示されるように、ロボット１がワークＷを把持していない状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上に動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。このとき、力について以下の式（１０）が成立する。
【００９３】
Ｆ＝Ｍｈ・ｇ・ｃｏｓθ （１０）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度である。
【００９４】
式（１０）は重力方向に対して垂直な平面上を動作している限り任意の時刻において成立する。そして、そのときの或る時刻における力Ｆを記録する。
【００９５】
姿勢取得部１３は、基準座標系Σ１に対するロードセル５の位置／姿勢を、その瞬間のロボット１の各軸角度やロボットリンク長などを使って公知の順運動学計算により求めることができる。基準座標系Σ１に対する重力方向もわかっているのでその瞬間における角度θも計算できる。これらの情報を使って式（１０）からハンド３の質量Ｍｈを計算する。
【００９６】
或る一つの時刻において式（１０）よりハンド３の質量Ｍｈを求めることができる。複数の時刻におけるハンド３の質量Ｍｈを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【００９７】
次いで、第二のステップを説明する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（１１）が成立する。
【００９８】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ・ｃｏｓθ （１１）
ここで、Ｆはロードセル５で検知した力（検出値）、
Ｍｈはハンド３の質量、
Ｍｗはワーク５の質量、
ｇは重力加速度、
θは測定方向と重力方向のなす角度である。
【００９９】
前述した第一のステップの場合と同様に、或る時刻における力Ｆを記録し、角度θを計算する。そして、ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（１１）からワークＷの質量Ｍｗを計算する。
【０１００】
或る一つの時刻において式（１１）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１０１】
実施例８
実施例８においては、力測定部５として三軸以上の力センサ５を使用する。実施例８は後述する二つのステップにより測定するという点で、前述した実施例６とは異なる。すなわち、実施例８においては、ハンド３の質量を計算する。
【０１０２】
はじめに、第一のステップについて説明する。図４（ａ）に示されるように、ロボット１がワークＷを把持していない状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上に動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。このとき、力について以下の式（１２）が成立する。
【０１０３】
Ｆ＝Ｍｈ・ｇ （１２）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ｇは重力加速度である。
【０１０４】
前述した式（８）は計測方向についての式であるが、式（１２）は重力方向の式である。式（１２）は、重力方向に対して垂直な平面上を動作している限り任意の時刻において成立し、そのときの或る時刻における力センサ５の出力を記録する。
【０１０５】
力センサ５の出力は力センサ座標系Σ２における値でＸ、Ｙ、Ｚの三成分ある。上述のように、基準座標系Σ１に対する力センサ座標系Σ２、重力方向ともにわかっているのでその瞬間における、力センサ５の力の重力方向成分Ｆも計算することができる。これらの情報を使って式（１２）からハンド３の質量Ｍｈを計算する。
【０１０６】
或る一つの時刻において式（１２）よりハンド３の質量Ｍｈを求めることができる。複数の時刻におけるハンド３の質量Ｍｈを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１０７】
次いで、第二のステップについて説明する。図４（ｂ）に示されるように、ロボット１がワークＷを把持した状態で周辺機器などに接触することなしに、且つロボット１の手首の姿勢を変化させることなしに、ワークＷを重力方向に対して垂直な平面上を動作させる。動作中の姿勢は一定とするが、必ずしも重力方向と一致していなくてもよい。その際の力について以下の式（１３）が成立する。
【０１０８】
Ｆ＝（Ｍｈ＋Ｍｗ）・ｇ （１３）
ここで、Ｆは力センサ５で検知した力の重力方向の成分（検出値）、
Ｍｈはハンド３の質量、
ＭｗはワークＷの質量、
ｇは重力加速度である。
【０１０９】
前述した第一のステップの場合と同様に、或る時刻における力の成分Ｆを計算する。ハンド３の質量Ｍｈは第一のステップにおいて定まっているので、式（１３）を用いてワークＷの質量Ｍｗを計算することができる。
【０１１０】
或る一つの時刻において式（１３）よりワークＷの質量Ｍｗを求めることができる。複数の時刻におけるワークＷの質量Ｍｗを求めその平均値を採用してもよい。そうすることでロードセル出力値のばらつき、ロボット自身の揺れによる振動等の外乱による影響を軽減することができる。
【０１１１】
実施例９
図６は本発明の実施例９に基づくロボットの側面図である。実施例５〜実施例８においては、アームの先端部２に作用する重力方向に対して垂直な平面上をロボットアーム先端部２が移動していた。これに対し、実施例９では、図６に示されるように、ロボットアームの先端部を一定速度で直線軌道上を移動させる。
【０１１２】
図６から分かるように、この直線軌道は必ずしも重力方向に対して垂直な平面上になくてもよい。一定速度かつ直線軌道に限定することで加減速に必要な力、遠心力、コリオリ力などの慣性力が力測定部５に作用しなくなる。従って、より正確にワークＷの質量Ｍｈを求められるのが分かるであろう。なお、ワークＷの質量Ｍｗの計算方法は実施例５〜実施例８と同様であるので説明を省略する。
【０１１３】
このように、本発明においては、ロボット１を停止させることなしにワークＷの質量を算出することができる。このため、ロボット１の減速時間、静止時間、再加速時間が不要となる。従って、質量の測定に伴ってロボット作業のサイクルタイムが延びることはない。
【０１１４】
さらに、ロボット１が任意の起動で動作しても、動力学項の影響を受けること無しに、ワークＷの質量を正確に取得することができる。また、本発明では複数の力測定部は必要でなく、単一の力測定部で足りる。このため、ロボットシステムの構成を単純にでき、ロボットシステムの構築に要する費用を抑えることが可能となる。なお、異なる実施例であっても、同様の構成であれば同様の効果が得られるのは当業者であれば明らかであろう。
【符号の説明】
【０１１５】
１ ロボット
２ アームの先端部（機構部の先端部）
３ ハンド
５ 力測定部、力センサ、ロードセル
６ 加速度センサ
９ ロボット制御装置
１１ 質量推定部
１２ 加速度取得部
１３ 姿勢取得部
Ｗ ワーク
【特許請求の範囲】
【請求項１】
把持されたワークの質量を測定するワーク質量測定機能を備えたロボットにおいて、
前記ロボットの機構部の先端部に作用する力を測定する力測定部と、
前記ロボットが移動しているときに前記力測定部により得られる力に基づいて、前記ロボットに把持されたワークの質量を推定する質量推定部と、を備えるロボット。
【請求項２】
前記力測定部は、前記ワークを把持しているときに測定した力測定値と前記ワークを把持していないときに測定した力測定値とに基づいて前記ワークの質量を推定することを特徴とする請求項１に記載のロボット。
【請求項３】
前記ロボットが移動しているときに、前記機構部の先端部の姿勢を変化させないようにしたことを特徴とする請求項１または２に記載のロボット。
【請求項４】
前記ロボットが移動しているときの複数の時刻において前記質量推定部が推定した前記ワークの複数の質量の平均値を質量推定値とすることを特徴とする請求項１乃至３のいずれか一項に記載のロボット。
【請求項５】
前記質量推定部は前記ロボットの機構部先端部に作用する加速度を測定または推定する加速度取得部を含んでおり、
前記ロボットが移動しているときに前記力測定部により測定される力および前記加速度取得部により取得される加速度に基づいて、前記質量推定部は前記ロボットが把持したワークの質量を推定することを特徴とする請求項１乃至４のいずれか一項に記載のロボット。
【請求項６】
前記加速度取得部は、動力学方程式により加速度を計算することを特徴とする請求項５に記載のロボット。
【請求項７】
前記加速度取得部は、前記ロボットに取付けられた加速度センサにより測定された加速度を取得することを特徴とする請求項５に記載のロボット。
【請求項８】
前記力測定部は複数の方向において前記力を測定し、
前記加速度取得部は前記複数の方向において前記加速度を取得し、
前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記質量の平均値を質量推定値とすることを特徴とする請求項１乃至７のいずれか一項に記載のロボット。
【請求項９】
前記力測定部は複数の方向において前記力を測定し、
前記加速度取得部は前記複数の方向において前記加速度を取得し、
前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記方向のうち、前記加速度の絶対値が最大である方向に対応する前記質量を質量推定値とすることを特徴とする請求項１乃至７のいずれか一項に記載のロボット。
【請求項１０】
前記質量推定部は、前記ロボットの機構部の先端部に作用する重力方向に対して垂直な平面上を前記機構部の先端部が移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした請求項１乃至４のいずれか一項に記載のロボット。
【請求項１１】
前記質量推定部は、前記ロボットの機構部の先端部が一定速度で直線軌道を移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした請求項１乃至４のいずれか一項に記載のロボット。
【請求項１２】
さらに、前記ロボットの機構部の先端部の姿勢を取得する姿勢取得部を具備し、
前記質量推定部は、前記力測定部により測定された一方向以上の力と、前記姿勢取得部により取得された前記姿勢とに基づいて、前記ロボットが把持したワークの質量を推定することを特徴とする請求項１０または１１に記載のロボット。
【請求項１】
把持されたワークの質量を測定するワーク質量測定機能を備えたロボットにおいて、
前記ロボットの機構部の先端部に作用する力を測定する力測定部と、
前記ロボットが移動しているときに前記力測定部により得られる力に基づいて、前記ロボットに把持されたワークの質量を推定する質量推定部と、を備えるロボット。
【請求項２】
前記力測定部は、前記ワークを把持しているときに測定した力測定値と前記ワークを把持していないときに測定した力測定値とに基づいて前記ワークの質量を推定することを特徴とする請求項１に記載のロボット。
【請求項３】
前記ロボットが移動しているときに、前記機構部の先端部の姿勢を変化させないようにしたことを特徴とする請求項１または２に記載のロボット。
【請求項４】
前記ロボットが移動しているときの複数の時刻において前記質量推定部が推定した前記ワークの複数の質量の平均値を質量推定値とすることを特徴とする請求項１乃至３のいずれか一項に記載のロボット。
【請求項５】
前記質量推定部は前記ロボットの機構部先端部に作用する加速度を測定または推定する加速度取得部を含んでおり、
前記ロボットが移動しているときに前記力測定部により測定される力および前記加速度取得部により取得される加速度に基づいて、前記質量推定部は前記ロボットが把持したワークの質量を推定することを特徴とする請求項１乃至４のいずれか一項に記載のロボット。
【請求項６】
前記加速度取得部は、動力学方程式により加速度を計算することを特徴とする請求項５に記載のロボット。
【請求項７】
前記加速度取得部は、前記ロボットに取付けられた加速度センサにより測定された加速度を取得することを特徴とする請求項５に記載のロボット。
【請求項８】
前記力測定部は複数の方向において前記力を測定し、
前記加速度取得部は前記複数の方向において前記加速度を取得し、
前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記質量の平均値を質量推定値とすることを特徴とする請求項１乃至７のいずれか一項に記載のロボット。
【請求項９】
前記力測定部は複数の方向において前記力を測定し、
前記加速度取得部は前記複数の方向において前記加速度を取得し、
前記質量推定部は前記複数の方向について前記質量を推定し、それにより、複数の前記方向のうち、前記加速度の絶対値が最大である方向に対応する前記質量を質量推定値とすることを特徴とする請求項１乃至７のいずれか一項に記載のロボット。
【請求項１０】
前記質量推定部は、前記ロボットの機構部の先端部に作用する重力方向に対して垂直な平面上を前記機構部の先端部が移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした請求項１乃至４のいずれか一項に記載のロボット。
【請求項１１】
前記質量推定部は、前記ロボットの機構部の先端部が一定速度で直線軌道を移動しているときに前記力測定部により得られる力に基づいて、前記ロボットが把持したワークの質量を推定するようにした請求項１乃至４のいずれか一項に記載のロボット。
【請求項１２】
さらに、前記ロボットの機構部の先端部の姿勢を取得する姿勢取得部を具備し、
前記質量推定部は、前記力測定部により測定された一方向以上の力と、前記姿勢取得部により取得された前記姿勢とに基づいて、前記ロボットが把持したワークの質量を推定することを特徴とする請求項１０または１１に記載のロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１３−５６４０２（Ｐ２０１３−５６４０２Ａ）
【公開日】平成２５年３月２８日（２０１３．３．２８）
【国際特許分類】
【出願番号】特願２０１１−１９７０７２（Ｐ２０１１−１９７０７２）
【出願日】平成２３年９月９日（２０１１．９．９）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
【公開日】平成２５年３月２８日（２０１３．３．２８）
【国際特許分類】
【出願日】平成２３年９月９日（２０１１．９．９）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
[ Back to top ]