JP2012137421A - ３軸力センサを用いて力制御をおこなうロボットの制御装置 - Google Patents

Abstract

【課題】３軸力センサによっては検出できない力およびモーメントを推定する。
【解決手段】ツール（４）およびワーク（Ｗ）の一方に対して他方をロボット（１）の手先部によって相対的に移動させ、ツールとワークとの間に作用する力を制御するロボット制御装置（１１）は、１軸方向の力と、該１軸に直交で且つ互いに直交する２軸方向の軸回りのモーメントとを検出する力検出部（３）と、ツール（４）とワーク（Ｗ）との間に作用する力を推定するための力推定用点を設定する力推定用点設定部（１２）と、力検出部により検出した１軸方向の力および２軸方向の軸回りのモーメントと、力推定用点設定部により設定された力推定用点の位置とに基づいて、前記２軸方向の力またはさらに前記１軸回りのモーメントととを推定する力推定部（１３）と、を備える。
【選択図】図１

Description

本発明は、１軸方向の力と、その軸に直交で且つ互いに直交する２軸方向の軸回りのモーメントとからなる三つの成分を計測する３軸力センサを用いて力制御をおこなうロボットシステムのロボット制御装置に関する。

３軸力センサとして、１軸方向の力と、その軸に直交で且つ互いに直交する２軸方向の軸回りのモーメントとからなる三つの成分を計測することができるセンサは公知である。特許文献１に開示されるように、このような３軸力センサは小型で安価に生産でき、現在では広範に普及している。

ここで、３軸力センサ３により力を検出できる軸をＺ軸、その軸に直交で且つ互いに直交する２方向の軸をそれぞれＸ軸、Ｙ軸とする。さらに、これらＸ軸、Ｙ軸、Ｚ軸方向に作用する力をそれぞれＦｘ、Ｆｙ、Ｆｚと表すと共に、Ｘ軸、Ｙ軸、Ｚ軸回りに作用するモーメントをそれぞれＭｘ、Ｍｙ、Ｍｚと表す。

特許文献１は５軸力センサを開示しており、この５軸力センサは、力Ｆｚ、モーメントＭｘ、Ｍｙの三つの成分を検出する第一計測部と、力Ｆｘ、Ｆｙの二つの成分を検出する第二計測部とを備えている。このような５軸力センサは、小型かつ、複数の軸方向の並進力Ｆｘ、Ｆｙ、Ｆｚ並びにＸＹ各軸回りのモーメントＭｘ、Ｍｙを直接、感知できる。

特開２０１０−１１２８６４号公報

しかしながら、特許文献１に開示される５軸力センサは力Ｆｘ、Ｆｙを計測する第二計測部を備えているので、その生産およびキャリブレーションに手間と時間とを要していた。

また、力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する従来の３軸力センサは、これら三つの成分を用いた力制御をおこなうことしかできない。このため、四つ以上の成分が要求される複雑な制御に対応するのは困難であった。

本発明はこのような事情に鑑みてなされたものであり、３軸力センサを用いる場合であっても、３軸力センサによって検出できない力およびモーメントを推定して、６軸力センサを用いた場合と同様な力制御をおこなうことのできるロボットの制御装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、ツールおよびワークの一方に対して他方をロボットの手先部によって相対的に移動させ、前記ツールと前記ワークとの間に作用する力を制御するロボット制御装置において、１軸方向の力と、該１軸に直交で且つ互いに直交する２軸方向の軸回りのモーメントとを検出する力検出部と、前記ツールと前記ワークとの間に作用する力を推定するための力推定用点を設定する力推定用点設定部と、前記力検出部により検出した前記１軸方向の力および前記２軸方向の軸回りのモーメントと、前記力推定用点設定部により設定された前記力推定用点の位置とに基づいて、前記２軸方向の力またはさらに前記１軸回りのモーメントを推定する力推定部と、を備えたことを特徴とするロボット制御装置が提供される。

２番目の発明によれば、１番目の発明において、前記力推定用点設定部は、前記ツールと前記ワークとの間の相対位置関係と、前記力推定用点の位置を算出するための力推定用点算出基準点の位置と、前記ツールと前記ワークとの間の押付方向と、前記ツールの形状または前記ワークの形状の情報とから前記力推定用点の位置を算出して設定する。

３番目の発明によれば、２番目の発明において、前記力推定用点の位置を算出するための前記力推定用点算出基準点の位置は変更可能であり、変更された前記力推定用点算出基準点の位置に基づいて前記力推定用点の位置を算出して設定する。

４番目の発明によれば、１番目の発明において、前記力推定用点設定部は、前記ロボットの動作中に前記力推定用点の位置を変更可能とする。

５番目の発明によれば、１番目から４番目のいずれかの発明において、さらに、前記ツールの磨耗量を推定して前記力推定用点の位置を補正する力推定用点補正部を備える。

６番目の発明によれば、１番目、４番目または５番目のいずれかの発明において、前記力推定用点設定部は、前記力推定用点に力をかけることにより、前記１軸方向の力と、前記２軸方向の軸回りのモーメントとに基づいて前記力推定用点の位置を算出して設定する。

７番目の発明によれば、６番目の発明において、前記力推定用点設定部は、前記力推定用点に、既知の力をかけることによって、該力推定用点の位置を算出して設定する。

８番目の発明によれば、６番目の発明において、前記力推定用点設定部は、前記力推定用点が３つの軸のうちのいずれかの軸上に配置されている場合に、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点の位置を算出して設定するか、あるいは、前記力推定用点が力を計測できる軸と残りの２軸のうちの一方の１軸とで張られる平面上に配置されている場合に、その平面と平行でなく、かつ、力を計測できる軸以外の２軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記該力推定用点の位置を算出して設定する。

９番目の発明によれば、２番目、３番目、または５番目のいずれかの発明において、さらに、前記力推定用点算出基準点に力をかけることにより、前記１軸方向の力と前記２軸方向の軸回りのモーメントとに基づいて、前記力推定用点算出基準点の位置を算出して設定する力推定用点算出基準点設定部を備える。

１０番目の発明によれば、９番目の発明において、前記力推定用点算出基準点設定部は、前記力推定用点算出基準点に、既知の力をかけることによって、該力推定用点算出基準点の位置を算出し設定する。

１１番目の発明によれば、９番目の発明において、前記力推定用点算出基準点設定部は、前記力推定用点算出基準点が３つの軸のうちのいずれかの軸上に配置されている場合に、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点算出基準点の位置を算出して設定するか、あるいは、前記力推定用点算出基準点が力を計測できる軸と残りの２軸のうちの一方の軸とで張られる平面上に配置されている場合に、その平面と平行でなく、かつ、力を計測できる軸以外の２軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点算出基準点の位置を算出して設定する。

本発明によって、力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する３軸力センサを用いて、力Ｆｘ、Ｆｙおよび／またはモーメントＭｚを精度よく推定することができる。
さらに、力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する３軸力センサを用いて力Ｆｘ、Ｆｙ、および／またはモーメントＭｚを推定するときには、力の作用点であるツールとワークとの間の接触点をより正確に推定する必要がある。３軸力センサによる力制御を実用的にするために、上記の手法に基づいて、接触点を簡易に、また、より正確に推定、設定する。これにより、３軸力センサを用いて、力Ｆｘ、Ｆｙおよび／またはモーメントＭｚを実用的に、より精度よく推定し、力Ｆｘ、Ｆｙ、ＦｚおよびモーメントＭｘ、Ｍｙ、Ｍｚを力制御に利用することができる。

本発明は、様々な方向への力制御をおこなうロボットシステムにおいて、力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する３軸力センサを適用している。この３軸力センサは同じ定格の４軸以上を検出できる多軸力センサと比べて安価かつ小型化が可能であるので、そのような多軸のセンサを前記３軸力センサに置き換え、力制御に利用することによって、ロボットシステム全体の価格を抑えると共に、ロボットシステムをより小型に作成することができる。

本発明に基づくロボット制御装置を含むロボットシステムの斜視図である。 （ａ）３軸力センサが検出する成分を示す図である。（ｂ）力推定用点を示す図である。 本発明に基づくロボット制御装置の動作を示すフローチャートである。 （ａ）ワークに接触するツールの略側面図である。（ｂ）ワークに接触するツールの他の略側面図である。（ｃ）ワークに接触するツールの略頂面図である。 （ａ）ワークに接触するツールの第一の略側面図である。（ｂ）ワークに接触するツールの第二の略側面図である。（ｃ）ワークに接触するツールの第三の略側面図である。（ｄ）ワークに接触するツールの第四の略側面図である。 （ａ）ロボットの先端部分の第一の拡大図である。（ｂ）ロボットの先端部分の第二の拡大図である。 （ａ）ワークの形状を示す第一の図である。（ｂ）ワークの形状を示す第二の図である。（ｃ）ワークの形状を示す第三の図である。（ｄ）ワークの形状を示す第四の図である。 ロボットの先端部分の別の拡大図である。 （ａ）ワークの頂面図である。（ｂ）ワークの他の頂面図である。 （ａ）別のワークの頂面図である。（ｂ）別のワークの他の頂面図である。 （ａ）ワークに接触するツールの他の略側面図である。（ｂ）ワークに接触するツールの略頂面図である。 （ａ）ワークに接触するツールの第一の略側面図である。（ｂ）ワークに接触するツールの第二の略側面図である。（ｃ）ワークに接触するツールの第三の略側面図である。（ｄ）ワークに接触するツールの第四の略側面図である。 （ａ）ワークに接触するツールの他の略側面図である。（ｂ）ワークに接触するツールの略頂面図である。 ロボットの先端部分のさらに別の拡大図である。 ３軸力センサ座標系を示す図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボット制御装置を含むロボットシステムの斜視図である。図１から分かるように、以下においては、力を検出できる軸をＺ軸、その軸に直交で且つ互いに直交する２方向の軸をそれぞれＸ軸、Ｙ軸とする。さらに、これらＸ軸、Ｙ軸、Ｚ軸の方向に作用する力をそれぞれＦｘ、Ｆｙ、Ｆｚと表すと共に、Ｘ軸、Ｙ軸、Ｚ軸回りに作用するモーメントをそれぞれＭｘ、Ｍｙ、Ｍｚと表す。

図１に示されるロボットシステム１０は、ロボット１と制御装置１１とを含んでいる。ロボット１は、６軸構成の垂直多関節型ロボットであるが、６自由度の多関節型ロボット以外のタイプのロボットでもよい。ロボット１のロボットアーム２の先端には、ワークＷを加工するツール４が取付けられている。なお、便宜上、ツール４は下向きの円錐形としているが、倣い作業や加工作業などに用いる他の形状のツール４を採用してもよい。

図示されるように、ロボットアーム２とツール４との間には、３軸力センサ３（以下、単に「力センサ」と称する場合がある）が配置されている。力センサ３はツール４に作用する力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する。また、図１に示されるように、ワークＷは作業台Ｂ上に載置されている。

なお、３軸力センサ３により検出される力ＦおよびモーメントＭは、３軸力センサ３に取付けられた或る物体に対して、他の物体が接触することにより作用する力、および／または３軸力センサ３に取付けられた物体の重心に作用する重力や慣性力（コリオリ力、ジャイロ効果を含む）などの影響により生じる。

図１に示されるように、ロボット１および力センサ３はロボット制御装置１１に接続されている。ロボット制御装置１１はデジタルコンピュータであり、ロボットシステム１０におけるロボット１の各軸の位置を制御する。また、力センサ３が検出する力およびモーメントデータは、ロボット制御装置１１に入力される。図示されるように、ロボット制御装置１１は、ツール４とワークＷとの間に作用する力を推定するための力推定用点を設定する力推定用点設定部１２を含む。ここで、力推定用点は、３軸力センサ３に外力が作用する場合に、前述した重力や慣性力などの影響以外に、別の物体が３軸力センサ３に接触することによって作用する力が、代表的な或る１点で作用すると仮定したときの点である。

さらに、ロボット制御装置１１は、力センサ３により検出された力ＦｚおよびモーメントＭｘ、Ｍｙと、力推定用点設定部１２により推定された力推定用点の位置とに基づいて力Ｆｘ、ＦｙまたはさらにモーメントＭｚを推定する力推定部１３を含む。力Ｆｘ、Ｆｙの一方で十分な場合はそれだけを推定してもよい。

さらに、ロボット制御装置１１は、ツール４の磨耗量を推定して力推定用点の位置を補正する力推定用点補正部１４と、力推定用点の位置を算出するための力推定用点算出基準点の位置を算出して設定する力推定用点算出基準点設定部１５を含んでいる。また、ロボット１の教示データや、ツールやワークの質量や重心データなど、本発明の実現に必要なデータなどは記憶部１６に格納されるものとする。

以下、図１等を参照しつつ、ロボット１に取付けたツール４をワークＷに倣わせる場合について説明する。ただし、ロボットアーム２の先端部分に取付けたツール４をワークＷに対して相対的に移動させる場合、およびロボットアーム２の先端部分に取付けたハンド（図１には示さない）により把持されたワークＷを、固定位置に在る加工ツールなどに対して相対的にハンドを移動させる場合であっても、本発明の範囲に含まれる。また、力センサ３は必ずしもロボット１に取付けられている必要はなく、作業台Ｂに固定されていてもよい。また、作業台Ｂが、他のロボットなどの移動可能な装置であり、それによって設置したワークやツールを移動させる場合であってもよい。

言い換えれば、本発明は、力センサ３がロボット１側に取付けられていて、ロボット１がツール４を備えるか、またはワークＷを把持する構成であってよい。さらに、本発明は、力センサ３が、作業台Ｂなどに取付けられるかまたは固定されていて、ロボット１がツール４を備えるかまたはワークを把持する構成であってもよい。また、作業台Ｂが移動可能な装置であってもよい。なお、以下の説明において、数式における”・”はベクトルの内積、”×”は外積、”＊”は乗算を表すものとする。

図２（ａ）は３軸力センサが検出する成分を示す図である。図２（ａ）に示されるように、本発明の１番目の実施形態においては、３軸力センサ３により検出される力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を用いて力制御をおこなう。図２（ｂ）は、力推定用点を示す図である。本発明においては、力ＦｚおよびモーメントＭｘ、Ｍｙと、３軸力センサに対する力推定用点Ｐｃの位置（図２（ｂ）に示される）とに基づいて、力Ｆｘ、Ｆｙ、および／または、モーメントＭｚ成分を推定する。そして、それらの値を基に、必要に応じて重力や慣性力などを補償し、力推定用点にかかる正味の力を推定し、ロボットで力制御をおこなう。

このため、１番目の実施形態においては、力ＦｚおよびモーメントＭｘ、Ｍｙの３成分を検出可能な３軸力センサ３を用いつつ、６方向の力およびモーメントを検出可能な６軸力センサなどと同様に、任意の方向の力制御をおこなうことが可能となる。並進方向の力制御しかおこなわない場合は、力Ｆｘ、Ｆｙの推定のみでもよい。また、力、モーメントのすべてを推定する必要がないとき、必要な成分のみを推定してもよい。

力ＦｚおよびモーメントＭｘ、Ｍｙの三つの成分を検出する３軸力センサ３は、通常、同じ定格の４軸以上を検出できる多軸力センサと比べて、構造が単純で組み立ても簡単なため、生産時の手間および時間などを抑え、安価にすることができる。従って、同じ定格の４軸以上を検出できる多軸のセンサを前記３軸力センサに置き換え、力制御に利用することによって、ロボットシステム全体の価格を抑えることができる。さらに、３軸力センサ３は小型化が可能であるので、従来の６軸方向を検出できる力センサを用いる場合などと比べて、ロボットシステムをより小型に作成することも可能である。

図２（ｂ）に示されるように、３軸力センサ３に対して固定された３軸力センサの座標系を、力センサ座標系と呼ぶことにする。そして、力センサ座標系から見たときの力推定用点Ｐｃの位置をＲｅ（ｒｘ、ｒｙ、ｒｚ）と設定する。

図３は本発明に基づくロボット制御装置の動作を示すフローチャートである。はじめに、ステップＳ１０で、３軸力センサ３が力ＦｚおよびモーメントＭｘ、Ｍｙを検出する。次いで、以下の要領で、ステップＳ２０において、力Ｆｘ、ＦｙまたはさらにモーメントＭｚを算出する。

３軸力センサ３により検出される力ＦｚおよびモーメントＭｘ、Ｍｙと、力推定用点Ｐｃ、および、力推定用点Ｐｃに作用していると推定される力Ｆｘ、Ｆｙの関係は、重力やロボットの手先部の動作による影響を考えない場合、以下の式（１）、（２）により表される。
Ｍｘ＝Ｆｚ＊ｒｙ−Ｆｙ＊ｒｚ （１）
Ｍｙ＝−Ｆｚ＊ｒｘ＋Ｆｘ＊ｒｚ （２）

従って、力Ｆｘ、Ｆｙは、以下の式（３）、（４）により表される。
Ｆｙ＝（−Ｍｘ＋Ｆｚ＊ｒｙ）／ｒｚ （３）
Ｆｘ＝（Ｍｙ＋Ｆｚ＊ｒｘ）／ｒｚ （４）
そして、求められた力Ｆｘ、Ｆｙにより、モーメントＭｚは以下の式（５）のように推定される。これらの計算は、力推定部１３によりおこなわれるものとする。
Ｍｚ＝Ｆｙ＊ｒｘ−Ｆｘ＊ｒｙ （５）

なお、ロボットの手先に３軸力センサ３を取付け、３軸力センサ３にツール４を取付けて、または、ワークＷを把持させて、ロボットの手先部を移動させる場合には、３軸力センサ３により検出される力には、ロボットアーム先端のエンドエフェクタがロボットアームと共に運動することにより発生する遠心力、コリオリ力といった慣性力などの動力学項も含まれている。以下では、重力や動力学項などの影響を考慮した力Ｆｘ、ＦｙおよびモーメントＭｚを力推定部１３が推定することについて説明する。

そのような重力および動力学項は、公知の方法、例えば特許第４２６７０２７号に開示される手法により算出される。具体的には、マニピュレータの動力学方程式をニュートン・オイラー法やラグランジュ法などを用いて解いて、ロボットアームの関節角度ベクトルθ、該関節角度ベクトルθの速度および加速度から、３軸力センサの座標系から見た、重力と共に、３軸力センサに取付けたツールの重心に作用する慣性力が３軸力センサの座標系中心に作用する力を求める。これにより、３軸力センサの座標系から見た、３軸力センサに作用する重力および動力学項を、力推定部１３が算出することができる。

上記のように求めた重力や動力学項などの影響を考慮した力Ｆｘ、ＦｙおよびモーメントＭｚを力推定部１３が推定する具体的な方法を以下で述べる。ロボットの手先の３軸力センサ３に取付けたツール４またはワークＷが、重力およびロボットの手先部の移動動作によって及ぼす３軸力センサ座標系から見た力ベクトルをＦｍとする。さらに、検出されるモーメントＭｘ、Ｍｙと推定されたモーメントＭｚからなるベクトルをＭｍ（Ｍｘ、Ｍｙ、Ｍｚ）と表す。さらに、推定される力Ｆｘ、Ｆｙと、検出される力ＦｚからなるベクトルＦｒ（Ｆｘ、Ｆｙ、Ｆｚ）とする。そして、ロボットの手先の３軸力センサ３に取付けたツール４またはワークＷの重心の、力センサ座標系での位置をＣｇ（ｃｘ、ｃｙ、ｃｚ）とする。位置Ｃｇは予め求めておく。

このような場合には、以下の式（６）で示される関係が得られる。
Ｍｍ＝Ｒｅ×Ｆｒ＋Ｃｇ×Ｆｍ （６）
前述したように、Ｒｅは、力推定用点Ｐｃの位置である。この式（６）を用いて、力推定部１３は、重力や動力学項などの影響を考慮した力Ｆｘ、ＦｙおよびモーメントＭｚを算出できる。

なお、力推定用点Ｐｃの位置Ｒｅは、力推定用点設定部１２によって設定される。力推定用点Ｐｃの位置Ｒｅは、ロボットの基準座標系上で与えられた位置を力センサ座標系上に表した位置、または、ロボットの手先部に対して与えられた位置を力センサ座標系上に表した位置などである。あるいは、これらそれぞれの位置により表される位置Ｒｅが固定、または、可変であってもよい。

また、力推定用点設定部１２は、想定される接触点付近の位置、またはＴＣＰ（ツール先端点）などの位置を力推定用点Ｐｃの位置Ｒｅとして設定してもよい。あるいは、予め実測した位置、または動作中に実測した位置を力推定用点Ｐｃの位置Ｒｅとして設定してもよい。

このような力推定用点Ｐｃの位置Ｒｅを設定するために、既知の位置になるようにロボットに対して教示動作などを設定してもよい。また、ロボットの動作中に所定間隔で画像を取得できるカメラなどを用いて取得した画像から、力推定用点Ｐｃの位置Ｒｅを設定してもよい。あるいは、接触部分が分かる接触センサを移動させ、動作中の接触位置を予め取得し、そのデータを利用して、力推定用点設定部１２が力推定用点Ｐｃの位置Ｒｅを設定してもよい。

上記方法によって、ロボットの手先に３軸力センサ３を取付け、さらにワークＷを把持するハンド６またはツール４を取付けて、ロボットの手先部を移動させる場合にも、力Ｆｘ、Ｆｙ、およびモーメントＭｚを精度よく推定することができる。

本発明の２番目から５番目の実施形態においては、力Ｆｘ、ＦｙおよびモーメントＭｚの推定精度を高めるために、３軸力センサ３に対する力推定用点Ｐｃの位置Ｒｅをより正確に推定するようにしている。

本発明においては、３軸力センサ３により検出できない方向の力やモーメントを力推定用点Ｐｃを用いて推定している。従って、力推定用点Ｐｃをより正確に設定することが望まれる。それゆえ、力Ｆｘ、ＦｙおよびモーメントＭｚを正確に推定する際に、以下の方法を用いることは、とても有効である。

力推定用点Ｐｃは、別の物体が３軸力センサ３に取付けたツールまたはワークに接触することによって作用する力が、代表的な或る１点で作用すると仮定したときの点である。従って、力推定用点Ｐｃをより正確に設定することによって、力Ｆｘ、ＦｙおよびモーメントＭｚをより正確に推定できる。

２番目の実施形態においては、ツール４とワークＷとの接触点が変化し、３軸力センサ３に対する接触点の位置が変化する場合に、基準となる点（力推定用点算出基準点）を用いて、幾何学的な関係に基づいて力推定用点Ｐｃの位置Ｒｅを算出する。

具体的には、２番目の実施形態においては、ツール４とワークＷの相対位置関係と、３軸力センサ３に対する力推定用点Ｐｃの位置を算出するために用いる「力推定用点算出基準点」と、押付方向と、ツール４またはワークＷの形状の情報を用いて、３軸力センサ３に対する力推定用点Ｐｃの位置を算出している。

以下、このことについて説明する。力推定用点算出基準点Ｐｓは、力推定用点Ｐｃを算出するために適切な任意の点であり、力推定用点算出基準点設定部１５により設定される。力推定用点算出基準点Ｐｓは、３軸力センサ３に対する接触点の位置が変化する場合、その中心に位置するような点、または算出するのに都合のよい点に設定することが望ましい。また、力推定用点算出基準点Ｐｓを力推定用点Ｐｃの最初の位置と同じ位置に設定し、その点に対して補正をかけるようにしてもよい。

図４（ａ）および図４（ｂ）はワークに接触するツールの略側面図であり、図４（ｃ）はワークに接触するツールの略頂面図である。これら図面を参照しつつ、力推定用点Ｐｃの或る算出方法を説明する。この場合には、力推定用点算出基準点Ｐｓは円錐形のツール４の中心線上の一部、例えばツール４の先端に設定される。

図４（ａ）および図４（ｃ）に示されるように、ツール４が円形のワークＷの周囲を倣っている。つまり、図４（ａ）および図４（ｃ）においては、ツール４の位置と、矢印で示される押付方向とを変えると共に、ツール４上の接触位置を変えつつ、ツール４を移動させている。このような場合には、力推定用点Ｐｃを実際の接触点により近い位置に推定することによって、力Ｆｘ、ＦｙおよびモーメントＭｚをより正確に推定することができる。

ここで、力推定用点Ｐｃの位置をＲｅ、力推定用点算出基準点Ｐｓの位置をＲｂ、力推定用点算出基準点Ｐｓから力推定用点ＰｃへのベクトルをＲｒとする。ロボット１の制御周期毎に、幾何的な関係から、ベクトルＲｒを求める。そして、以下の式（７）により、力推定用点設定部１２は力推定用点Ｐｃの位置Ｒｅを求める。
Ｒｅ＝Ｒｂ＋Ｒｒ （７）

なお、図４（ｂ）に示されるように、ツール４がワークＷに対して傾斜していて、ツール４上の接触位置を変えて動かす場合にも、上記と同様に、力推定用点算出基準点Ｐｓと、力推定用点算出基準点Ｐｓと力推定用点Ｐｃとの位置関係に基づいた計算から、力Ｆｘ、Ｆｙ、およびモーメントＭｚをより正確に推定できる。

また、図５（ａ）から図５（ｄ）はワークに接触するツールの略側面図である。この場合には、図５（ａ）および図５（ｂ）から分かるように、ツール４とワークＷの相対位置関係は上方に向かって変化している。同様に、図５（ｃ）および図５（ｄ）に示されるように、ツール４がワークＷに対して傾斜している場合においても、ツール４とワークＷの相対位置関係は上方に向かって変化している。このようにツール４とワークＷの相対位置関係が変化する場合であっても、式（７）を用いることにより、力Ｆｘ、Ｆｙ、モーメントＭｚの値をより正確に推定することできる。

さらに、図６（ａ）および図６（ｂ）はロボットの先端部分の拡大図である。これら図面においては、ロボットの手先部に３軸力センサ３が取付けられている。そして、３軸力センサ３よりも先端にハンド６が取付けられており、ハンド６がワークＷを把持している。一方、ツール４は別の場所に固定されている。

図６（ａ）においては、このような状態で、ワークＷの加工箇所を変えながら、加工がおこなわれる。図６（ａ）に示されるワークＷの断面は円形である。このように、ワークＷが比較的単純な形状である場合には、加工がおこなわれる場所（力推定用点Ｐｃとほぼ同じ）と、力推定用点算出基準点Ｐｓ（この場合は、ワークＷの中心）との間の距離が一定である。このような場合には、前述したのと同様な手法により力Ｆｘ、Ｆｙ、モーメントＭｚの値をより正確に推定できることが分かるであろう。

これに対し、図６（ｂ）に示されるワークＷの断面は矩形であり、加工がおこなわれる場所（力推定用点Ｐｃとほぼ同じ）と、力推定用点算出基準点Ｐｓ（ワークＷの中心）との間の距離が一定ではない。図７（ａ）から図７（ｄ）は、把持されるワークＷの形状が比較的複雑で、力推定用点算出基準点Ｐｓと加工作業をおこなう部分との距離がロボットの動作時に一定でなく、かつ、加工作業をおこなう部分が、力推定用点算出基準点Ｐｓから押付方向に存在するとした場合の例である。

このような場合には、ワークＷの加工する部分の形状情報を予め取得しておき、ハンド６で把持したときの掴み位置と姿勢から、３軸力センサ３の力センサ座標系に対するワークＷの加工部分の形状情報を算出する。そして、力推定用点算出基準点Ｐｓの位置（例えばワークＷの中心）と、押付方向と、３軸力センサに対するワークＷの加工作業をおこなう部分の形状情報とに基づいて、力推定用点Ｐｃを算出できる。

また、図８はロボットの先端部分の別の拡大図である。図８に示されるように、ロボット１側に３軸力センサ３を設け、固定のツール４に対して、ハンド６により把持されるワークＷを作用させる場合には、以下の方法により、３軸力センサ３に対する、力推定用点Ｐｃの位置Ｒｅを算出することができる。

はじめに、固定されたツール４の外周部上に、力推定用点設定部１２により力推定用点Ｐｃを設定する。図８に示されるように、力推定用点Ｐｃの位置はロボット１の動作中に変化しうる。また、力推定用点算出基準点Ｐｓはツール４の中心に位置している。
次いで、ツール４とワークＷとが可能な限り力推定用点Ｐｃにて接触するように、ロボット１の動作を教示する。そのようなロボットの動作の教示パラメータとしては、ロボット１の軌道や速度、押付方向、目標押付力などが挙げられる。

前記ロボット１が把持したワークを固定ツールに接触させる場合に、図９（ａ）および図１０（ａ）では、ツール４に対する力推定用点Ｐｃの位置が変化しない場合、図９（ｂ）および図１０（ｂ）では、ツール４に対する力推定用点Ｐｃの位置が変化し、押付方向も変化させている場合を示す。このように、複雑な形状を有するワークであって、ロボット１が移動することによって、３軸力センサ３に対する、力推定用点Ｐｃの位置が動的に複雑に変化する場合であっても、ロボットの基準座標系に対する力推定用点Ｐｃの位置を基にすることによって、ワークＷの加工する部分の形状情報を必要とせず、３軸力センサ３に対する力推定用点Ｐｃの位置を簡易に求めることができる。

そして、ロボット１の動作を教示するときに、力推定用点算出基準点Ｐｓを用いる場合には、力推定用点算出基準点Ｐｓや、力推定用点Ｐｃと力推定用点算出基準点Ｐｓとの相対位置（方向と距離など）を設定する（２番目または３番目の実施形態）。また、ロボット１の動作を教示するときに、力推定用点算出基準点Ｐｓを使用せず、力推定用点Ｐｃを直接的に用いる場合は、力推定用点Ｐｃの位置を直接、動的に、設定・更新する（４番目の実施形態）。これによって、ロボット１の動作と、ロボット１に対する力推定用点Ｐｃとが対応付けられる。

ロボット１が移動しているとき、ロボットの位置・姿勢に基づいて、力推定部１３は、３軸力センサ３に対する、力推定用点Ｐｃの位置を算出する。ここで、ロボット１の基準座標系と３軸力センサ３の力センサ座標系との位置・姿勢関係を表す行列をＴｒｆ、ロボットの基準座標系と力推定用点Ｐｃとの位置・姿勢関係を表す行列をＴｒｅとする。これらにより、３軸力センサ３に対する力推定用点Ｐｃの位置は、ＴｒｆとＴｒｅとの関係から求められる。これは当業者には自明である。

この場合、力推定用点Ｐｃと作業台Ｂなどに固定したツール４との位置関係は比較的簡単に表せるが、力推定用点Ｐｃと３軸力センサ３との位置関係はワークＷの形状やロボット１の手先部の姿勢によって複雑に、動的に変化する。このとき、ツール４上の力推定用点Ｐｃとロボット１との位置関係に基づいて、３軸力センサ３に対する力推定用点Ｐｃの位置を随時更新することにより、力推定用点Ｐｃの位置を動的に変えることができる。

このような手法を用いる場合には、ワークＷの形状が複雑で、かつ、３軸力センサ３に対する力推定用点Ｐｃの位置が動的に複雑に変化する場合であっても、力推定用点Ｐｃを簡易に算出することが可能となる。また、力推定用点算出基準点Ｐｓを用いることによって、力推定用点Ｐｃの位置を相対的な位置関係で表すことができ、従って、力推定用点Ｐｃの位置をより簡易に表すこともできる。

このように、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを、ツール４またはハンド６などのロボット１の手先部に設定するか、または、ロボット１の基準座標系に対して設定する。そして、力推定用点Ｐｃが幾何的な関係によって求まるように教示することにより、３軸力センサ３で検出できない力およびモーメント、例えば力Ｆｘ、ＦｙおよびモーメントＦｚを精度よく算出することができる。

３番目の実施形態においては、力推定用点Ｐを求めるときに用いる基準点を、動作中などにおいても、適宜変えられるようにするのが好ましい。すなわち、力推定用点補正部１４が３軸力センサ３に対する力推定用点算出基準点Ｐｓの位置を、教示操作盤やキーボードなどからの入力またはロボット１の動作プログラム、ロボット１の動作に応じた切り替えなどの所定の設定に基づいて変化させる。これにより、異なるツール４へ切り替えた場合、ツール４上での接触箇所を変えた場合であっても、接触推定点の位置のずれの補正をおこなうことができる。

図１１（ａ）はワークに接触するツールの他の略側面図であり、図１１（ｂ）はワークに接触するツールの略頂面図である。これら図１１（ａ）および図１１（ｂ）から分かるように、ロボットの動作時にツール４の位置と押付方向とを変え、ツール４上の接触位置を変えて動かしている。このような場合には、３軸力センサ３に対する、力推定用点Ｐｃの位置も変化させる必要がある。さらに、図１１（ａ）から分かるように、ツール４のワークＷに対する高さが変化すると、力推定用点Ｐｃを変える必要がある（ＰｃａからＰｃｂ）。

図１１（ａ）に示されるように、ツール４の下方位置における、力推定用点算出基準点Ｐｓａのツール軸方向の位置の、ツール周上で、ワークに接触させている場合には、力推定用点算出基準点Ｐｓａとその部分でのツールの半径Ｒａに基づいて力推定用点Ｐｃａを算出する。また、ツール４の上方位置における、力推定用点算出基準点Ｐｓｂのツール軸方向の位置の、ツール周上で、ワークに接触させている場合には、力推定用点算出基準点Ｐｓｂとその部分でのツールの半径Ｒｂに基づいて力推定用点Ｐｃｂを算出する。このように、異なる位置の力推定用点算出基準点を使用することによって、より正確な接触点を推定することができる。そして、力推定用点Ｐｃを実際の接触点により近い位置に推定することによって、より正確な力を推定することが可能となる。

図１２（ａ）から図１２（ｄ）はワークに接触するツールの略側面図である。これら図面においては、ワークＷ上の位置に応じて、ツール４上の接触箇所を変えている。このような場合でも、前記図１１に示した場合の方法と同様である。つまり、ワークに接触させている部分がツール４の下方位置に在るか（図１２（ａ）および図１２（ｃ））または、ワークに接触させている部分がツール４の上方位置に在るか（図１２（ｂ）および図１２（ｄ））に応じて、異なる位置の力推定用点算出基準点を使用して、力推定用点Ｐｃを推定する。このように力推定用点算出基準点Ｐｓの値を適宜、変更することによって、押付方向と力推定用点Ｐｃまでの距離情報から、より正確な接触点を推定することができる。

図１３（ａ）はワークに接触するツールの他の略側面図であり、図１３（ｂ）はワークに接触するツールの略頂面図である。図１３（ａ）および図１３（ｂ）に示される例においては、ツール４の位置と押付方向を変え、ツール４上の接触位置を変えて動作させている。

４番目の実施形態においては、このような場合において、力推定用点Ｐｃを、ロボット１の動作中などにおいても、適宜変えられるようにしている。つまり、３軸力センサ３に対する、ツール４とワークＷの接触箇所が変わるときに、３軸力センサ３に対する力推定用点Ｐｃの位置を更新することによって、適切に設定することができる。また、力推定用点Ｐｃを実際の接触点により近い位置に推定することによって、より正確な力を推定することができる。

このような変更手法としては、予め用意した設定値への切替え、教示操作盤やキーボードなどからの入力またはロボット１の動作プログラム、ロボット１の動作に応じた切替え、ツール４とワークＷの位置関係に応じた、力推定用点Ｐｃを用意する、押付方向と対応する力推定用点Ｐｃの位置を予め用意し、押付方向に応じて変更する、進行方向と対応する力推定用点Ｐｃの位置を予め用意し、進行方向に応じて変更する等が挙げられる。

図１４はロボットの先端部分のさらに別の拡大図である。図１４に示されるように、ロボット１側に３軸力センサ３を設け、固定されたツール４に対して、ロボットのハンド６が把持するワークＷを作用させる。このような場合に、固定されたツール４に接触させるためにツール４に設定した目標接触点を力推定用点Ｐｃとすると、ロボット１が動くにつれて、３軸力センサ３に対する力推定用点Ｐｃの位置は動的に変化する。

そのような場合に、ツール４上の力推定用点Ｐｃとロボット１との位置関係に基づいて、３軸力センサ３に対する力推定用点Ｐｃの位置を随時更新することにより、力推定用点Ｐｃの位置を動的に変えることができる。また、固定されたツール４上での接触させる箇所を変えるときには、力推定用点Ｐｃを更新すればよい。このような計算は、前記したように、位置および姿勢を表す変換行列を用いた計算から容易に算出することができる。

５番目の実施形態においては、ツール４の磨耗量の推定に基づいて、力推定用点補正部１４が３軸力センサ３に対する力推定用点Ｐｃを補正するのが好ましい。力推定用点Ｐｃを実際の接触点により近い位置に推定することによって、より正確な力を推定することができる。３軸力センサ３において、磨耗量の推定に基づいて、力Ｆｘ、ＦｙおよびモーメントＭｚをより正確に求める。

摩耗量を推定する手法については、例えば、実験をおこなって、データテーブルを予め作成しておくことが望ましい。そのような、データテーブルは、ツール４とワークＷの材質、ツール４にかかる力、ツール４とワークＷとの相対速度、ツール４の回転数、実行時間などとツール４の磨耗量との関係を表すものとする。そして、予め作成されたデータテーブルを記憶部１６から読込んで、力推定用点補正部１４が、前述した材質、力等に対応したツール４の磨耗量を推定する。あるいは、実験等に基づいて磨耗量算出のための関数を予め作成し、そのような関数に基づいて、摩耗量を推定してもよい。その後、推定された摩耗量に基づいて、力推定用点補正部１４が力推定用点Ｐｃを補正する。

６番目、７番目、９番目、１０番目の実施形態においては、既知の力に基づいて、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを推定するのが好ましい。そして、このときに３軸力センサ３により検出される、１軸方向の力Ｆｚと、その軸に直交、かつ、互いに直交する２方向の軸回りのモーメントＭｘ、Ｍｙに基づいて、力推定用点設定部１２または力推定用点算出基準点設定部１５が力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを算出して、設定する。既知の力を作用させる場合、押し込み量によって押付力が分かるばね装置などを用いるか、または、力が計測可能な、固定した計測装置に押し付けるか、または、力が計測可能な装置をその部分に押し付けるなどの方法が考えられる。それによって既知の力が作用しているときに、教示操作盤などからそのタイミングを知らせ、ロボットの姿勢から重力の影響を補償した力およびモーメント情報を取得して、指定された、または、計測された力と共に、上記の計算に適用する。また、どの程度の既知の力が望ましいかは、実験等により予め求めておいてもよい。

このように、ロボットシステム１０では、既知の力を作用させることによって、簡易に、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを設定することができる。具体的な方法を次に説明する。

図１５は３軸力センサ座標系を示す図である。図１５において、３軸力センサ座標系における点Ｐ１において力Ｆが作用するときには、以下の関係式が成り立つ。
Ｍｘ＝ｂ＊Ｆｚ−ｃ＊Ｆｙ 式（８）
Ｍｙ＝−ａ＊Ｆｚ＋ｃ＊Ｆｘ 式（９）
この場合には、モーメントＭｚは検出不可能であるものの、力ＦｚおよびモーメントＭｘ、Ｍｙは検出可能である。

これら式（８）および式（９）により、位置を求めたい点に対して、既知の力（例えば、Ｆｚは任意で、（Ｆｘ、Ｆｙ）＝（１、０）、（０、１）などのように異なる値）を、作用する方向や大きさが異なるように複数回、作用させることによって、Ｐ１（ａ、ｂ、ｃ）を算出することができる。

６番目、８番目、９番目および１１番目の実施形態においては、適当な未知の力に基づいて、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを推定することができる。つまり、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓに、適当な未知の大きさの力を作用させ、そのときの、３軸力センサ３により検出される、１軸方向の力Ｆｚと、その軸に直交、かつ、互いに直交する２方向の軸回りのモーメントＭｘ、Ｍｙとに基づいて、重力の影響を補償し、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを算出し、設定する。適当な未知の大きさの力を作用させるときには、力を作用させたタイミングを、教示操作盤などを用いて設定する、または、ある閾値以上の力が作用したときなどの条件によって設定し、そのときの力およびモーメント、ロボットの姿勢などを取得することが望ましい。

このように、ロボットシステム１０では、適当な力を作用させることによって、簡易に、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓを設定することができる。

図１５を参照して説明したように、３軸力センサ座標系における点Ｐ１において力Ｆが作用するときには、式（８）および式（９）が成り立つ。従って、前述したようにＰ１（ａ、ｂ、ｃ）を算出することができる。

これら式（８）および式（９）によって、位置を求めたい点が、３つの軸（Ｘ、Ｙ、Ｚ軸）のうちのどれかの軸上に配置されている場合に、その点に対して、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、適当な大きさの力Ｆを作用させることによって、Ｆｚの値と力の作用する方向の関係から、力Ｆｘ、Ｆｙの値を求めて、力の作用点である、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓの位置Ｐ１（ａ、ｂ、ｃ）を算出できる。

この場合には、Ｐ１がＺ軸上に存在すると、ａ＝０、ｂ＝０方向が既知であるので、力Ｆｚの値から、力Ｆｘ、Ｆｙが分かる。従って、ｃを求めることができる。
また、位置を求めたい点に対して、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、方向が異なるように、複数回、適当な大きさの力を作用させることによって、Ｆｚの値と力の作用する方向の関係から、力Ｆｘ、Ｆｙの値を求めて、Ｐ１（ａ、ｂ、ｃ）を算出できる。

同様にして、位置を求めたい点が、力を計測できる軸とそれ以外の１つの軸で張られる平面上に配置されている場合に、その点に対して、その平面と平行でない、かつ、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、適当な大きさの力Ｆを作用させることによって、Ｆｚの値と力の作用する方向の関係から、力Ｆｘ、Ｆｙの値を求めて、力の作用点である、力推定用点Ｐｃまたは力推定用点算出基準点Ｐｓの位置Ｐ１（ａ、ｂ、ｃ）を算出できる。

この場合には、Ｐ１がＸ−Ｚ平面上に存在すると、ａ＝０方向が既知なので、Ｆｚの値から、力Ｆｘ、Ｆｙが分かる。この条件では、Ｆｘ≠０である。従って、ｂ、ｃを求めることができる。このようにして、本発明においては、力推定用点設定部１６が力推定用点Ｐｃを簡易に設定できたり、力推定用点算出基準点設定部１５が力推定用点算出基準点Ｐｓを簡易に設定できたりすることが分かるであろう。

１ ロボット
２ ロボットアーム
３ ３軸力センサ（力検出部）
４ ツール
６ ハンド
１０ ロボットシステム
１１ ロボット制御装置
１２ 力推定用点設定部
１３ 力推定部
１４ 力推定用点補正部
１５ 力推定用点算出基準点設定部

Claims (11)

1. ツールおよびワークの一方に対して他方をロボットの手先部によって相対的に移動させ、前記ツールと前記ワークとの間に作用する力を制御するロボット制御装置において、
   １軸方向の力と、該１軸に直交で且つ互いに直交する２軸方向の軸回りのモーメントとを検出する力検出部と、
   前記ツールと前記ワークとの間に作用する力を推定するための力推定用点を設定する力推定用点設定部と、
   前記力検出部により検出した前記１軸方向の力および前記２軸方向の軸回りのモーメントと、前記力推定用点設定部により設定された前記力推定用点の位置とに基づいて、前記２軸方向の力またはさらに前記１軸回りのモーメントを推定する力推定部と、を備えたことを特徴とするロボット制御装置。
2. 前記力推定用点設定部は、前記ツールと前記ワークとの間の相対位置関係と、前記力推定用点の位置を算出するための力推定用点算出基準点の位置と、前記ツールと前記ワークとの間の押付方向と、前記ツールの形状または前記ワークの形状の情報とから前記力推定用点の位置を算出して設定することを特徴とする請求項１に記載のロボット制御装置。
3. 前記力推定用点の位置を算出するための前記力推定用点算出基準点の位置は変更可能であり、
   変更された前記力推定用点算出基準点の位置に基づいて前記力推定用点の位置を算出して設定することを特徴とする請求項２に記載のロボット制御装置。
4. 前記力推定用点設定部は、前記ロボットの動作中に前記力推定用点の位置を変更可能とすることを特徴とする請求項１に記載のロボット制御装置。
5. さらに、前記ツールの磨耗量を推定して前記力推定用点の位置を補正する力推定用点補正部を備えることを特徴とする請求項１〜４のいずれか１項に記載のロボット制御装置。
6. 前記力推定用点設定部は、前記力推定用点に力をかけることにより、前記１軸方向の力と、前記２軸方向の軸回りのモーメントとに基づいて前記力推定用点の位置を算出して設定することを特徴とする請求項１、４、５のいずれか１項に記載のロボット制御装置。
7. 前記力推定用点設定部は、前記力推定用点に、既知の力をかけることによって、該力推定用点の位置を算出して設定することを特徴とする請求項６に記載のロボット制御装置。
8. 前記力推定用点設定部は、
   前記力推定用点が３つの軸のうちのいずれかの軸上に配置されている場合に、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点の位置を算出して設定するか、あるいは、
   前記力推定用点が力を計測できる軸と残りの２軸のうちの一方の１軸とで張られる平面上に配置されている場合に、その平面と平行でなく、かつ、力を計測できる軸以外の２軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記該力推定用点の位置を算出して設定する、ことを特徴とする請求項６に記載のロボット制御装置。
9. さらに、前記力推定用点算出基準点に力をかけることにより、前記１軸方向の力と前記２軸方向の軸回りのモーメントとに基づいて、前記力推定用点算出基準点の位置を算出して設定する力推定用点算出基準点設定部を備えることを特徴とする請求項２、３、５のうちのいずれか１項に記載に記載のロボット制御装置。
10. 前記力推定用点算出基準点設定部は、
    前記力推定用点算出基準点に、既知の力をかけることによって、該力推定用点算出基準点の位置を算出し設定することを特徴とする請求項９のロボット制御装置。
11. 前記力推定用点算出基準点設定部は、
    前記力推定用点算出基準点が３つの軸のうちのいずれかの軸上に配置されている場合に、力を計測できる軸以外の２つの軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点算出基準点の位置を算出して設定するか、あるいは、
    前記力推定用点算出基準点が力を計測できる軸と残りの２軸のうちの一方の軸とで張られる平面上に配置されている場合に、その平面と平行でなく、かつ、力を計測できる軸以外の２軸で張られる平面と平行でない既知の方向に、任意の力をかけることによって、前記力推定用点算出基準点の位置を算出して設定する、ことを特徴とする請求項９のロボット制御装置。

Images