JP2011235374A - ロボットの負荷推定装置及び負荷推定方法 - Google Patents

Abstract

【課題】オンラインで特定動作を必要とせず、ロボットが把持するワークの重量及び重心位置を推定する装置を提供する。
【解決手段】ワーク重量を想定せずにモータに向けて送られたトルク指令と把持されたワークの重量に応答して現実に生じているトルクとの差異に着目してワークの重量を推定する。ロボットに作用する重力トルク及び摩擦トルクを演算する手段（２７）と、現実に生じているトルクから重力トルク及び摩擦トルクを減算することにより正味の外乱トルクを演算する手段（２４）と、正味の外乱トルクをロボットの手先力及び手先モーメントに変換する手段（２５）と、負荷が追加される前にロボットの手先に生じている第１の手先力推定値と、負荷が追加され微小量だけ持ち上げた後にロボットの手先に生じている第２の手先力推定値との差分を計算することにより負荷の重量を推定し、同様にして第１及び第２の手先モーメントの差分を計算することにより負荷の重心位置を推定する手段（２６）とを備えている。
【選択図】図２−１

Description

本発明は、サーボモータを使用して多軸のロボットを駆動する装置の制御に関し、特に、ハンドリング用途などでロボット先端のハンドが把持したワークの重量及び重心位置を推定するロボットの負荷推定装置及び方法に関する。

産業用ロボットで組立作業やハンドリング作業を行う場合、動作時間短縮や振動低減などの制御性能向上のために、把持したワークの重量及び重心位置を正確に知る必要がある。例えば、動作時間短縮のために加減速時間を最短にしようとする場合、加減速時間を求めるために必要な最大加速度αはモータの最大出力トルクτmaxと負荷イナーシャＩの関係（α＝τmax／Ｉ）から求めることができ、この負荷イナーシャＩを求めるためにワーク重量及び重心位置の情報が必要となる。

また、サービスロボット等、人間と近接して用いられるロボットでは、人が近くにいる場合に、安全性を考慮してロボットのアームや移動体の発生トルクを制限する必要がある。この場合でも発生トルクを制限するため、アームの自重を補償する重力トルクを演算する必要があり、ここでもワーク重量及び重心位置の情報が重要となる。

なお、ここでワークとは、ロボット工学における一般的な用法に従い、ロボット自体とは別の物体を意味する。産業用ロボットの場合であればロボットによって把持され移動される対象である製品などがワークであり、人型のサービスロボットの場合には例えばコップや道具がワークである。また、ワーク以外にもユーザによってロボットに付加的に取り付けられるツールなど物体の重量も、ワークの場合と同様に推定の対象となりうる。ワークとツールとは共にロボット自体とは元々別個の物体であって、ロボットに把持又は付加されることの結果としてロボットの適切な動作制御のためにその重量を考慮する必要があるという点で同様である。従って、この出願では、両者を特に区別することなく、ワークの場合を念頭において説明する。

ワーク重量の推定方法としては、例えば、特許文献１には、手首軸に適応制御を使ってモーメントを推定する例がある。特許文献１の段落［００１４］には「・・・付加的に取り付けられた物体の負荷重量を表わすパラメータ（質量、重心位置等）は、上記適応制御の過程を通して得られたデータを使って近似的な関係式で表現されるので、これを付加的に取り付けられた物体の負荷重量を表わすパラメータを未知数とする方程式として数値計算によって解けば、それらパラメータの値を求めることが出来る。・・・」という記載がある。

また、特許文献２には、基準姿勢で負荷の上げ下げを行い、平均電流値から推定する例がある。段落［０００９］に「・・・アーム先端に負荷を取り付けた状態でロボットを動作させたときの各駆動軸モータに流れる駆動電流を測定し、この駆動電流よりモータ駆動トルクを求める。一方、ロボットを質点モデルとして考えたとき、この質点モデルに負荷の重量及び重心位置のパラメータを含ませるようにすれば、負荷トルクはこのロボットの質点モデルから力学的に求めることができる。ここで、モータ駆動トルクと負荷トルクが等価であると仮定すれば、ロボットの質点モデルに未知数として含まれている負荷の重量及び重心位置を計算により算出することができる。」との記載がある。

また、特許文献３には、加速時に慣性モーメントを推定（負荷重量も）し、移動速度や減速を決定する例がある。第２頁右下欄第８行以下に「・・・移動開始点でのワークを把持していない時のモータ軸換算の慣性モーメントをＪ_Ａ、移動開始点でのワーク把持部−モータ軸間距離をｒ_Ａ、負荷重量をＭ_Ｌとする。・・・モータ駆動手段１３によりモータ３の発生最大トルクでモータ速度を直線的に立ち上げる。このとき加速度検出手段６より得られる加速度、トルクをそれぞれα_Ａ、Ｔ_ｍとすると以下の関係式が成り立つ。α_Ａ＝Ｔ_ｍ／（Ｊ_Ａ＋Ｍ_ＬＸｒ_Ａ ^２）」という記載がある。この関係式ではＭ_Ｌ以外は既知であるからＭ_Ｌを導くことができるとされている。

特開平８−１９０４３３号公報（第４頁、図４） 特開平１０−１３８１８７号公報（第８頁、図１） 特開昭６２−０３１４０６号公報（第２頁、図１）

しかしながら、特許文献１については、適応制御で推定するため時間を要するという問題があった。また、動作プログラムは適応制御による動作が十分に収束するように、ロボットの手先がある程度の速度と加速度をもって適度の範囲で位置・姿勢を変える必要があるという問題もあった。例えば、段落［００２０］に「この動作プログラムは、適応制御による動作が十分に収束するように、ロボットの手先がある程度の速度と加速度をもって適度の範囲で位置・姿勢を変えるようなものであることが好ましい。」との記載がある。

また、特許文献２については、オフラインで特定の姿勢を使って、推定する必要があるという問題があった。例えば、段落［００１２］に「なお、この計算処理においては負荷の重量及び重心位置を未知数とする連立方程式を解くことになるが、これらの未知数を精度良く算出するために、好ましくは実行中に大きなアンバランストルクが駆動軸にかかるように動作パターンプログラムが作成されるようにする。具体的には、負荷の動作において重力の影響を大きく受けるような、すなわち負荷が上下に揺動するような動作を行わせることにより、駆動軸に大きなアンバランストルクがかかるようにする必要があり、このためにはロボットの基準姿勢を良好な位置に選定する必要がある。」との記載がある。

また、特許文献３については、最大トルクで立ち上げる必要があり、動作範囲等が取れないという問題があった。例えば、第２頁右下欄第１２行以下に「まずモータ駆動手段１３によりモータ３の発生最大トルクでモータ速度を直線的に立ち上げる。」との記載があり、モータを最大トルクで始動させることを前提にしている。

つまり、オフラインの特定動作による方法では、最大トルクでモータを回転させる必要や計測に時間がかかる等の問題があり、オンライン推定では、予め無負荷の状態量を計測しておく必要や逆動力学を解く必要があるなど運用面で問題がある。

本発明はこのような問題点に鑑みてなされたものであり、オンラインで特定動作を必要とせず、ワーク重量及び重心位置を推定する装置を提供することを目的とする。

課題を解決するための手段及び発明の効果

制御工学において、制御の対象となる量は制御量と称され、希望する制御量を得るために操作する量は操作量と称される。しかし、制御量に影響を与える因子は操作量だけではなく、操作量以外に制御量の決定に影響する因子は「外乱」と総称される。重量が未知であるワークをロボットが把持する場合を想定すると、トルク指令（＝操作量）を操作して希望する加減速度（＝制御量）を達成しようとする場合に、未知であるワーク重量は外乱のひとつである。本発明では、ワーク重量を想定せずにモータに向けて送られたトルク指令と把持されたワークの重量に応答して現実に生じているトルクとの差異に着目し、この差異を用いてワークの重量及び重心位置を推定する。

更に、本発明は、別の実施例では、いったんワーク重量及び重心位置が推定された後で、推定されたワーク重量及び重心位置とロボットアームの重量及び重心位置とを加算した上で上述した方法によるワーク重量及び重心位置の推定を反復的に実行し、ワーク重量及び重心位置の推定値の精度を向上させることができる。

本発明では、ワークの重量を推定することに加え、具体的な座標成分を用いて後述するように、ワークの重心位置を推定することができる。重心位置を推定することにより、重量だけを推定する場合と比較して、負荷イナーシャを正確に計算することが可能になり、その結果、モ−タに生じさせるべきトルクをより正確に決定することが可能になる。更には、正確な重心位置が推定されるのであるから、重量だけしか推定されない場合とは異なって、ロボットがワークを把持する位置に関して限定がなく、どの位置で把持した場合でも正確に負荷イナーシャを計算することが可能になる。
上述したワーク重量及び重心位置推定方法（より一般的には負荷推定方法）に基づき、本発明による負荷重量及び重心位置推定装置は、ロボットの各軸に作用する重力トルク及び摩擦トルクを演算する重力トルク及び摩擦トルク演算部と、前記ロボットの各軸において現実に生じているトルクから前記重力トルク及び前記摩擦トルクを減算することにより前記ロボットの各軸に作用する正味の外乱トルクを演算する外乱トルク演算部と、前記正味の外乱トルクをロボットの手先力及び手先モーメントに変換する手先力演算部と、を備えている。この負荷重量及び重心位置推定装置は、更に、前記負荷が追加される前に前記ロボットの手先に生じている第１の手先力推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先力推定値との差分を計算することにより負荷の重量を推定し、前記負荷が追加される前に前記ロボットの手先に生じている第１の手先モーメント推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先モーメント推定値との差分を計算することにより負荷の重心位置を推定する負荷重量及び重心位置推定部を備えている。

本発明による負荷重量及び重心位置推定装置では、上述した構成を備えることにより、センサを用いずにロボットに追加されたワークやツールなどの負荷重量及び重心位置をオンラインで推定するという効果が達成される。例えば、産業用ロボットのエンドエフェクタに様々なツールを追加する場合であっても、追加されるツールの重量及び重心位置をユーザが個別に測定して測定値を入力することは不要である。また、人間と近接して用いられるサービスロボットの場合にも、背景技術として先に述べた従来技術における負荷重量推定の場合とは異なり、ロボットアームに大きなトルクを生じさせたり大きな空間的な範囲にわたる動作を要求したりすることなく推定が可能であることにより、負荷重量推定時の人間との衝突などが回避され、サービスロボットの安全な動作制御が可能となる。

別の実施例では、本発明による負荷重量及び重心位置推定装置において、いったん推定された負荷重量及び重心位置の精度を向上させるため、前記重力トルク及び摩擦トルク演算部は前記推定された負荷重量及び重心位置を前記ロボットの重量及び重心位置に加算した上で重力トルクを再計算し、前記外乱トルク演算部は再計算された重力トルクを減算することによって正味の外乱トルクを再計算し、前記手先力演算部は再計算された正味の外乱トルクを手先力に変換して第３の手先力推定値及び手先モーメント推定値を演算する。更に、前記負荷重量及び重心位置推定部は前記第２の手先力推定値と前記第３の手先力推定値との差分から負荷重量補正値を推定し、前記第２の手先モーメント推定値と前記第３の手先モーメント推定値との差分から重心位置補正値を推定し、前記負荷重量補正値及び重心位置補正値がそれぞれについて予め設定された閾値以下となる場合には負荷重量推定を終了し、前記負荷重量補正値及び重心位置補正値のいずれかが前記閾値よりも大きい場合には当該負荷重量補正値及び重心位置補正値を反映させた新たな負荷重量及び重心位置を用いて負荷重量補正値及び重心位置補正値の推定を繰り返す。上述した反復的な動作により、先の実施例において得られた重量推定値及び重心位置推定値の精度を希望する目的に応じて向上させることができる。

更に別の実施例では、本発明による負荷重量及び重心位置推定装置において、予定された負荷重量及び重心位置の精度が達成されたときに推定動作を停止し、また前記負荷重量及び重心位置推定部は、予め設定した回数だけ推定を行った後で負荷重量補正値及び重心位置補正値のいずれかが前記閾値よりも大きい場合には推定を終了する。このように閾値を予め設定して推定の回数に限度を設けることにより、予め設定された推定回数の後で閾値の範囲に入らないときには推定動作を終了させる。このような構成により、負荷重量の推定値が収束しなければ推定動作がいつまでも継続することを回避することができる。

また、本発明は、端的に、ロボットに追加される負荷の重量及び重心位置を推定する方法として構成することも可能である。この場合、本発明によるロボットに追加される負荷の重量及び重心位置を推定する方法は、ロボットの各軸に作用する重力トルクと摩擦トルクとを演算するステップと、前記ロボットの各軸において現実に生じているトルクから前記重力トルク及び前記摩擦トルクを減算することにより前記ロボットの各軸に作用する正味の外乱トルクを演算するステップと、前記正味の外乱トルクをロボットの手先力及び手先モーメントに変換するステップと、前記負荷が追加される前に前記ロボットの手先に生じている第１の手先力及び手先モーメント推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先力及び手先モーメント推定値との差分を計算することにより負荷の重量及び重心位置を推定するステップと、前記推定された負荷重量及び重心位置を前記ロボットの重量及び重心位置に加算した上で重力トルクを再計算するステップと、再計算された重力トルクを減算することによって正味の外乱トルクを再計算するステップと、再計算された正味の外乱トルクを手先力及び手先モーメントに変換して第３の手先力推定値及び手先モーメント推定値を演算するステップと、前記第２の手先力推定値と前記第３の手先力推定値との差分から負荷重量補正値を推定し、前記第２の手先モーメント推定値と前記第３の手先モーメント推定値との差分から重心位置補正値を推定し、前記負荷重量補正値及び重心位置補正値がそれぞれについて予め設定された閾値以下となる場合には負荷重量推定及び重心位置推定を終了し、前記負荷重量補正値及び前記重心位置補正値のいずれかが前記閾値よりも大きい場合には当該負荷重量補正値及び重心位置補正値を反映させた新たな負荷重量及び重心位置を用いて負荷重量補正値及び重心位置補正値の推定を繰り返すステップと、を含む。

本発明の第１実施例におけるロボットの負荷推定装置の構成図である。 本発明の第１実施例におけるモータ制御の構成図であり、特に、サービスロボットにおいて安全を確保しようとする場合である。 本発明の第１実施例におけるモータ制御の構成図であり、特に、産業用ロボットにおいてワーク重量を推定する場合である。 外乱オブザーバを用いない場合の本発明の第１実施例におけるモータ制御の構成図である。 外乱オブザーバを用いない第２の場合の本発明の第１実施例におけるモータ制御の構成図である。 本発明の第１実施例における作業動作を説明するフローチャートである。 本発明の第２実施例における作業動作を説明するフローチャートである。 本発明においてワークの重心位置を求める計算方法を実際の座標を用いて示す図である。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明の第１実施例におけるロボットの負荷推定装置の構成図である。図において、１はロボット、２はロボットコントローラ、３はエンドエフェクタであるグリッパ、４はワークである。ロボット１がグリッパ３でワーク４を上方から把持するハンドリング作業を想定する。

図２−１は、本発明のロボットコントローラ内における１軸分のモータ制御の構成図である。図において、２１はモータの位置速度制御部、２２はモータ、２３はエンコーダ、２４は外乱トルク演算部、２５は手先力演算部、２６はワーク重量及び重心位置推定部、２７は重力トルク及び摩擦トルク補償部である。図２−１に示されているのは、人間の近傍で用いられるサービスロボットにおいてワーク重量の推定をする例であり、この場合は人体の安全確保が最大限に配慮されなければならない。そのために、後述する図２−２に示されている産業用ロボットの場合との構成上の差異として、位置速度制御部２１の内部に、モータ２２へのトルク指令を一定限度に制限するリミッタが設けられている。このリミッタにより位置速度制御部２１からモータ２２に送られるトルク指令を一定の値以下に制限する必要があるが、それには正確な重力補償トルク（重力を考慮して補償を行ったトルク）が不可欠である。位置速度制御部２１は図示しない上位の指令生成部からの位置指令とモータ２２の後端に付属したエンコーダ２３からの位置フィードバック情報からモータ２２へのトルク指令を演算している。なお、ここで「エンコーダ」とは、この技術分野の当業者に周知であるが、サーボモータなどに付属する位置センサを意味する。

本発明による負荷推定装置では、ワーク重量及び重心位置は次のようにして推定される。まず、各モータの位置速度制御部２１から外乱トルク演算部２４への２つの矢印として示されている（重力トルク及び摩擦トルク補償部２７からの重力摩擦補償トルクが加算される前の）トルク指令と位置ＦＢ（フィードバック）値とが外乱トルク演算部２４に送られる。位置速度制御部２１からモータ２２に与えられるトルク指令は、目標とする特定のトルク値を出力せよという指令である。エンコーダ２３からの位置フィードバック値は、様々な外乱の存在を反映して現実に生じているトルクと関数関係を有する位置情報である。外乱トルク演算部２４では、位置速度制御部２１から送られたこれらの値を用い、例えば外乱オブザーバによる演算を行い、外乱トルクτdisを求める。モータ２２は、位置速度制御部２１からトルク指令を受け取るが、外乱が存在するためにトルク指令とは異なるトルクを生じる。ここで「外乱トルク」とは、外乱が存在するために現実に生じているトルクがトルク指令に対応する理想のトルクから逸脱しているトルクのズレを意味する。この時、ワークを把持する前のロボットアーム自身の重量は既知であるため、重力トルク及び摩擦トルク補償部２７でロボット自体の重量に起因する重力補償トルクと関節などにおける摩擦に起因する摩擦補償トルクを演算して、外乱トルク演算部２４に送る。外乱トルク演算部２４では、位置速度制御部２１から受け取った２つの値から計算した前記外乱トルクτdisから重力補償トルクと関節などにおける摩擦に起因する摩擦補償トルクとを減算して、ロボットの各軸に作用する純粋な外乱トルクτdisを求めることができる。

手先力演算部２５では、位置フィードバック値から一般的にヤコビ行列（Jacobian Matrix）と呼ばれる関節座標系とロボット座標系との間の微小変位関係式を求める。なお、ここで関節座標系とはロボット１の各関節に設定される座標系であり、ロボット座標系とは、ロボットのベース部分に定義される座標系である。また、フランジ座標系とは、各関節の動作によって位置姿勢が変化するロボット先端部（図１ではグリッパ３の根元）に設定される座標系である。ヤコビ行列は、異なる座標系の間での変数変換を考察する際に広く用いられる周知の手段である。具体的には、ヤコビ行列を用いることにより、一方の座標系における座標を他方の座標系における座標の行列表現として与えることができ、その転置逆行列（逆行列の転置行列）を用いることで、下記の数式１に示すように、前記各軸の外乱トルクτdisをロボット１のフランジ座標原点位置での手先力及び手先モーメント推定値Ｆestimateに変換することができる。Ｊはヤコビ行列であり、右肩の−１はＪの逆行列を意味し、更に右肩のＴは行と列とを入れ替えた転置行列を意味する。Ｔは左肩に付されることもある。ここで、図１にあるような一般的な関節６軸のロボット（自由度は６である）であれば、Ｆestimateは直交する３つの座標軸に沿った並進方向の３つの力とそれぞれの軸を中心とする回転方向の３つの回転モーメントの計６個の値からなり、ヤコビ行列は６×６行列となる。
［数式１］Ｆestimate＝（Ｊ^−１）^Ｔτdis
手先力演算部２５は、外乱トルク演算部２４から受け取った外乱トルクを、ヤコビ行列を用いてロボット座標系から見たフランジ座標原点位置での手先力及び手先モーメントに変換し、ワーク重量及び重心位置推定部２６に送る。ワーク重量及び重心位置推定部２６では、ロボットがワークを把持する前後での手先力及び手先モーメント推定値の差分を計算することにより、ワーク重量及び重心位置を推定する。以上のデータ処理により、ワーク重量及び重心位置の推定が完了する。図２−１では、ワーク重量及び重心位置推定部２６から重力トルク及び摩擦トルク補償部２７に推定されたワーク重量及び重心位置が送られる矢印が示されている。しかし、ワーク重量及び重心位置の推定をとりあえず１回だけ行う第１の実施例では、ワーク重量及び重心位置推定部２６においてワーク重量及び重心位置が推定された時点ですべてのデータ処理は完了する。

次に、上記手先力及び手先モーメント推定値（Ｆestimate）を用いてワーク重量及び重心位置を推定する方法について、図２−１および図３の作業動作を説明するフローチャートを用いて再度説明する。図３に示すように、ワーク重量及び重心位置の推定は下記の（１）〜（４）の手順で行われる。
（１）ワーク把持前の手先力演算（ＳＴＥＰ１）
ロボット１を待機姿勢からグリッパ３がワーク４を把持する直前の姿勢に作業プログラムを実行する。ただし、各モータに作用する摩擦の方向が把持前後で同じになるように微小量（１ｍｍ程度）だけグリッパを上方に動作させるため、予め微小量だけ下方に一度ロボットを降下させてから上昇させる。この上昇後の外乱トルクτdisを各モータの外乱トルク演算部２４で求め、手先力演算部２５で上述した数式１を用いて第１の手先力及び手先モーメント推定値Ｆestimate1を求める。
（２）ワーク把持（ＳＴＥＰ２）
グリッパ３でワーク４を把持し、ロボットでグリッパ３を微小量（１ｍｍ程度）持ち上げ、ワークを地面から浮かせる。
（３）ワーク把持後の手先力演算（ＳＴＥＰ３）
ＳＴＥＰ１と同様の手順により、各モータの外乱トルク演算部２４で求めた外乱トルクτdisから、手先力演算部２５で第２の手先力及び手先モーメント推定値Ｆestimate2を求める。
（４）差分演算（ＳＴＥＰ４）
ワーク重量及び重心位置推定部２６で、ワーク把持前の第１の手先力及び手先モーメント推定値Ｆestimate1と、ワーク把持後の第２の手先力及び手先モーメント推定値Ｆestimate2との差分ΔＦestimate1を求める。手先力推定値はロボット座標系のＸＹＺ成分を持っているが、ここではＺ成分のみを使用する。この時、第１と第２の手先力推定値の差分のＺ成分ΔＦestimate1_ｚがワーク重量推定値ｍに相当する（Ｆｚ＝ｍｇ ここで、ｇは重力加速度を表す）。また、手先モーメント推定値も同様にロボット座標系のＸＹＺ成分を持っている。これらの手先力推定値と手先モーメント推定値はフランジ座標系からロボット座標系までの回転行列を用いることで、フランジ座標系における手先力推定値と手先モーメント推定値に変換できる。手先力推定値も同様にロボット座標系からフランジ座標系の値に変換する。フランジ座標系における手先モーメント推定値を手先力推定値で割ることで、フランジ座標系における重心位置を求めることができる。具体的にはフランジ座標系の手先力推定値と手先モーメント推定値の差分のＸＹＺ成分をそれぞれΔＦestimate1_ｘfとΔＦestimate1_ｙfとΔＦestimate1_ｚfとΔＦestimate1_ＭｘfとΔＦestimate1_ＭｙfとΔＦestimate1_Ｍｚfとすると、フランジ座標系における重心の座標成分は、次のように求められる。すなわち、Ｘ成分は、Ｘｇf＝（−ΔＦestimate1_Ｍｙf／ΔＦestimate1_ｚf）又はＸｇf＝ΔＦestimate1_Ｍｚf／ΔＦestimate1_ｙf、同様にＹ成分は、Ｙｇf＝ΔＦestimate1_Ｍｘf／ΔＦestimate1_ｚf又は（−ΔＦestimate1_Ｍｚf／ΔＦestimate1_ｘf）、Ｚ成分は、Ｚｇf＝ΔＦestimate1_Ｍｙf／ΔＦestimate1_ｘf又は（−ΔＦestimate1_Ｍｘf／ΔＦestimate1_ｙf）となる。このワーク重量推定値ｍ及びフランジ座標系における重心位置推定値（Ｘｇf，Ｙｇf，Ｚｇf）を用いて負荷イナーシャ等の演算を行う。具体的な座標成分を用いた以上の計算の概要は、図５に示されている。なお、以上の説明において「手先力」と称しているのは上述した３つの座標軸に沿った並進方向の力であり、「手先モーメント」と称しているのは３つの座標軸を中心として回転する方向のモーメントを意味する。

このように、本実施例では、４段階でワーク重量及び重心位置の推定を行っているが、各段階での推定に必要な処理自体は大変短く、通常のワーク把持動作内で十分に処理が完了し、タクトタイムもほとんど増加しない。

また、本実施例では、ワーク把持を上方から行ったが、横からの把持でもワーク重量及び重心位置の推定は可能である。
本発明による方法では、上述したように、ロボットの各軸に作用する重力トルク及び摩擦トルクに関して補償がなされたロボットの各軸に作用する外乱トルクを計算し、外乱トルクをロボットの手先力及び手先モーメントに変換する。以上の計算をワーク把持の前後で２回行うことにより、一定の重量を有するワークを把持したことによって生じるトルクの差異に着目するのである。すなわち、エンドエフェクタでワークを把持する直前に計測した第１の手先力及び手先モーメント推定値とワークを把持して微小量だけ持ち上げた後に計測した第２の手先力及び手先モーメント推定値との差分からワーク重量及び重心位置推定値を算出することにより、センサを必要とすることなく短時間でワーク重量及び重心位置をオンラインで推定できる。また、上述した従来技術では必要であったワーク重量推定用のロボットの可動範囲を大きく使うような特定動作も不要である。

前記実施例１では、ワーク把持前後の手先力及び手先モーメント推定値の差からワーク重量及び重心位置を推定したが、本実施例では更に推定したワーク重量及び重心位置が正しいかを収束演算で確認する方法について述べる。

図４の作業動作を説明するフローチャートを用いて説明する。図４に示すように、ワーク重量及び重心位置の推定の（１）〜（４）は前記実施例１と同様であるため説明を省略する。本実施例では下記の（５）〜（７）の手順が追加されている。
（５）重量パラメータの更新（ＳＴＥＰ５）
前記実施例１で推定されたワーク重量推定値ｍ及びフランジ座標系における重心位置推定値（Ｘｇf，Ｙｇf，Ｚｇf）を基に、重力トルク演算に使用する手先負荷のパラメータ値を更新する。すなわち、推定されたワーク重量及び重心位置とロボットの自重及び重心位置とを加算することにより、把持されたワークも含めてロボット重量と想定する。その上で実施例１の重量推定を反復的に実行することにより、最初に推定したワーク重量及び重心位置の精度を向上させることを考えるのである。
（６）重力補償と手先力及び手先モーメント推定値の再演算（ＳＴＥＰ６）
更新された手先負荷パラメータ値を用いて、重力トルク及び摩擦トルク補償部２７で重力トルクを再計算し、外乱トルク演算部２４で更新された重力トルクを減算した外乱トルクτdisを再演算し、前記手先力演算部２５で前記外乱トルクτdisを手先力及び手先モーメントに変換して第３の手先力及び手先モーメント推定値Ｆestimate3を演算する。
（７）差分演算と収束演算（ＳＴＥＰ７）
ワーク重量及び重心位置推定部２６が、実施例１で推定された第２の手先力推定値Ｆestimate2と今回測定した第３の手先力推定値Ｆestimate3の差分のＺ成分ΔＦestimate2_zからワーク重量補正値Δｍを推定する。同様に、実施例１で推定された第２の手先モーメント推定値Ｆestimate2と今回測定した第３の手先モーメント推定値Ｆestimate3の差分と、実施例１で推定された第２の手先力推定値Ｆestimate2と今回測定した第３の手先力推定値Ｆestimate3の差分からワーク重心位置補正値（ΔＸｇf，ΔＹｇf，ΔＺｇf）を推定する。このワーク重量補正値Δｍ及びワーク重心位置補正値（ΔＸｇf，ΔＹｇf，ΔＺｇf）は前記実施例１での推定誤差に相当するため、収束演算させることで誤差が大きいかどうかを判断する。

重力トルク及び摩擦トルク補償部２７は、ワーク重量及び重心位置を用いて重力補償トルク補正分を演算し、外乱トルク演算部２４へ送る。外乱トルク演算部２４は、位置速度制御部２１から得た状態量から外乱トルクを計算する際に、重力トルク及び摩擦トルク補償部２７から得た重力補償トルク補正分を考慮することにより外乱トルクが小さくなり、ワーク重量及び重心位置がゼロに収束していく。所定の限度までゼロの近傍になった（すなわち、重力補償トルクが正しい）と判定された場合には、重力補償トルク補正分を元々補償していた重力補償トルクに加算して、重力トルク及び摩擦トルク補償部２７から位置速度制御部２１に補償している重力補償トルクを更新する。このように、収束させて正しいことを判断してから位置速度制御部に補償しているため、何らかの理由により不正確なワーク重量及び重心位置が計算され、結果的に大きな重量補償トルクを補償してモータが暴走することを回避できる。

ワーク重量補正値Δｍ及びワーク重心位置補正値（ΔＸｇf，ΔＹｇf，ΔＺｇf）がそれぞれについて予め設定された閾値以下である場合には、ワーク重量及び重心位置推定を終了する。いずれかが閾値より大きい場合にはＳＴＥＰ５に戻り、ワーク重量補正値Δｍ及びワーク重心位置補正値（ΔＸｇf，ΔＹｇf，ΔＺｇf）をそれぞれ前記ワーク重量推定値ｍ、重心位置推定値（Ｘｇf，Ｙｇf，Ｚｇf）に加算して重力トルク演算に使用する手先負荷のパラメータ値を更新し、重力トルク及び摩擦トルク補償部２７で重力トルクを再計算し、外乱トルク演算部２４で更新された重力トルクを減算した外乱トルクτdisを再演算し、前記手先力演算部２５で前記外乱トルクτdisを手先力及び手先モーメントに変換して第４の手先力推定値Ｆestimate4を演算する。前回の第３の手先力推定値Ｆestimate3と今回測定した第４の手先力推定値Ｆestimate4の差分のＺ成分ΔＦestimate3_zからワーク重量補正値Δｍ2を推定する。重心位置補正値に関しても同様である。このワーク重量補正値Δｍ2及びワーク重心位置補正値（ΔＸｇf2，ΔＹｇf2，ΔＺｇf2）が予め設定された閾値以下である場合にはワーク重量及び重心位置推定を終了し、いずれかが閾値より大きい場合にはＳＴＥＰ５に戻り、再度重力トルクの再計算からワーク重量補正値及びワーク重心位置補正値の推定を繰り返す。

本実施例では、ワーク重量推定はワーク重量補正値Δｍが閾値以下になるかならないかで判断していたが、前記ワーク重量及び重心位置推定部２６内で、予め設定した推定回数で閾値以下にならない場合には推定を終了するようにしてもよい。

また、ワーク重量補正値Δｍが前回値よりも大きくなる場合には、別の何らかの外乱が作用しているとして推定を終了するようにしてもよい。
また、ワーク重量とワーク重心位置を同時に補正せず、１回目で求めたワーク重心位置のみ用いてワーク重量のみ収束計算させても良く、演算負荷を減らすこともできる。

実施例２の方式によれば、ワーク把持前後の外乱トルクから求めたワーク重量推定値及び重心位置推定値を用いて重力トルクを再計算して、外乱トルクを再推定して手先力及び手先モーメントに変換した第３の手先力を演算し、第２と第３の手先力推定値及び手先モーメント推定値の差分からワーク重量補正値及び重心位置補正値を推定し、ワーク重量補正値及び重心位置補正値が閾値以下である場合にはワーク重量及び重心位置推定を終了し、いずれかが閾値より大きい場合にはワーク重量補正値及び重心位置補正値をワーク重量推定値及び重心位置推定値に加算して重力トルクの再計算からワーク重量補正値及び重心位置補正値の推定を繰り返すことで、推定値を収束させて推定値が正しいかを判断しており、推定精度が高い。また、予め設定した推定回数で閾値以下にならない場合には推定を終了することで、収束せずに終了しないことを防ぐことができる。

以下では、図２−２から図２−４に示された場合について、追加的な説明を行う。
図２−２は、産業用ロボットのハンドリング作業においてワーク重量を推定する例である。図２−１に示されていたサービスロボットの場合と同様に、図２−２において実施例１のようにワーク重量を１回だけ推定する場合には、ワーク重量及び重心位置推定部２６においてワーク重量が推定された時点で本発明による装置の動作は終了する。しかし、実施例２のようにワーク重量がいったん推定されると推定されたワーク重量をもはや外乱と見なすのではなくロボット自体の重量に含めて動作を反復する場合には、推定されたワーク重量を考慮して重力トルク及び摩擦トルク補償部２７で重力補償トルク補正分を再度演算し、外乱トルク演算部２４へ送られる。外乱トルク演算部２４において、新たに計算された重力トルクを外乱トルクから差し引くことで外乱トルクが小さくなる。この処理を反復的に実行することにより、ワーク重量及び重心位置推定部２６から重力トルク及び摩擦トルク補償部２７に与えられるワーク重量はゼロに収束していく。所定の限度までゼロの近傍になった（つまり、重力補償トルクが正しい）と判定された場合に、ワーク重量の推定を完了する。推定したワーク重量を基にして、例えば、加速時間を演算するために必要なイナーシャ値を変更することにより、ワークに応じて最適な加減速度が得られる。この方法により精度の高いワーク重量の推定が可能になるので、重量の極めて僅かな差に基づいてワークの仕分けを行うことができる。

図２−３は、外乱オブザーバを用いない応用例である。図２−１（サービスロボットの場合）がベースであるが、図２−２（産業用ロボットの場合）にも適用可能である。アンプに内蔵された電流センサで実電流を計測し、外乱トルク演算部２４が、計測された実電流とトルク指令との差分を計算して、外乱トルクを求める。ワーク重量を推定する方法は、図２−１と同じである。

図２−４は、外乱オブザーバを用いない別の応用例である。図２−３と同様に図２−１がベースであるが、図２−２にも適用可能である。モータ２２と負荷との間の減速機にトルクセンサを設置し、このトルクセンサを用いて実トルクを計測する。外乱トルク演算部２４が、計測された実トルクとトルク指令との差分として外乱トルクを求める。ワーク重量を推定する方法は、図２−１と同じである。

本発明は、産業用ロボットで組立作業やハンドリング作業を行う際に、オンラインでワーク重量及び重心位置を推定することで、推定された重量及び重心位置に応じて制御機能を選択したり切り替えたりできる。また、サービスロボットのように人間と近接して用いられるロボットでは、把持されたワークの重量によってワークの種類を選別したりすることができる。

１ ロボット
２ ロボットコントローラ
３ グリッパ
４ ワーク
２１ 位置速度制御部
２２ モータ
２３ エンコーダ
２４ 外乱トルク演算部
２５ 手先力演算部
２６ ワーク重量及び重心位置推定部
２７ 重力トルク及び摩擦トルク補償部

Claims (4)

1. ロボットに追加される負荷の重量及び重心位置を推定する装置であって、
   前記ロボットの各軸に作用する重力トルク及び摩擦トルクを演算する重力トルク及び摩擦トルク演算部と、
   前記ロボットの各軸において現実に生じているトルクから前記重力トルク及び前記摩擦トルクを減算することにより前記ロボットの各軸に作用する正味の外乱トルクを演算する外乱トルク演算部と、
   前記正味の外乱トルクをロボットの手先力及び手先モーメントに変換する手先力演算部と、
   前記負荷が追加される前に前記ロボットの手先に生じている第１の手先力推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先力推定値との差分を計算することにより負荷の重量を推定し、更に、前記負荷が追加される前に前記ロボットの手先に生じている第１の手先モーメント推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先モーメント推定値との差分を計算することにより負荷の重心位置を推定する負荷重量及び重心位置推定部と、
   を有することを特徴とするロボットの負荷推定装置。
2. 請求項１記載の装置において、
   前記重力トルク及び摩擦トルク演算部は前記推定された負荷重量及び重心位置を前記ロボットの重量及び重心位置に加算した上で重力トルクを再計算し、前記外乱トルク演算部は再計算された重力トルクを減算することによって正味の外乱トルクを再計算し、前記手先力演算部は再計算された正味の外乱トルクを手先力に変換して第３の手先力推定値及び手先モーメント推定値を演算し、
   前記負荷重量及び重心位置推定部は前記第２の手先力推定値と前記第３の手先力推定値との差分から負荷重量補正値を推定し、前記第２の手先モーメント推定値と前記第３の手先モーメント推定値との差分から重心位置補正値を推定し、前記負荷重量補正値及び重心位置補正値がそれぞれについて予め設定された閾値以下となる場合には負荷重量推定及び重心位置推定を終了し、前記負荷重量補正値及び重心位置補正値のいずれかが前記閾値よりも大きい場合には当該負荷重量補正値及び重心位置補正値を反映させた新たな負荷重量及び重心位置を用いて負荷重量補正値及び重心位置補正値の推定を繰り返すことを特徴とする装置。
3. 請求項１又は請求項２記載の装置において、前記負荷重量及び重心位置推定部は、予め設定した回数だけ推定を行った後で負荷重量補正値及び重心位置補正値のいずれかが前記閾値よりも大きい場合には推定を終了することを特徴とする装置。
4. ロボットに追加される負荷の重量及び重心位置を推定する方法であって、
   ロボットの各軸に作用する重力トルクと摩擦トルクとを演算するステップと、
   前記ロボットの各軸において現実に生じているトルクから前記重力トルク及び前記摩擦トルクを減算することにより前記ロボットの各軸に作用する正味の外乱トルクを演算するステップと、
   前記正味の外乱トルクをロボットの手先力及び手先モーメントに変換するステップと、
   前記負荷が追加される前に前記ロボットの手先に生じている第１の手先力推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先力推定値との差分を計算することにより負荷の重量を推定し、更に、前記負荷が追加される前に前記ロボットの手先に生じている前記ロボットの手先に生じている第１の手先モーメント推定値と、前記負荷が追加され微小量だけ持ち上げた後に前記ロボットの手先に生じている第２の手先モーメント推定値との差分を計算することにより負荷の重心位置を推定するステップと、
   前記推定された負荷重量及び重心位置を前記ロボットの重量及び重心位置に加算した上で重力トルクを再計算するステップと、
   再計算された重力トルクを減算することによって正味の外乱トルクを再計算するステップと、
   再計算された正味の外乱トルクを手先力及び手先モーメントに変換して第３の手先力推定値及び手先モーメント推定値を演算するステップと、
   前記第２の手先力推定値と前記第３の手先力推定値との差分から負荷重量補正値を推定し、前記第２の手先モーメント推定値と前記第３の手先モーメント推定値との差分から重心位置補正値を推定し、前記負荷重量補正値及び重心位置補正値がそれぞれについて予め設定された閾値以下となる場合には負荷重量推定及び重心位置推定を終了し、前記負荷重量補正値及び重心位置補正値のいずれかが前記閾値よりも大きい場合には当該負荷重量補正値及び重心位置補正値を反映させた新たな負荷重量及び重心位置を用いて負荷重量補正値及び重心位置補正値の推定を繰り返すステップと、
   を含むことを特徴とする負荷推定方法。