JP7148321B2 - 多関節ロボットの制御装置 - Google Patents

Description

本発明は多関節ロボットの制御装置に関する。

従来、ロボットの各可動軸に生ずるトルクであって、溶接点からロボットに加わる外力によって生ずるトルクを正確に算出するために、外積及び内積を用いた論理的な式を用いるスポット溶接ロボットが知られている（例えば、特許文献１参照。）。

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

前記スポット溶接ロボットでも、外力の方向と各可動軸の回転軸方向延びる方向との関係を考慮して、各可動軸に生ずるトルクが計算されている。しかし、各可動軸は減速機を介してサーボモータが接続されており、減速機内の回転抵抗、サーボモータ内の回転抵抗等が実際は存在しているので、抵抗を考慮しない理論式で各可動軸に生ずるトルクを求めると、実際に生ずるトルクと異なることが多い。特に、外力モーメントの方向と各可動軸の回転軸方向とがなす角度が大きくなる程、理論式から得られる結果に対する差が大きくなり、当該角度が１０°、２０°程度で差が無視できないものとなる場合も多い。

一方、ロボットの複数の可動軸のうち典型的には選択した１つの可動軸をワークの動き、ツールの動作によって生ずる力等に追従させる制御を行う場合がある。この追従制御は、選択された可動軸に対して、モータの電流値を制限したり、位置／速度ループゲインを下げたりすることによって行われる。外力によるトルクの影響を受けやすい可動軸でトルク制限等を行うと追従性がよくなるため、外力によって各可動軸モータに生ずるトルクを精度よく推定できることが望ましい。

本発明は、前述の事情に鑑みてなされている。本発明の目的の一つは、複数の可動軸のうちアームに加わる外力を感度よく検出できる可動軸の正確な判断を可能とする多関節ロボットの制御装置の提供である。

上記課題を解決するために、本発明は以下の手段を採用する。
本発明の一態様は、ロボットのアームを構成する複数のアーム部材が複数の可動軸周りに動く多関節ロボットの制御装置であって、前記アームの先端部にサンプル外力を加えた時の前記各可動軸周りのトルク又はモータ電流値の検出値に基づいて求められた補正係数又は補正関数が格納されている記憶部と、前記アームの前記先端部に加わる外力の方向に応じて変化する前記各可動軸におけるトルク検出感度を、前記各可動軸の回転軸方向と前記外力の方向との関係を示す数式要素と、前記補正係数又は前記補正関数とに少なくとも基づき演算するトルク感度演算手段と、を備え、前記トルク検出感度は、前記各可動軸におけるモータの電流検出値又はトルクセンサの検出値の前記外力に対する感度を示すものである。

上記態様では、大きさおよび方向がわかっているサンプル外力に基づいて補正係数又は補正関数が求められ、各可動軸の回転軸方向と外力の方向との関係を示す数式要素に加え、補正係数又は補正関数を用いて、各可動軸のトルク検出感度が算出される。このため、複数の可動軸のうちアームに加わる外力を感度よく検出できる可動軸の正確な選択が可能となる。

上記態様において、好ましくは、前記トルク感度演算手段が、前記各可動軸と前記外力の作用点との距離と、前記数式要素と、前記補正係数又は前記補正関数とを少なくとも用いて前記各可動軸における前記トルク検出感度を演算する。
当該態様では、各可動軸と外力の作用点との距離も用いて、各可動軸のトルク検出感度が算出される。このため、複数の可動軸のうちアームに加わる外力を感度よく検出できる可動軸の正確な判断が可能となる。

上記態様において、好ましくは、前記複数の可動軸のうち前記トルク検出感度が優位である少なくとも１つを前記外力に追従する可動軸として選択する軸決定手段を備える。
当該態様では、前述のように正確に算出されたトルク検出感度を用いて、複数の可動軸のうち優位である少なくとも１つが外力に追従する可動軸として決定される。これは、ワークの動き、ツールの動作によって生ずる力等にロボットを追従させる制御を正確に行う上で有利である。

上記態様において、好ましくは、前記アームにおいて前記外力が加わる位置の検出又は推定を行う外力位置取得手段をさらに備え、前記外力位置取得手段は、前記複数の可動軸のうち前記トルク検出感度が優位である少なくとも１つの前記可動軸周りのトルク又はモータ電流値の検出値を、前記検出又は前記推定に用いる。
当該態様では、トルク検出感度が優位である可動軸周りのトルク又はモータ電流値の検出値が、外力が加わる位置の検出又は推定に用いられるので、外力が加わる位置をより正確に知ることができる。

本発明によれば、複数の可動軸のうちアームに加わる外力を感度よく検出できる可動軸の正確な判断を可能となる。

本発明の一実施形態のロボットおよび制御装置の概略構成図である。 本実施形態のロボットの動作説明図である。 本実施形態の制御装置のブロック図である。

本発明の一実施形態に係る多関節ロボットであるロボット１０の制御装置２０が、図面を用いながら以下説明されている。
本実施形態のロボット１０は、図１に示されるように、ベースＢとアーム１０ａとを有する。アーム１０ａは複数のアーム部材１１，１２，１３，１４，１５，１６を有し、複数のアーム部材１１，１２，１３，１４，１５，１６がそれぞれ可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６周りに動く。なお、図１に示されるロボット１０は垂直多関節ロボットであるが、水平多関節ロボットであってもよく、特定の種類のロボットに限定されない。

ロボット１０は、複数の可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６周りにそれぞれアーム部材１１，１２，１３，１４，１５，１６を駆動する複数のサーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａ（図３）を有する。サーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａにそれぞれ減速機１１ｂ，１２ｂ，１３ｂ，１４ｂ，１５ｂ，１６ｂ（図３）が接続され、サーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａの駆動力が減速機１１ｂ，１２ｂ，１３ｂ，１４ｂ，１５ｂ，１６ｂによって減速されてアーム部材１１，１２，１３，１４，１５，１６に伝達される。また、各サーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａはその作動位置を検出するための作動位置検出装置（図示せず）を有し、作動位置検出装置は一例としてエンコーダである。作動位置検出装置の検出結果は制御装置２０に送信される。

制御装置２０は、図３に示されるように、ＣＰＵ等であるプロセッサ２１と、表示装置２２と、不揮発性ストレージ、ＲＯＭ、ＲＡＭ等を有する記憶部２３と、キーボード等の入力装置２４と、送受信部２５と、サーボモータ１１ａ～１６ａに各々対応したサーボ制御器２６とを備え、ロボット１０のサーボモータ１１ａ～１６ａを制御する。記憶部２３にはシステムプログラム２３ａが格納されており、システムプログラム２３ａが制御装置２０の基本機能を担っている。

記憶部２３には、動作プログラム２３ｂと、補正係数取得プログラム２３ｃと、感度演算プログラム（トルク感度演算手段）２３ｄと、軸決定プログラム（軸決定手段）２３ｅとが格納されている。プロセッサ２１は、動作プログラム２３ｂに基づきサーボモータ１１ａ～１６ａを制御し、これによりロボット１０がアーム１０ａの先端部に設けられたツールＴを用いて所定の作業を行う。

また、プロセッサ２１は、入力装置２４から所定の開始信号の入力があった時、又は、送受信部２５において所定の開始信号が受信された時に、補正係数取得プログラム２３ｃに基づき各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の補正係数を求め、求めた補正係数を記憶部２３に格納する。

例えば、プロセッサ２１は、サーボモータ１１ａ～１６ａを制御することによって、アーム１０ａを第１の姿勢にする。この時、ツールＴにサンプル外力Ｆが加えられる。プロセッサ２１によるサーボモータ１１ａ～１６ａの制御によってツールＴを所定の部材に押し当て、これによってサンプル外力ＦがツールＴに加えられてもよく、所定の装置によってツールＴを押すこと、ツールＴが所定の物体を保持すること等によって、ツールＴにサンプル外力Ｆが加えられてもよい。

この時のサンプル外力Ｆの方向および大きさがわかっており、サンプル外力Ｆの方向および大きさは制御装置２０に入力される。サンプル外力Ｆの方向および大きさが入力装置２４又は送受信部２５を介して制御装置２０に入力されてもよく、サンプル外力Ｆの方向および大きさがアーム１０ａの先端部、ツールＴ等に設けられた力センサＤによって検出され、検出結果が制御装置２０に送信されてもよい。

なお、サンプル外力Ｆの方向および大きさは制御装置２０が認識しているアーム１０ａの基準座標系に対応したものである。一例では、サンプル外力ＦはツールＴの一部に可動軸Ｊ６に沿った方向又は垂直な方向に加わるものであり、サンプル外力Ｆの方向がアーム１０ａの姿勢の変更に応じて変化する。

プロセッサ２１は、補正係数取得プログラム２３ｃに基づき、可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６のそれぞれについて、補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎを求める。補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎを求めるため、一例として、プロセッサ２１は、アーム１０ａの姿勢情報又は各サーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａの作動位置検出装置の情報からわかる位置ベクトルＰ_ｉおよび回転軸ベクトル方向の単位ベクトルＡ_Ｕｉと、サンプル外力Ｆの方向および大きさとを、下記式（１）に適用して、理論的なトルクを求める。

位置ベクトルＰ_ｉ、単位ベクトルＡ_Ｕｉ、垂線の長さ（各可動軸Ｊ１～Ｊ６と外力Ｆの作用点との距離）ｒ_ｉ等のｉには可動軸Ｊ１～Ｊ６に対応する１～６の数字が入り、例えば、Ｐ_１は可動軸Ｊ１に関する位置ベクトル、Ｐ_２は可動軸Ｊ２に関する位置ベクトル、・・・Ｐ_６は可動軸Ｊ６に関する位置ベクトルである。

Ｐ_ｉは、サンプル外力Ｆの作用点から各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６に下ろした垂線と回転軸の交点を点Ｏとして、点ＯからＦの作用点を結ぶ位置ベクトルであり、Ａ_Ｕｉは、各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６に沿った方向に延びる単位ベクトルであり、φは、回転軸ベクトルとＰ_ｉ×Ｆ（外積）の成す角度である。Ｐ_ｉ×Ｆ（外積）で得られるベクトルは、ベクトルＰ_ｉとベクトルＦの両方に直交する。
Ｐ_ｉ×Ｆ（外積）およびｃｏｓφは、各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の回転軸方向と外力Ｆの方向との関係を示す数式要素である。

そして、プロセッサ２１は、補正係数取得プログラム２３ｃに基づき、可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の各々について、サンプル外力Ｆに対し式（１）によって理論的に得られる理論トルク（モーメントＭ_ａｉ）と、サンプル外力Ｆを加えた時にサーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａによって各々実測されるモータ電流値又はトルク値とに基づき、補正係数を求め、求められた補正係数を対応する可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の番号と共に記憶部２３に格納する。各サーボモータ１１ａ～１６ａにトルクセンサが設けられている場合、各サーボモータ１１ａ～１６ａのトルクセンサの検出値が前記補正係数の計算に用いられてもよい。

ここで、好ましくは補正係数はφに対応するように求められ、記憶部２３に格納される。例えば、第１の姿勢において、式（１）に示される位置ベクトルＰ_ｉとサンプル外力Ｆの外積と、可動軸Ｊ１の回転軸方向（可動軸Ｊ１の単位ベクトルＡ_Ｕｉ）とが成す角度φが１０°である場合、記憶部２３には、第１の姿勢およびサンプル外力Ｆの時の可動軸Ｊ１の補正係数Ｒ_１が角度φに対応して格納される。第２の姿勢、第３の姿勢、・・・第ｎの姿勢の時に同様に補正係数Ｒ_２、Ｒ_３、・・・Ｒ_ｎが角度φに対応して格納される。図１は可動軸Ｊ１に関する位置ベクトルＰ_ｉ、可動軸Ｊ１の単位ベクトルＡ_Ｕｉ等が示されているが、可動軸Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の各々について、同様に補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎが角度φに対応して格納される。

記憶部２３に格納される補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎは、角度φを所定の角度おきに変化させて得られるものであってもよく、角度φをランダムに変化させて得られるものであってもよい。

プロセッサ２１は感度演算プログラム２３ｄにより作動し、記憶部２３に格納されている補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎに基づき得られる補正関数ｆ_ｉ（φ）を下記式（２）に適用することによって、実際にアーム１０ａに働く外力Ｆに対するトルク検出感度Ｓ_ｉを演算する。

トルク検出感度Ｓ_ｉ、回転軸ベクトルＡ_Ｕｉ、垂線の長さ（各可動軸Ｊ１～Ｊ６と外力Ｆの作用点との距離）ｒ_ｉ、ギヤ比Ｇ_ｉ等のｉには可動軸Ｊ１～Ｊ６に対応する１～６の数字が入り、例えば、Ｓ_１は可動軸Ｊ１のトルク感度、Ｓ_２は可動軸Ｊ２のトルク感度、・・・Ｓ_６は可動軸Ｊ６のトルク感度である。ｒ_ｉは外力Ｆの作用点から各回転軸ベクトルＡ_Ｕｉに下した垂線の長さであり、Ｇ_ｉは減速機１１ｂ，１２ｂ，１３ｂ，１４ｂ，１５ｂ，１６ｂの各々のギヤ比である。

補正関数ｆ_ｉ（φ）のｉには可動軸Ｊ１～Ｊ６に対応する１～６の数字が入り、ｆ_１（φ）は可動軸Ｊ１の補正関数、ｆ_２（φ）は可動軸Ｊ２の補正関数、・・・ｆ_６（φ）は可動軸Ｊ６の補正関数である。
補正関数ｆ_ｉ（φ）は、可動軸Ｊ１～Ｊ６の各々について、補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎとその時の角度φとの関係から得られる式である。補正関数ｆ_ｉ（φ）の代わりに、可動軸Ｊ１～Ｊ６の各々について、補正係数Ｒ_１、Ｒ_２、Ｒ_３、・・・Ｒ_ｎとその時の角度φとを対応させた補正テーブルを用いることも可能である。

一例として、可動軸Ｊ１のトルク検出感度Ｓ_１について、図１および図２を用いて説明する。図１と図２は、アーム部材１４の可動軸Ｊ４周りの回転位置が互いに異なるが、他の可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ５，Ｊ６の回転位置は同じである。この場合、図１と図２は、可動軸Ｊ１の回転軸ベクトルＡ_Ｕ１とＰ_１×Ｆ（外積）との成す角度φが互いに異なり、角度φは図１よりも図２の方が大きくなる。

一方、前述のように、可動軸Ｊ２の補正係数Ｒ_１、Ｒ_２、・・・Ｒ_ｎが角度φに対応して格納され、例えば、角度φが１０°、２０°、および３０°の時の補正係数Ｒ_１、Ｒ_２、およびＲ_３が０．８５、０．６５、および０．４である時、可動軸Ｊ１の補正関数ｆ_１（φ）は、両者を対応付けるための例えば二次曲線の近似関数となる。近似関数はその他のｎ次関数、対数関数、指数関数等であってもよい。

例えば、図１においてφが１０°であり、図２においてφが２０°である場合、図１の状態で可動軸Ｊ１の補正係数Ｒが０．８５となり、図２の状態で可動軸Ｊ１の補正係数Ｒが０．６５となる。このように角度φと補正係数Ｒとが対応するように、各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の補正関数ｆ_ｉ（φ）が作成される。

式（２）に示されるように、各可動軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６のトルク検出感度Ｓ_ｉは、垂線の長さｒ_ｉが大きい程、または、補正関数ｆ_ｉ（φ）により求められる補正係数が大きいほど、大きくなる（良好になる）が、角度φが大きい程小さくなる。なお、補正係数はφが小さいほど大きくなる傾向がある。
また、式（２）に示されるように、ギヤ比が小さい方がトルク検出感度Ｓ_ｉが大きくなる。

実際にツールＴに働く外力Ｆの大きさおよび方向が入力装置２４、送受信部２５等を介して入力された時、又は、実際にツールＴに働く外力Ｆである力センサＤの検出値を受信した時に、プロセッサ２１は、式（２）を用いて可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求める。
なお、補正関数ｆ_ｉ（φ）を作成する代わりに、角度φと補正係数Ｒ_１、Ｒ_２、・・・Ｒ_ｎとが対応している補正テーブルを作成してもよい。この場合、プロセッサ２１は、補正テーブルに基づき補正係数を決定し、決定された補正係数を式（２）のｆ_ｉ（φ）の位置に代入し、可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求める。
また、可動軸Ｊ１～Ｊ６にそれぞれ１つの補正係数が決定されており、当該補正係数が補正関数ｆ_ｉ（φ）および補正テーブルの代わりに記憶部２３に格納されていてもよい。この場合、プロセッサ２１は、記憶部２３に格納されている補正係数を式（２）のｆ_ｉ（φ）の位置に代入し、可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求める。

そして、プロセッサ２１は、可動軸Ｊ１～Ｊ６のうちトルク検出感度Ｓ_ｉが優位である典型的には１つの可動軸を、動作プログラム２３ｂに基づく所定の作業中に作業相手に追従させる可動軸として設定する。優位であるとは、例えば、可動軸Ｊ１～Ｊ６のトルク検出感度Ｓ_ｉのうち最も数値が大きいもの、又は２番目に数値が大きいものである。

なお、プロセッサ２１が、可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを所定の表示装置２２に表示してもよく、トルク検出感度Ｓ_ｉが優位である１つ、２つ、又は複数の可動軸を表示装置２２に表示してもよい。また、プロセッサ２１が、可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを表示のために他の装置に送信してもよく、トルク検出感度Ｓ_ｉが優位である１つ、２つ、又は複数の可動軸を表示のために他の装置に送信してもよい。

トルク検出感度Ｓ_ｉが優位である可動軸が表示される場合、操作者は、優位である可動軸のうち少なくとも１つを、動作プログラム２３ｂに基づく所定の作業中に作業相手に追従させる可動軸として設定する。

また、上位制御装置３０が、制御装置２０から、求められた補正係数を対応する可動軸Ｊ１～Ｊ６の番号と共に受信し、受信した補正係数を可動軸Ｊ１～Ｊ６の番号と共にメモリ３１に格納してもよい。又は、上位制御装置３０が、制御装置２０から、求められた補正関数又は補正テーブルを可動軸Ｊ１～Ｊ６の番号と共に受信し、受信した補正関数又は補正テーブルを可動軸Ｊ１～Ｊ６の番号と共にメモリ３１に格納してもよい。

この時、上位制御装置３０は、好ましくは、ロボット１０の機種の情報、ロボット１０のサイズの情報、ロボット１０の作業に関する情報、動作プログラム２３ｂの種類の情報、サーボモータの種類の情報、およびツールＴの種類の情報のうち少なくとも１つを含む適用情報も制御装置２０から受信し、受信した補正係数、補正関数、又は補正テーブルを適用情報と対応付けてメモリ３１に格納する。上位制御装置３０は、他のロボットの制御装置から同様の情報を受信し、受信した情報をメモリ３１に格納する。

上位制御装置３０は、例えば新たに設置されたロボットの制御装置から、要求および情報を受信する。受信する情報は、新たに設置されたロボットの機種、ロボットのサイズ、動作プログラムの種類、ロボットの作業、サーボモータの種類、ツールの種類等である。
この場合、上位制御装置３０は、メモリ３１に格納されている適用情報のうち受信した情報に近い又は一致する適用情報を決定し、決定された適用情報に応じた補正係数、補正関数、又は補正テーブルを新たに設置されたロボットの制御装置に送信し、新たに設置されたロボットの制御装置において、受信する補正係数、補正関数、又は補正テーブルが用いられる。

［実施例１］
上記実施形態は、移動するワークをロボット１０によって支持する場合に使用できる。この場合、ワークは、例えばロール成形、押出し成形等によって順次成形されたワークをロボット１０の先端部のツールＴによって支持し、ロボット１０はツールＴをワークに追従させる。ロボット１０は成形機およびワークに対して所定の位置に配置されており、当該位置においてワークを支持する。その時のロボット１０の姿勢と、当該姿勢においてワークからツールＴに加わる力の方向と、記憶部２３に格納されている補正係数、補正関数、又は補正テーブルとに基づき、プロセッサ２１が可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求める。

そして、プロセッサ２１又は操作者が、可動軸Ｊ１～Ｊ６のうちトルク検出感度Ｓ_ｉが優位である少なくとも１つの可動軸を、動作プログラム２３ｂに基づくワークの支持作業中にワークに追従させる可動軸として選択する。プロセッサ２１は、ワークからツールＴに加わる力に選択された可動軸を追従させる制御を行う。例えば、選択された可動軸のサーボモータで検出される検出電流値に基づき当該サーボモータを制御し、必要に応じて他のサーボモータも前記検出電流値に基づき制御する。

［実施例２］
上記実施形態は、ロボット１０の先端部のツールＴによってワークを把持する場合、ロボットの先端部のスポット溶接ガンであるツールＴによって溶接点を挟む場合等に使用できる。
例えば、スポット溶接ガンであるツールＴによってワークの溶接点を挟む時に、ロボット１０の各可動軸Ｊ１～Ｊ６が完全に固定されていると、ツールＴの挟む力によってワークに変形等が生じる恐れがある。このため、可動軸Ｊ１～Ｊ６のうち少なくとも１つの可動軸をツールＴ側からの力に追従できる状態にする。

追従する可動軸の選択の際、ツールＴで溶接点を挟む時のロボット１０の姿勢と、当該姿勢において溶接点からツールＴに加わる力の方向と、記憶部２３に格納されている補正係数、補正関数、又は補正テーブルとに基づき、プロセッサ２１が可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求める。そして、プロセッサ２１又は操作者が、可動軸Ｊ１～Ｊ６のうちトルク検出感度Ｓ_ｉが優位である少なくとも１つの可動軸を、ツールＴ側からの力に追従する可動軸として選択する。プロセッサ２１は、溶接点からツールＴに加わる力に選択された可動軸を追従させる制御を行う。例えば、選択された可動軸のサーボモータで検出される検出電流値に基づき当該サーボモータを制御し、必要に応じて他のサーボモータも前記検出電流値に基づき制御する。

なお、前述のようにプロセッサ２１が可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求めた後、プロセッサ２１がトルク検出感度Ｓ_ｉが優位である少なくとも１つの可動軸のトルク（モータ電流値）を監視し、溶接点、ワーク等とツールＴとの接触により監視トルクが所定値を超えると、サーボモータ１１ａ～１６ａの制御によって監視トルクが所定値以下となる方向にロボット１０のアーム１０ａの位置および姿勢を制御することも可能である。

なお、上記実施形態では、式（１）および式（２）において、各可動軸Ｊ１～Ｊ６の回転軸方向と外力Ｆの方向との関係を示す数式要素としてＰ_ｉ×Ｆ（外積）、ｃｏｓφ等を用いているが、各可動軸Ｊ１～Ｊ６の回転軸方向と外力Ｆの方向とが成す角度が大きくなる程小さくなる１次関数、２次関数等の数式要素を用いることも可能である。

上記実施形態では、大きさおよび方向がわかっているサンプル外力Ｆに基づいて補正係数又は補正関数が求められる。また、各可動軸Ｊ１～Ｊ６の回転軸方向と外力Ｆの方向との関係を示す数式要素に加え、補正係数又は補正関数を用いて、各可動軸Ｊ１～Ｊ６のトルク検出感度が算出される。このため、複数の可動軸Ｊ１～Ｊ６のうちアーム１０ａに加わる外力Ｆを感度よく検出できる可動軸の正確な選択が可能となる。

また、上記実施形態では、各可動軸Ｊ１～Ｊ６と外力Ｆの作用点との距離ｒ_ｉと、各可動軸Ｊ１～Ｊ６の回転軸方向と外力Ｆの方向との関係を示す数式要素と、補正係数又は補正関数とを少なくとも用いて、各可動軸Ｊ１～Ｊ６におけるトルク検出感度が演算される。
当該構成では、各可動軸Ｊ１～Ｊ６と外力Ｆの作用点との距離ｒ_ｉも用いて、各可動軸Ｊ１～Ｊ６のトルク検出感度が算出される。このため、複数の可動軸Ｊ１～Ｊ６のうちアーム１０ａに加わる外力Ｆを感度よく検出できる可動軸Ｊ１～Ｊ６の正確な判断が可能となる。

また、上記実施形態では、複数の可動軸Ｊ１～Ｊ６のうちトルク検出感度が優位である少なくとも１つが外力Ｆに追従する可動軸として決定される。
当該構成では、前述のように正確に算出されたトルク検出感度を用いて、複数の可動軸Ｊ１～Ｊ６のうち優位である少なくとも１つが外力Ｆに追従する可動軸として決定される。これは、ワークの動き、ツールの動作によって生ずる力等にロボット１０を追従させる制御を正確に行う上で有利である。

なお、プロセッサ２１が、記憶部２３に格納されている外力位置取得プログラム（外力位置取得手段）２３ｆに基づき、アーム１０ａにおいて外力が加わる位置の検出又は推定を行うことも可能である。この場合、プロセッサ２１が前述のように可動軸Ｊ１～Ｊ６の各々のトルク検出感度Ｓ_ｉを求めた後、トルク検出感度Ｓ_ｉが優位である少なくとも１つの可動軸の検出トルク（モータ電流値）を用いて、アーム１０ａにおいて外力が加わる位置の検出又は推定を行う。プロセッサ２１は各サーボモータ１１ａ，１２ａ，１３ａ，１４ａ，１５ａ，１６ａの作動位置検出装置に基づきアーム１０ａの位置および姿勢を認識している。一例では、認識しているアーム１０ａの位置および姿勢と、トルク検出感度Ｓ_ｉが優位である可動軸の検出トルク（モータ電流値）とを用いて、プロセッサ２１がアーム１０ａにおいて外力が加わる位置の検出又は推定を行う。その他、トルク検出感度Ｓ_ｉが優位である可動軸の検出トルク（モータ電流値）と公知の方法とを用いて外力が加わる位置の検出又は推定を行うことも可能である。

１０ ロボット
１０ａ アーム
１１～１６ アーム部材
１１ａ～１６ａ サーボモータ
１１ｂ～１６ｂ 減速機
２０ 制御装置
２１ プロセッサ
２２ 表示装置
２３ 記憶部
２３ａ システムプログラム
２３ｂ 動作プログラム
２３ｃ 補正係数取得プログラム
２３ｄ 感度演算プログラム（トルク感度演算手段）
２３ｅ 軸決定プログラム（軸決定手段）
２３ｆ 外力位置取得プログラム（外力位置取得手段）
２４ 入力装置
２５ 送受信部
２６ サーボ制御器
３０ 上位制御装置
３１ メモリ
Ｊ１～Ｊ６ 可動軸
Ｂ ベース
Ｔ ツール
Ｄ 力センサ

Claims (4)

1. ロボットのアームを構成する複数のアーム部材が複数の可動軸周りに動く多関節ロボットの制御装置であって、
   前記アームの先端部にサンプル外力を加えた時の前記各可動軸周りのトルク又はモータ電流値の検出値に基づいて求められた補正係数又は補正関数が格納されている記憶部と、
   前記アームの前記先端部に加わる外力の方向に応じて変化する前記各可動軸におけるトルク検出感度を、前記各可動軸の回転軸方向と前記外力の方向との関係を示す数式要素と、前記補正係数又は前記補正関数とに少なくとも基づき演算するトルク感度演算手段と、を備え、
   前記トルク検出感度は、前記各可動軸におけるモータの電流検出値又はトルクセンサの検出値の前記外力に対する感度を示すものである、多関節ロボットの制御装置。
2. 前記トルク感度演算手段が、前記各可動軸と前記外力の作用点との距離と、前記数式要素と、前記補正係数又は前記補正関数とを少なくとも用いて前記各可動軸における前記トルク検出感度を演算する、請求項１に記載の多関節ロボットの制御装置。
3. 前記複数の可動軸のうち前記トルク検出感度が優位である少なくとも１つを前記外力に追従する可動軸として選択する軸決定手段を備える、請求項１又は２に記載の多関節ロボットの制御装置。
4. 前記アームにおいて前記外力が加わる位置の検出又は推定を行う外力位置取得手段をさらに備え、
   前記外力位置取得手段は、前記複数の可動軸のうち前記トルク検出感度が優位である少なくとも１つの前記可動軸周りのトルク又はモータ電流値の検出値を、前記検出又は前記推定に用いる、請求項１～３の何れかに記載の多関節ロボットの制御装置。

Images