JP6223640B2 - キャリブレーション装置およびこれを用いたロボットシステム - Google Patents

Description

この発明は、例えば力制御を行うロボットにおいて、ロボット手先の姿勢や動作によらず、外的作用により生じた外力のみを抽出するキャリブレーション装置およびこれを用いたロボットシステムに関するものである。

キャリブレーション装置は、自動加工装置や自動組立装置、ロボットのような機械装置に対するキャリブレーション操作を実施するものであり、備え付けられた作業対象に作用する手先効果器等、質量を持った作用装置に係る手先負荷の重力補償を実施するものである。

例えば、手先負荷とロボットアームとの間にロードセルや力覚センサを備えたロボットにおいて、重力補償や慣性力の補償を目的として、作業前に手先負荷の重量および重心位置を計測または推定し、この値に基づいて手先負荷の姿勢や加減速動作に応じた重力または慣性力を演算し、力センサの値から差し引くことで、外的作用により生じた外力を抽出している。

ここで、手先負荷以外の外力が作用しない条件下で、ロボットの手先フランジまたはロボット手先の位置姿勢を計測するとともに、その位置姿勢において、力覚センサで３軸力および３モーメントからなる６自由度の力情報を計測し、手先負荷によりある姿勢から別の姿勢に変化する際に作用する力センサ出力の差から、最小二乗法を利用してオフセット電圧を推定するキャリブレーション装置が提案されている（例えば、特許文献１参照）。

また、据え付け位置の不備による重力方向の誤差を考慮して、複数姿勢で得られる力情報から所定の計算を実施することで、重力方向ベクトルや手先負荷重量、手先負荷の重心位置ベクトル、力覚センサのバイアス値等、外力を抽出する際に差し引くべき値を最小二乗法によって求めるキャリブレーション装置が提案されている（例えば、特許文献２参照）。

特開平７−２０５０７５号公報 特開２０１２−４０６３４号公報

特許文献１、２に示されるように、キャリブレーション装置において、センサ取り付け時に生じているオフセット出力やバイアス出力を精度よく取り除くことは、手先負荷の重量や重心位置を精度よく推定する上で必要なことである。

ここで、特許文献１、２に示されたキャリブレーション装置では、取得するデータには、余分な外力が作用しないことが仮定されている。しかしながら、実際に用いられるロボットにおいて、力覚センサの先に取り付けられた手先負荷の周辺には、ツール部分に取り付けられた通信ケーブルや信号線といった配線が外力作用として含まれることが多く、正確なキャリブレーションをすることができなない。

特に、任意の複数姿勢で取得したデータから、最小二乗法等を用いて各パラメータを算出する方法を用いる場合、ケーブル等に起因する外力による成分を含むデータとそうでないデータとが混在するため、特に手先負荷が小さい場合には、バイアス値成分を正確に推定することができない。その結果、手先負荷質量や重心位置ベクトルの推定が正確でなく、高精度にキャリブレーションを実施することができないという問題がある。

この発明は、上記のような課題を解決するためになされたものであり、外力による影響を考慮してバイアス成分を除去し、手先負荷質量および重心位置ベクトルを高精度に算出することで、高精度にキャリブレーションを実施することができるキャリブレーション装置を得ることを目的とする。

この発明に係るキャリブレーション装置は、先端に取り付けられ、作業対象に対して作用するツール部分の力制御を行う機械装置において、前記作業対象との接触により、前記ツール部分に生じた外力のみを抽出するキャリブレーション装置であって、前記ツール部分の位置情報を取得する位置情報取得部と、前記ツール部分に作用する力情報を取得する力情報取得部と、前記力情報を取得するセンサのセンサ座標系の原点を通過する任意の回転軸を指定する回転軸指定部と、前記回転軸周りに前記ツール部分を回転させる姿勢指令値を生成する姿勢生成部と、前記力情報からバイアス値および手先負荷の重力作用分を差し引いて、前記作業対象との接触により生じた外力のみを抽出するキャリブレーション処理を実施するキャリブレーション部と、を備え、前記キャリブレーション部は、前記姿勢指令値に応じて前記ツール部分を回転させた際の前記位置情報および前記力情報に基づいて、近似曲線を生成する近似曲線生成部と、前記近似曲線、前記位置情報および前記力情報に基づいて、前記力情報のバイアス値を推定するバイアス値推定部と、前記力情報から前記バイアス値を除去し、前記バイアス値が除去された前記力情報を用いて、前記手先負荷の質量および重心位置ベクトルを算出する質量・重心位置推定部と、推定された前記バイアス値、前記手先負荷の質量および重心位置ベクトルを用いて、前記力情報から前記バイアス値および前記手先負荷の重力作用分を差し引く外力成分演算部と、を有するものである。

この発明に係るキャリブレーション装置によれば、先端に取り付けられ、作業対象に対して作用するツール部分の力制御を行う機械装置において、前記作業対象との接触により、前記ツール部分に生じた外力のみを抽出するキャリブレーション装置であって、前記ツール部分の位置情報を取得する位置情報取得部と、前記ツール部分に作用する力情報を取得する力情報取得部と、前記力情報を取得するセンサのセンサ座標系の原点を通過する任意の回転軸を指定する回転軸指定部と、前記回転軸周りに前記ツール部分を回転させる姿勢指令値を生成する姿勢生成部と、前記力情報からバイアス値および手先負荷の重力作用分を差し引いて、前記作業対象との接触により生じた外力のみを抽出するキャリブレーション処理を実施するキャリブレーション部と、を備え、前記キャリブレーション部は、前記姿勢指令値に応じて前記ツール部分を回転させた際の前記位置情報および前記力情報に基づいて、近似曲線を生成する近似曲線生成部と、前記近似曲線、前記位置情報および前記力情報に基づいて、前記力情報のバイアス値を推定するバイアス値推定部と、前記力情報から前記バイアス値を除去し、前記バイアス値が除去された前記力情報を用いて、前記手先負荷の質量および重心位置ベクトルを算出する質量・重心位置推定部と、推定された前記バイアス値、前記手先負荷の質量および重心位置ベクトルを用いて、前記力情報から前記バイアス値および前記手先負荷の重力作用分を差し引く外力成分演算部と、を有している。
そのため、外力による影響を考慮してバイアス成分を除去し、手先負荷質量および重心位置ベクトルを高精度に算出することで、高精度にキャリブレーションを実施することができる。

この発明の実施の形態１に係るキャリブレーション装置が適用されたロボットシステムを示す構成図である。 この発明の実施の形態１に係るキャリブレーション装置を示すブロック構成図である。 この発明の実施の形態１に係るロボットシステムのロボット座標系、ワールド座標系、重力座標系、センサ座標系の位置関係を例示する説明図である。 この発明の実施の形態１に係るロボットシステムのメカニカルフランジ座標系とセンサ座標系との関係を例示する説明図である。 この発明の実施の形態１に係るキャリブレーション装置のキャリブレーション部およびこれを用いたロボットシステムを示すブロック構成図である。 この発明の実施の形態１に係るキャリブレーション装置のキャリブレーション部内のパラメータ推定部を詳細に示すブロック構成図である。 （ａ）、（ｂ）は、この発明の実施の形態１に係るキャリブレーション装置において、指定軸上の姿勢生成部により生成される姿勢指令値による動作を例示する説明図である。 この発明の実施の形態１に係るキャリブレーション装置において、パラメータ推定部のバイアス値推定部による軸力のバイアス推定動作を示す説明図である。 この発明の実施の形態１に係るキャリブレーション装置において、パラメータ推定部のバイアス値推定部によるモーメントのバイアス推定動作を示す説明図である。 この発明の実施の形態２に係るキャリブレーション装置において、パラメータ推定部のバイアス値推定部が、バイアス推定結果について所定の誤差レベルか否かを判断する処理を示すフローチャートである。 （ａ）、（ｂ）は、この発明の実施の形態２に係るキャリブレーション装置において、パラメータ推定部のバイアス値推定部が、バイアス推定結果について所定の誤差レベルか否かを判断する場合を例示する説明図である。 この発明の実施の形態２に係るキャリブレーション装置において、キャリブレーション部に入力する前に誤差情報表示部によってユーザに表示し、エラーデータを選択する場合を例示する説明図である。 この発明の実施の形態３に係るキャリブレーション装置において、キャリブレーション部でツール部に関する質量の影響を考慮する他に、作業開始時の接触状態を保持するオフセット保持の具体例を示す説明図である。 この発明の実施の形態３に係るキャリブレーション装置のキャリブレーション部を示すブロック構成図である。

以下、この発明に係るキャリブレーション装置およびこれを用いたロボットシステムの好適な実施の形態につき図面を用いて説明するが、各図において同一、または相当する部分については、同一符号を付して説明する。

なお、以下の各実施の形態では、キャリブレーション装置として、具体的にロボットを利用したシステムにおいてキャリブレーション処理を行うロボットシステムを例に挙げて説明する。しかしながら、この発明に係るキャリブレーション装置は、ロボットを活用しない場合であっても、力制御を行う機械装置については、同じ構成でキャリブレーションを実施可能なので、適用範囲をロボットシステムに限定するものではない。

具体的には、キャリブレーション装置としては、自動加工装置、自動組立装置、ロボットのような自動加工または自動組立を実施する機械装置であって、力制御を行う機械装置についてキャリブレーションを実施するものが挙げられる。

また、この発明に係るキャリブレーション装置は、センサ座標系の原点を通過する任意軸周りに姿勢変化を生じた際のロボットの手先負荷位置情報と、力センサの出力情報である力情報とから近似曲線を生成する。また、近似曲線、位置情報および力情報から、キャリブレーション処理に不要なケーブルやバネ等に起因する張力や斥力による外力成分を除去してバイアス値を推定し、力覚センサデータからバイアス値を除去した後、手先負荷質量および重心位置ベクトルを推定し、これらの情報に基づいて手先負荷の重力補償を行い、手先負荷に作用する外力成分を演算するものである。

これにより、不要な外力成分を有するデータを除去した抽出データからバイアス値を推定した後、手先負荷質量および重心位置ベクトルを推定することで、取得データの中から手先負荷質量および重心位置ベクトルの推定処理に用いられる複数の入力情報に含まれる外力の影響を除去でき、高精度に重力補償および外力成分推定を行うことができる。

実施の形態１．
図１は、この発明の実施の形態１に係るキャリブレーション装置が適用されたロボットシステムを示す構成図である。ここでは、まず、キャリブレーション装置の処理内容について説明する。図１において、ロボットシステムは、ロボットアーム１、ロボットのコントローラ２およびロボットアーム１に取り付けられたツール部４を基本的な構成として有している。

また、力制御を行うロボットシステムでは、ロボットアーム１とツール部４との間に、力情報を取得する力情報センサとして力センサ３が設けられている。通常、ロボットは、任意の座標系上で、ロボットアーム１の先端位置またはロボットアーム１の先端に取り付けられたツール部４の先端位置を位置制御で制御し、コントローラ２で指定された所望の位置に移動する。

なお、力センサ３を用いた力制御を導入すると、位置制御だけでなく、インピーダンス制御や力制御が実現可能となる。また、インピーダンス制御や力制御については後述するが、受動的または能動的に接触状態、すなわちツール部４と周辺環境や周辺物体との間の力の作用状態を制御する制御方式である。

この制御方式を活用することで、一般にロボットを用いた研磨作業、バリ取り作業、コネクタ等の組立作業、テーパを有する軸と穴との嵌めあい作業等、接触状態を考慮する必要がある作業を実施できるようになる。

キャリブレーション装置は、このようなロボットシステムにおいて、力情報を用いてロボット等の機械装置を制御する場合に必要な装置である。また、ロボットが作業するにあたり、ロボットアーム１の先端に、作業に用いるためのハンドや工具、センサが取り付けられ作業を実施するが、これらをツール部分と呼ぶ。このとき、力制御を活用した作業を実施するには、手先側のツール部分と作業対象とが、どのような接触状態にあるのかを正確に知る必要がある。ここで、接触状態とは、力およびモーメントの大きさや方向ベクトルで表現できる。

ロボット等の機械装置は、力情報を用いて作業をする場合、例えばバリ取りや研磨といった加工作業や、接触状態を有する組立作業といった作業を実施することができるようになるが、このとき、ロボット等の機械装置が正しく作業するためには、接触状態を目的状態に保ちながら作業をすることが望まれる。ここで、目的状態とは、加工や作業のためにあらかじめ設計された接触状態のことであり、作業目的に合わせて作業速度や必要な精度と合わせて作業者が調整する。

ところで、力情報は、力覚センサ・ロードセルといったセンサから電気信号として取得している。また、力情報を取得するセンサは、ロボットの手先側のフランジ位置に取り付けられる。なお、センサは、構造体の歪みを利用して電気信号を出力するため、センサ構造体をロボットに取り付けたときに生ずる構造体の歪みや、衝突等で生じた構造体の歪みや、センサ自身の電気回路のゼロ点設定によっては、バイアスを生じることがある。

ここで、ゼロ点設定とは、生じているバイアス成分を相殺するように、出力値に対してオフセット量を加えて調整する設定であり、通常ロボット動作前に行う。また、バイアスとは、センサに出力される定常的なオフセット量のことであり、センサ出力のみを観測すると、姿勢によらず一定量の大きさの外力が作用しているように見える。すなわち、ツールに対して作業対象による作用力を生じない無負荷状態の場合でも、一定の作用力を生じているように計測される。そのため、バイアス成分は、除去されなければならない。

上述したように、力情報を用いたロボット等の機械装置の制御においては、インピーダンス制御や力制御が行われる。なお、インピーダンス制御は、位置指令値を与えて制御する場合に、外力が作用した際のツール部先端の挙動について仮想的な剛性や粘性を設定し、作業対象とツール先端との位置関係を制御する制御方式である。一方、力制御は、力目標値を与えて、力目標値に追従するように制御する制御方式である。

このとき、それぞれの制御方式で、力や位置関係を目標状態として適切に指令値設定またはインピーダンス・パラメータ設定することで、組立搬送やバリ取り研磨のような作業に適用することができる。ここで、指令値とは、力制御の力目標値や、インピーダンス制御の位置制御の位置姿勢目標値、または位置指令実行中の手先速度指令値のことを指す。また、インピーダンス・パラメータとは、例えば、一般のインピーダンス制御におけるスティフネス行列、ダンピング行列および慣性行列における各要素値のことを表す。

なお、これらの制御を実施する場合、力情報にバイアスを生じていると、外部から作用する実際の作用力とは異なったベクトル量の力情報が制御系に入力される。そのため、ロボットツール部に関するインピーダンス制御または力制御が、実際の作用力とは違う力に基づいて行われるため、ユーザが設計していた目的の位置および状態に到達しないか、または、ロボットツール部が、コントローラで指定する目標作用力に追従できなくなる。そこで、通常は、バイアスの影響を低減する操作を行うことが多い。

ここで、上記特許文献１の例えば段落００３１に記載されているように、特定の姿勢において取得される力情報をバイアス量として出力値から相殺する方法では、垂直多関節型のロボット等、姿勢変更により重力方向およびツール先端の向きが変更可能な場合には、重力影響分の補償量が変化するため、演算される外力の大きさが不正確になる。

また、重力や手先加速度による慣性力の影響を除去し、正確な外力を求めるために、ツール部分、すなわちセンサから見た手先負荷について質量および重心位置を正確に同定し、同時にバイアス量を推定することにより、力情報からバイアス量および手先負荷の重力作用分をあらかじめ差し引くことで、実際の外力を正確に計算することができる。

また、上記特許文献１、２の方法では、何れもＮ（Ｎ＝１，２，・・・）個の姿勢において、それぞれ力情報を取得してバイアス量推定、およびその他のパラメータとして、ツール質量、ツール重心位置、重力方向等の推定を行うが、何れの方法も、取得データから外力成分が理想的に除去できている状態で位置情報および力情報が取得できていることを前提としている。

また、作用力の誤差としての外力成分は、ローパスフィルタや移動平均フィルタで示されるフィルタ処理で影響を低減可能なホワイトノイズを想定しており、特定の方向に依存して生じる誤差データは考慮されていない。

しかしながら、実際には、ロボットシステムおよび機械システムには、図１に示すように、目的に応じてツール先端に取り付けられるケーブルとして、力センサケーブル５、触覚センサやビジョンセンサのセンサケーブル６、エアケーブル７といったケーブル類が設けられ、さらに、図示しない電源線といった配線が設けられているため、ケーブル類や配線の作用で外力を生じる。また、これらのケーブルや配線の取り付け方によっては、センサノイズ以上の外力成分が力情報に含まれてしまうことがある。

その結果、このような情報に基づいて上記特許文献１、２の方法を適用すると、それぞれのデータがどの程度影響を受けているのかを判断することが難しく、例えば最小二乗法等を適用した解が、誤差の影響を受けるという課題がある。なお、この影響は、高精度な制御を行う上では無視できない。

そこで、この発明は、上記の課題を解決するために、位置情報および力情報を取得する姿勢として、センサ座標系の任意の軸周りに回転する姿勢変更を実施したデータを用いることで、ケーブルや配線による外力作用分の顕著なものを除去し、一定の傾向があるものについては、補正を行うことで、高精度にバイアスを推定し、バイアス推定処理後の力情報を入力として用いることで、高精度にツール部分の質量、すなわち手先負荷の質量および重心位置を推定することができるものである。

以下、図２を参照しながら、この発明の実施の形態１に係るキャリブレーション装置のキャリブレーション手順について詳しく説明する。図２は、この発明の実施の形態１に係るキャリブレーション装置を示すブロック構成図である。図２に示したキャリブレーション装置は、力センサの情報から、作業に関係するロボットの手先部分やツール先端に作用する外力作用分のみを抽出するために、力センサの情報に対してキャリブレーション処理を実施するものである。

ロボットがある姿勢Ｒ_k（ｋ＝１，２，・・・，Ｍ）をとるとき、姿勢Ｒ_kにおける力センサ情報をＦ（ｋ）とする。ここで、姿勢とは、回転行列Ｒ_kで表現している。また、回転行列とは、ある基準座標系からみて、現在注目している座標系がどういった姿勢変化をしているかを、３×３行列で表現したものである。

力センサ情報Ｆ（ｋ）には、バイアスＦ＿ｂｉｓ（ｋ）、手先負荷分の作用力Ｆ＿ｍａｓ（ｋ）、外的な作用力Ｆ＿ｅｘｔ（ｋ）および電気信号としてのノイズ成分Ｆ＿ｎｏｓ（ｋ）があり、次式（１）で表される。

Ｆ（ｋ）＝Ｆ＿ｂｉｓ（ｋ）＋Ｆ＿ｍａｓ（ｋ）
＋Ｆ＿ｅｘｔ（ｋ）＋Ｆ＿ｎｏｓ（ｋ） ・・・（１）

また、力センサ情報Ｆ（ｋ）からＦ＿ｂｉｓ（ｋ）、Ｆ＿ｍａｓ（ｋ）、Ｆ＿ｎｏｓ（ｋ）を除去することで、求めたい外力成分Ｆ＿ｅｘｔ（ｋ）が求められる。外力成分Ｆ＿ｅｘｔ（ｋ）は、式（１）を変形して、次式（２）で表される。

Ｆ＿ｅｘｔ（ｋ）＝Ｆ（ｋ）−Ｆ＿ｂｉｓ（ｋ）
−Ｆ＿ｍａｓ（ｋ）−Ｆ＿ｎｏｓ（ｋ） ・・・（２）

ここで、外力成分Ｆ＿ｅｘｔ（ｋ）を演算する方法としては、ローパス等のフィルタ処理でノイズ成分Ｆ＿ｎｏｓ（ｋ）を除去し、バイアス値およびツール部の負荷推定に基づく手先負荷影響を演算して、これらを差し引くことで求められる。なお、バイアス成分は、固定されたときに作用している一定の力、具体的には軸力およびモーメントとし、ツールの姿勢に依存しない。

すなわち、Ｆ＿ｂｉｓ（ｋ）は、姿勢ｋに依存せず、すべての姿勢ｋに対して共通の値、Ｆ＿ｂｉｓ（ｋ）＝［Ｆ＿ｂｉｓ＿ｘ，Ｆ＿ｂｉｓ＿ｙ，Ｆ＿ｂｉｓ＿ｚ，Ｍ＿ｂｉｓ＿ｘ，Ｍ＿ｂｉｓ＿ｙ，Ｍ＿ｂｉｓ＿ｚ］となる。また、Ｆ＿ｍａｓ（ｋ）については、質量とセンサ座標系と重心位置との関係性を求めることで演算可能となる。

一般には、外力をかけない状態で複数姿勢Ｒ_kをとり、このときの位置情報と力情報とに基づいて、キャリブレーション処理を行う。この発明では、キャリブレーション時に取り扱うデータが、Ｆ＿ｅｘｔ（ｋ）が０にならない力情報を含めて取り扱う場合について、取得姿勢に対して拘束条件を与えることで高精度なキャリブレーション処理を実施し、これに基づいて外力成分による作用力Ｆ＿ｅｘｔ（ｋ）を計算する。この方法について以下に説明する。

図３は、この発明の実施の形態１に係るロボットシステムのロボット座標系、ワールド座標系、重力座標系、センサ座標系の位置関係を例示する説明図である。図３に示されるように、システム内に固定されたロボット本体の基準座標系として定義されるロボット座標系をΣｒｏｂ、同一システム内の機器に共通の座標系として定義されるワールド座標系をΣｗｌｄ、重力加速度方向を−Ｚ方向とした重力座標系をΣｇｒｖとおく。

通常、水準器を用いてロボットを据え付ける台の上面の法線方向、すなわちロボット座標系ΣｒｏｂのＺ方向と、重力座標系ΣｇｒｖのＺ軸方向である重力方向とが直交するように高精度にレベリングされることが多いため、自動化システムでは、ロボット座標系Σｒｏｂおよび重力座標系Σｇｒｖについては、Ｚ軸がほぼ一致し、誤差の影響は少ないとする。

また、重力座標系の定義としてΣｇｒｖのＸ軸方向およびＹ軸方向は、簡単のためロボット座標系Σｒｏｂと一致させるものとする。また、設計図に基づいてロボットシステムを構築する場合、図面に記載された精度で位置姿勢関係は既知としてよいため、ワールド座標系Σｗｌｄおよびロボット座標系Σｒｏｂも、相対関係としての同次変換行列^wldＴ_robが既知であるとしてよい。すなわち、Σｗｌｄ、Σｒｏｂ、Σｇｒｖの相対関係の初期値は、およそ推定されるものとして説明を進める。

ここで、ロボットアームの末端で、センサやツールを取り付けることができる部分のことをロボットメカニカルフランジと称する。ロボットメカニカルフランジの座標系は、ロボット座標系からみた手先位置指令値で演算することができる。

次に、ロボットメカニカルフランジにセンサを取り付ける場合に、センサ座標系Σｓｅｎを定義する方法について説明する。この発明では、出力する情報が３軸または６軸の場合を想定しているが、センサ座標系Σｓｅｎは、筐体に対してあらかじめＸ軸、Ｙ軸、Ｚ軸方向が定義されているので、これに従うものとする。

図４は、この発明の実施の形態１に係るロボットシステムのメカニカルフランジ座標系とセンサ座標系との関係を例示する説明図である。図４に示されるように、ロボット座標系に対するメカニカルフランジ座標系をΣｍｅｃとすると、ここで定義されているメカニカルフランジの座標系とセンサ座標系Σｓｅｎとを比較することで、同次変換行列を求めることができる。なお、厳密には誤差を含むが、初期値は分かるものして取り扱うことができる。

ここで、同次変換行列とは、回転行列Ｒ（３×３）および参照座標系で定義された位置関係を示す位置ベクトルＰからなる４×４の行列のことである。例えば、基準となる座標系をワールド座標系Σｗｌｄにとり、注目する座標系をロボット座標系Σｒｏｂとして同次変換行列^wldＴ_robを表現すると、回転行列Ｒ、位置ベクトルＰおよび同次変換行列^wldＴ_robは、次式（３）〜（５）で表される。

上述した位置関係が成り立っている場合において、さらにバイアスを正確に推定することができ、ツール部分の質量および重心位置が分かれば、外力成分Ｆ＿ｅｘｔ（ｋ）を演算することができる。これらが正しく演算できないのは、各座標系の位置関係が不正確で重力方向および重力加速度にずれがある場合、バイアス成分が不正確な場合、またはツール部分の質量や重心位置が不正確な場合が存在することによる。

これに対して、この発明では、これらを最適化計算で誤差を最小化するパラメータを算出するのではなく、固有の動作を行うことで誤差要因となるデータの除去および個別の推定処理を実施することで、バイアス、物体質量および重心位置の推定精度を向上させることを特徴とする。

そこで、まず、ロボットのキャリブレーション処理に必要な位置情報および力情報を取得する。位置情報としてツール部分の位置情報を取得するが、位置情報とは、直交座標系で表現されたＸ軸、Ｙ軸、Ｚ軸に対する並進３自由度である［Ｘ、Ｙ、Ｚ］およびオイラー表現されたＸ軸、Ｙ軸、Ｚ軸に対する回転３自由度である［Ａ、Ｂ、Ｃ］のことを指す。このとき、キャリブレーション用の位置情報および力情報を取得する目的で、図２の回転軸指定部１００により、センサ座標系の原点を通過する任意の回転軸Ｖｅｃ＿ｒｏｔを指定する。

ここで、センサ座標系の定義を基準としたキャリブレーションでは、回転軸指定部１００で指定された回転軸Ｖｅｃ＿ｒｏｔ周りに回転した姿勢で位置情報および力情報を取得する。そのため、図２の指定軸上の姿勢生成部１０１は、回転軸Ｖｅｃ＿ｒｏｔに基づいて、位置情報および力情報を取得するための姿勢を決定し、姿勢指令値として出力する。

なお、回転軸Ｖｅｃ＿ｒｏｔに対し、基準となる位置姿勢から回転量を大きくとる方が誤差は少なくなるので、各姿勢は、例えば回転軸Ｖｅｃ＿ｒｏｔの正回転方向に対して４５度間隔で取得するように決定することができる。一方、後述するように、この発明では、回転軸Ｖｅｃ＿ｒｏｔに対する回転量が小さくても推定処理ができるため、周辺物体との干渉等の理由で、姿勢変化に対する制約が大きい場合にも情報取得が可能である。

また、回転軸Ｖｅｃ＿ｒｏｔについては、センサ座標系の主軸であるセンサ座標系のＸ軸、Ｙ軸、Ｚ軸を指定してもよい。なお、後述する近似曲線生成部では、指定する回転軸に直交する面上に直交する主軸を定義する必要があるため、あらかじめセンサ座標系の主軸を指定すると、変換処理が不要となる。一方で、主軸以外の軸を回転軸として指定できるため、ユーザが周辺環境との干渉を考慮して決定することもできるし、メーカーがあらかじめ回転軸Ｖｅｃ＿ｒｏｔおよび回転角度量θを決定しておくこともできる。

このように指定された回転軸Ｖｅｃ＿ｒｏｔに対して、姿勢指令値に基づいて姿勢変更を行い、キャリブレーションに用いるデータを取得する。ここでは、ロボット１０２に取り付けられた力情報取得部１０３として、例えばロードセルや力センサを用いて、現在のロボット状態から力情報を取得する。

また、位置情報取得部１０４は、例えばロボットの各軸に取り付けられたエンコーダからの情報を使ってロボットの手先位置を演算し、さらにセンサ座標系の位置姿勢を取得する位置情報を取得する。なお、位置情報取得部１０４は、ロボットツール部分にマーカーを張り付け、ビジョンセンサでマーカーを計測することで、ロボット外部からロボット姿勢を計測または推定することもできる。

ここで、外部センサを用いる場合には、ロボット固有の機構誤差であるバックラッシやたわみ等の誤差の影響を考慮しなくてもよい。なお、外部センサを取り付ける場合は、センサ取り付け位置がロボット座標系Σｒｏｂまたはワールド座標系Σｗｌｄである基準座標系から見てどういった位置に据え付けられているのか、ジグを用いて正確に位置決めするか、外部センサが基準マーカー等を参照して据え付け位置を推定する必要がある。

このようにしてロボットツール先端を回転させるキャリブレーションのための位置情報および力情報を取得し、最小でも回転軸Ｖｅｃ＿ｒｏｔ１つに対して、３つ以上の点を取得する必要がある。これは、このキャリブレーション装置が円を用いた近似を適用するためである。なお、基本的には４つ以上の点で誤差の影響を考慮することが望ましい。

このようにして、回転軸Ｖｅｃ＿ｒｏｔ周りに姿勢を変化させながら位置情報および力情報を取得し、データ記憶部１０５に記憶していく。続いて、記憶したデータを利用してキャリブレーション部１０６においてキャリブレーション処理を行う。

図５は、この発明の実施の形態１に係るキャリブレーション装置のキャリブレーション部およびこれを用いたロボットシステムを示すブロック構成図である。キャリブレーション装置の使用にあたっては、まず、パラメータを求めるパラメータ推定部２０１において、パラメータ推定処理を行う。次に、実際にロボットを使って作業を実施するとき（通常動作時）に、推定されたパラメータを用いて、力情報からバイアス値および重力影響分を差し引く処理を行う。以下、これについて説明する。

図５において、このキャリブレーション装置では、パラメータ推定部２０１において、パラメータであるツール部分を含めた手先負荷の質量、ツール部分を含めた手先負荷の重心位置、およびバイアスを求める。以上のパラメータ推定部に関する説明は、図６を用いて後述する。

次に、通常動作時のロボット１０２は、指令値生成部２０３によって生成される姿勢指令値により動作し、現在のロボット状態に応じて、力情報取得部１０３で力情報が取得される。このとき、外力成分演算部２０７は、パラメータ記憶部２０２に記憶されたパラメータに基づいて、位置情報取得部１０４で得られるロボットの現在位置姿勢Ｒ_kにおける手先負荷質量による力の作用分Ｆ＿ｍａｓ（ｋ）を演算する。

また、外力成分演算部２０７は、上記式（２）に示したように、センサデータＦ（ｋ）から手先負荷質量による力の作用分Ｆ＿ｍａｓ（ｋ）およびバイアスによる作用分Ｆ＿ｂｉｓを差し引くことで、接触による外力成分Ｆ＿ｅｘｔ（ｋ）を求める。また、外力成分演算部２０７は、この外力成分を、指令値生成部２０３に作用外力演算値としてフィードバックすることで、キャリブレーション処理を行う。

以上のように、キャリブレーション部１０６は、ロボット１０２に対して、データ記憶部１０５に記憶された位置情報および力情報に基づいて、パラメータを求めるパラメータ推定処理を行うパラメータ推定部２０１と、パラメータ推定部２０１における推定結果を記憶するパラメータ記憶部２０２と、記憶されたパラメータを用いて、バイアスによる作用分および手先負荷質量による力の作用分を除いて、接触による外力成分を求める外力成分演算部２０７とから構成されている。この発明は、キャリブレーション部１０６を、指令値生成部２０３、ロボット１０２、力情報取得部１０３および位置情報取得部１０４から構成されるロボットシステムに適用するものである。

以下、さらにこの発明の特徴となっているパラメータ推定部２０１について詳細に説明する。図６は、この発明の実施の形態１に係るキャリブレーション装置のキャリブレーション部内のパラメータ推定部を詳細に示すブロック構成図である。

図６に示されるように、回転軸指定部１００で指定された回転軸Ｖｅｃ＿ｒｏｔ周りに回転させた姿勢でデータを複数取得し、取得したデータをデータ記憶部１０５に格納し、パラメータ推定部２０１でまずバイアス値および仮の質量を求める。また、パラメータ推定部２０１は、さらに求められたバイアス値および仮の質量に基づいて、質量および重心位置の推定を実施する。推定された質量および重心位置情報は、パラメータ記憶部２０２に出力される。

また、パラメータ推定部２０１に記載されたバイアス値推定部２１および近似曲線生成部２０によるバイアス値の具体的な求め方を以下に説明する。まず、回転軸Ｖｅｃ＿ｒｏｔとして、重力方向に垂直な面内で、センサ座標系の任意軸の１つとしてセンサ座標系の主軸周りで回転させた場合を考える。ここでは、重力方向に対してセンサ−Ｚ軸方向が一致する方向を基準姿勢Ｒ_k0とする。また、回転軸Ｖｅｃ＿ｒｏｔとしてセンサ座標系主軸のＹ軸を選ぶ。

続いて、図７〜９を参照しながら、軸力のバイアス値Ｆ＿ｂｉｓ＿ｘ、Ｆ＿ｂｉｓ＿ｙ、Ｆ＿ｂｉｓ＿ｚの計算方法について説明する。図７（ａ）、（ｂ）は、この発明の実施の形態１に係るキャリブレーション装置において、指定軸上の姿勢生成部により生成される姿勢指令値による動作を例示する説明図である。図７に示されるように、基準姿勢Ｒ_k0からＹ軸周りに回転させた姿勢を３点以上取得する。ここで取得した軸力データＦｘ、Ｆｚを、縦軸にＦｚ、横軸にＦｘをとって記述する。

なお、重力方向に垂直な面内に回転軸を選ぶ場合、回転運動をした際の作用力のベクトル変化に注目したときに、そのまま回転軸に直交する軸、ここではＸ軸およびＺ軸への力情報のプロットが、図８に示されるように、円軌道を描く特徴が現れる。この特徴を利用して、取得した力情報に対して各位置姿勢で取得したデータをプロットした際に、円となる近似曲線を生成する。

近似曲線生成部２０では、重力方向をセンサ座標系−Ｚ軸に合わせ、Ｙ軸周りに回転させた場合に得られるＸ軸方向の軸力をＦｘとし、Ｚ軸方向の軸力をＦｚとする。次に、横軸をＦｘ、縦軸をＦｚとして各点（Ｆｘ，Ｆｚ）に対する円近似を求める。これは、円の中心位置の原点からのオフセット量を、Ｘ軸力をプロットする横軸方向にはＦｘ＿ｂ、Ｚ軸力をプロットする縦軸方向にはＦｚ＿ｂとおくと、Ｆｘ＿ｂ、Ｆｚ＿ｂ、円の半径Ｒを変数とした円の公式として、次式（６）のように定義できる。

（Ｆｘ−Ｆｘ＿ｂ）＾２＋（Ｆｚ−Ｆｚ＿ｂ）＾２＝Ｒ＾２ ・・・（６）

ここで、これらの多項式の最小二乗近似解を求めるには、Ｆｘ＿ｂ、Ｆｚ＿ｂ、Ｒを変数とした関数ｆ（Ｆｘ＿ｂ，Ｆｚ＿ｂ，Ｒ）を次式（７）のように定義し、二乗して偏微分した結果が０となるような解を求める最小二乗近似を適用することができる。

ｆ（Ｆｘ＿ｂ，Ｆｚ＿ｂ，Ｒ）
＝（Ｆｘ−Ｆｘ＿ｂ）＾２＋（Ｆｚ−Ｆｚ＿ｂ）＾２−Ｒ＾２ ・・・（７）

これによって得られたＦｘ＿ｂ、Ｆｚ＿ｂが、Ｘ軸力のバイアス値Ｆ＿ｂｉｓ＿ｘ、Ｆ＿ｂｉｓ＿ｚである。なお、ここで得られるＲは、質量分の外力Ｍｇ’である。この外力Ｍｇ’から得られる質量を、仮の質量Ｍ＿ｔｍｐとする。また、回転主軸をＸ軸に変更して同様の処理を実施することで、Ｆ＿ｂｉｓ＿ｙも得られる。このようにして、軸力に関するバイアス値を算出することができる。

また、近似曲線生成部２０では、モーメントに関するバイアス値を以下のように求める。まず、モーメントに関しては、回転運動を実施した場合に、回転軸Ｖｅｃ＿ｒｏｔをＹ軸に選ぶと、回転軸Ｖｅｃ＿ｒｏｔ周りの姿勢変化Ｒ_kに対して、図９に示されるように、コサインカーブに位相差とオフセットとをかけた曲線に近似できるという特徴がある。

すなわち、数値モデルとしては、Ｙ軸周りのモーメントのバイアス成分をＭ＿ｙ＿ｂとして、基準姿勢Ｒ_k0からのＹ軸周りの回転角度をθとしたとき、横軸にθ、縦軸にモーメントをとった際の位相差φと定義する。また、コサインカーブの振幅をＡｍとおく。このとき、取得したモーメントデータをＭ＿ｙとすると、次式（８）が成り立つ。

Ｍ＿ｙ＝Ｍ＿ｙ＿ｂ＋Ａｍ＊ＣＯＳ（θ＋φ） ・・・（８）

なお、回転角度θに対する周期性としては、３６０度でちょうど１周期となる周波数になることが特徴である。このモデルに対して、ｆ（Ｍ＿ｙ＿ｂ、Ａｍ、φ）とする関数を次式（９）のように定義し、ニュートンラプソン法で繰り返し演算を実施することで、Ｍ＿ｙ＿ｂ、Ａｍ、φに関する近似解を得ることができる。

ｆ（Ｍ＿ｙ＿ｂ，Ａｍ，φ）
＝Ｍ＿ｙ−Ｍ＿ｙ＿ｂ−Ａｍ＊ＣＯＳ（θ＋φ） ・・・（９）

これによって得られたＭ＿ｙ＿ｂが、求めるＹ軸周りのモーメントのバイアス値Ｍ＿ｂｉｓ＿ｙとなる。なお、解の収束性が悪く近似解が得られない場合は、３６０の勾配数で３６０度を割った角度、例えば２であれば、θとして０度と１８０度とを選び、例えば４であれば、θとして０度と９０度と１８０度と２７０度とを選び、それぞれのＭ＿ｙの平均値をとってもよい。

また、同様にして、回転軸Ｖｅｃ＿ｒｏｔとしてＸ軸を選定することで、Ｍ＿ｂｉｓ＿ｘを得ることができる。また、Ｍ＿ｂｉｓ＿ｚについては、Ｙ軸、Ｘ軸で実施したのと同様に、重力方向に直交する面上にＺ軸を一致させる姿勢Ｒ_k1を基準位置とし、回転軸Ｖｅｃ＿ｒｏｔとしてＺ軸を選定し、Ｍ＿ｂｉｓ＿ｙを求めたときと同様の処理を実施することができる。

なお、他の方法として、単純にＲ_k0の姿勢から回転軸Ｖｅｃ＿ｒｏｔをＺ軸として選定し、３６０の勾配数で３６０度を割った角度、例えば２であれば、θとして０度と１８０度とを選び、例えば４であれば、θとして０度と９０度と１８０度と２７０度とを選び、それぞれのＭ＿ｚの平均値をとってもよい。

以上示した方法で、近似曲線生成部２０では、近似された曲線の式から仮のバイアス値推定が行われ、バイアス値Ｆ＿ｂｉｓおよび仮の質量Ｍ＿ｔｍｐが求められる。

なお、このとき、ケーブルが引っ掛かったデータが混入している場合は、図８のエラーデータ１３のような点が存在する。この発明の特徴は、この点を、近似曲線生成部２０の結果である近似曲線と力情報と位置情報とからキャリブレーション処理中に除去することにある。

具体的には、図８に示すようなエラーデータが数点である場合は、全部でＮ個の姿勢Ｒ_kでデータを取得した場合に、Ｎ−Ｍ個ずつのグループに分け、それぞれで最大誤差を比較することで、エラーデータを抽出することができる。ただし、ＭおよびＮは、Ｍ＜Ｎとなる正の整数である。

また、ツール部分の手先負荷質量および重心位置の算出については、図６の質量・重心位置推定部２２により実施される。ここで、力制御用の軸設定を、キャリブレーションされた状態のＸ軸、Ｙ軸、Ｚ軸の座標軸設定で、センサ中心位置にセンサ座標系Σｓｅｎとおき、手先負荷の重心位置にメカニカルフランジ座標系Σｍｅｃと同じ姿勢で重心座標系Σ_Lを定義する。

ここでは、センサ座標系と手先負荷の重心座標系との位置関係（ｘｑ，ｙｑ，ｚｑ，Ａｑ，Ｂｑ，Ｃｑ）、および質量ｍを未知変数ｑとして定義する。すなわち、ｑ＝（ｘｑ，ｙｑ，ｚｑ，Ａｑ，Ｂｑ，Ｃｑ，ｍ）として定義する。

ここで、モデルとセンサ出力との誤差を、モデルから推定される力Ｆ_mdlとセンサで実際に取得した前述のデータ^SＦ_iとの差で定義すると、モデルとセンサ出力との誤差は、次式（１０）で表される。なお、初期値としてｍ＝Ｍ＿ｔｍｐとする。

すなわち、最も誤差が小さいｑを探すことが本来の目的であるので、繰り返し演算でｆ（ｑ_i）がそれぞれ０に近づくようｑ_iを求める問題に帰着させることができる。なお、ｉはイテレーションである。

また、ニュートンラプソン法によると、漸近的に０に近づくための変数ｑ_iを求める場合は、次式（１１）で表される更新則が成り立つ。

ここで、繰り返し計算により、近似的に近付くときの毎回計算の差分の形で表現しなおすと、次式（１２）が得られる。

なお、具体的に式を展開すると、求める形は、次式（１３）の演算となる。

式（１３）は、式（１１）と式（１２）とをｄｑについて整理した表現となっている。式（１３）について説明すると、右辺で偏微分する変数については、一般にｑに関する変数すべてで定義される。ここでは、手先負荷の質量・重心位置の推定手法について、重力方向も考慮した一般形で表現しており、推定したい変数を重心位置＝（Ｘｓ，Ｙｓ，Ｚｓ，Ａｓ，Ｂｓ，Ｃｓ）、質量ｍ、ワールド座標系Σｗｌｄから見た重力ベクトルの傾きを、Ｘ軸およびＹ軸周りに関する回転量で、Ａｗ、Ｂｗを変数とした形式で表現している。

この発明の実施の形態１のように、重力方向の傾きを既知として除く場合は、Ａｗ，Ｂｗを除き、推定したい変数を重心位置＝（Ｘｑ，Ｙｑ，Ｚｑ，Ａｑ，Ｂｑ，Ｃｑ）、質量ｍとする対応が可能である。前述の変数ｑに合わせると、偏微分する変数は、Ｘｑ，Ｙｑ，Ｚｑ，Ａｑ，Ｂｑ，Ｃｑ，ｍとなる。

このとき、モデルから推定される力Ｆ_mdlは、下記のように定義できる。ただし、重心座標系Σ_Lをメカニカルフランジ座標系Σｍｅｃと同じ軸方向に定義し、重心座標系Σ_Lからみた軸力の３次元ベクトルである質量による外力ベクトルを^Lｆ、同じく重心座標系Σ_Lからみた質量によるモーメントベクトルを^Lｍとし、重心座標系から見た重力加速度ベクトルを^Lｇと表現している。また、ワールド座標系Σｗｌｄからみた重心座標系Σ_Lへの姿勢行列（３×３行列）は、^WＲ_Lで表現している。以下、次式（１４）はＦ_mdlの定義を示し、次式（１５）〜（２１）は式（１４）の要素の定義を示す。

なお、センサと負荷重心との位置関係、すなわち、ツール部分の重心位置から見たセンサ座標系への同次変換行列^LＴ_Sは、次式（２２）のように定義される。

また、式中に示されたｎ、ｏおよびａは、重心座標系Σ_Lからセンサ座標系Σｓに姿勢変更する場合のオイラー角（Ａ，Ｂ，Ｃ）を用いて、次式（２３）のように定義される。

以上の定義に基づいて、上記式（１２）、式（１３）を用いて更新していくと、漸近的に上記式（１０）が０に近づいていく。これによって、質量ｍおよびセンサ座標系とツール部分の重心位置との相対関係として（ｘｑ，ｙｑ，ｚｑ，Ａｑ，Ｂｑ，Ｃｑ）を得ることができる。

ロボット座標系Σｒｏｂとセンサ座標系Σｓｅｎとの相対関係は、センサ取り付け時のキャリブレーションで分かっているものとし、上述したように水準器を活用する等して、重力方向はロボット座標系Σｒｏｂに対して既知とするため、結果としてセンサ座標系Σｓｅｎからみた重力方向と質量ｍとの関係から、センサ座標系上で手先負荷の重力または慣性力の影響分を計算することができる。ただし、重力方向の情報については、水準器による取得に限るものではない。

このように、上述したキャリブレーション装置によれば、除去されなかったエラーデータを除去することができる。また、これによって得られた正確なバイアス値に基づいて算出される、質量およびツール部分の重心位置がより高精度に得られる。そのため、従来では得られなかった、エラーデータを含む力情報を用いた力情報キャリブレーションの高精度化を実現することができるため、力制御性能が格段に向上する効果が期待される。

以上のように、実施の形態１によれば、先端に取り付けられ、作業対象に対して作用するツール部分の力制御を行う機械装置において、作業対象との接触により、ツール部分に生じた外力のみを抽出するキャリブレーション装置であって、ツール部分の位置情報を取得する位置情報取得部と、ツール部分に作用する力情報を取得する力情報取得部と、センサ座標系の原点を通過する任意の回転軸を指定する回転軸指定部と、回転軸周りにツール部分を回転させる姿勢指令値を生成する姿勢生成部と、力情報からバイアス値および手先負荷の重力作用分を差し引いて、作業対象との接触により生じた外力のみを抽出するキャリブレーション処理を実施するキャリブレーション部とを備え、キャリブレーション部は、姿勢指令値に応じてツール部分を回転させた際の位置情報および力情報に基づいて、近似曲線を生成する近似曲線生成部と、近似曲線、位置情報および力情報に基づいて、力情報のバイアス値を推定するバイアス値推定部と、力情報からバイアス値を除去し、バイアス値が除去された力情報を用いて、手先負荷の質量および重心位置ベクトルを算出する質量・重心位置推定部と、推定されたバイアス値、手先負荷の質量および重心位置ベクトルを用いて、力情報からバイアス値および手先負荷の重力作用分を差し引く外力成分演算部とを有している。
そのため、外力による影響を考慮してバイアス成分を除去し、手先負荷質量および重心位置ベクトルを高精度に算出することで、高精度にキャリブレーションを実施することができる。
また、バイアス値を独立して高精度に推定することができ、結果として全体のキャリブレーション精度が向上するので、従来にない高精度な力制御を行うことができる。

実施の形態２．
上記実施の形態１に記載のキャリブレーション装置では、ケーブルや配線等によって生じるエラーデータを抽出するために、データをいくつかのグループ化を実施して、結果が大きくずれるものを特定し、エラーデータを発見する方法をとっていた。しかしながら、これだけでは、例えば、次のような場合に、エラーデータを除去できないことがある。

事例１：エラーデータが多く含まれる場合。この場合には、エラーデータのみを抽出するのが難しい。
事例２：センサ座標系の回転軸とロボットによって与えられる回転軸とがずれていて、近似曲線が円形でフィッティングできない場合。

そこで、この発明の実施の形態２では、このような問題を解決するために、図１０に示されるように、バイアス値推定部２１において、ツール部分に取り付けられるケーブルおよび配線による外力の作用により生じる力情報の増加分が所定の誤差レベルか、すなわち設定値を超えたか否かを判定して、誤差レベルに到達していれば質量・重心位置推定部２２に移り、そうでない場合は、さらに追加で新しい姿勢を取得することを特徴とする。

図１１（ａ）に示されるように、円形の場合は、フィッティングした円よりも半径が±Ｘ％変動した円を新たに定義し、それを所定の誤差レベルの閾値として用いる。ただし、Ｘの値は、ケーブルや配線の外力やノイズで受ける力の変動の大きさに応じて個別に定義する。

これは、ノイズで受ける力の変動が大きい場合には、フィルタをかけることでノイズの影響は低減できるものの、Ｘを小さくし過ぎると全てがエラーデータとなってしまうようなことが生じるためである。

この発明の実施の形態２に係るキャリブレーション装置により、所定の誤差レベルから外れるデータを徐々に除去していくことができるため、エラーデータを含まないデータに近づけていくことが可能である。

また、図１１（ｂ）に示されるように、上記事例２に該当するような場合は、楕円体フィッティングが必要になる。この場合は、上記式（６）に示した円の式を楕円の式に置き換えて定義することで対応することができ、楕円のまま評価するため、短径および長径に対してそれぞれ±Ｘ％という形で定義することができる。

さらに別の方法として、図１２に示されるように、データ記憶部１０５に記憶された位置情報および力情報を、図１１に示した形式でグラフとして表示する誤差情報表示部３０１と、マウスやキーボード、タッチパネルといった入力機器によりデータを選択させるデータ選択部３０２とを介して、ユーザにエラーデータを選択させる処理を通じてエラーデータを除去し、キャリブレーション処理を行ってもよい。

以上のように、実施の形態２によれば、複数のエラーデータを含む場合であっても、的確に選択することができ、高精度なキャリブレーションを実現することができるので、結果として、従来にない高精度な力制御を行うことができる。

実施の形態３．
上記実施の形態１または実施の形態２のキャリブレーション装置では、ツール部分による質量の影響を考慮するようなキャリブレーションは可能であった。しかしながら、例えば図１３に示されるように、ティーチングポイント１２に従って動くロボットにおいて、Ｐ２の地点で作業を開始する場合に、この時点で作用する外力Ｆ＿ｅｘｔに対して力制御をかけたくない場合がある。すなわち、ロボットの周辺物体１１にＰ２の時点で生じた接触した状態を保ちながら、Ｐ３の位置まで作業を進めたい場合がある。

このような場合には、単にツール部分のキャリブレーションを適用するだけでは不十分であり、接触状態を保持するような制御をすることは難しい。一方で、単純にゼロ点調整をするだけのシステムでは、ツール部分の姿勢変更には対応しないことになる。そのため、例えば、ロボットの周辺物体１１で拘束されていない方向、例えばＸｓｅｎ周りに回転する方向にツール部分を姿勢変更した場合には、センサ座標系に対する重力の軸方向が変化するため、ツール部分の姿勢変更による影響を考慮できず、外力が作用したと誤判断することになる。

そこで、この発明の実施の形態３では、このような問題を解決するために、図１４に示されるように、Ｐ２に至ったときに、ユーザがツール部分の位置姿勢の指令値を指定するオフセット位置姿勢指定部２０８により所定の位置姿勢情報を取得し、この位置姿勢にツール部分を移動した際に演算される作用外力の演算値をオフセット保持部２０９に記憶する。また、オフセット保持部２０９は、この瞬間に外力成分演算部２０７によって計算されていた作用外力の演算値を記憶する。

また、作業外力の演算値を記憶した次の制御周期以降について、外力成分演算部２０７によって計算されていた作用外力の演算値から、先程オフセット保持部２０９に記憶された作用外力の演算値を差し引いて出力する。

ただし、外力成分演算部２０７によって計算されていた作用外力の演算値の中には、従来の作用外力の演算値と、オフセット保持部２０９に記憶された作用外力の演算値を差し引いたオフセット処理後の作用外力の演算値との両方を情報として流すこととしている。これにより、さらにオフセット位置姿勢指定部２０８で姿勢を指定した場合に、その位置で生じている作用外力分だけオフセットとして演算することができる。

以上のように、実施の形態３によれば、特定の位置で生じる接触状態を保ちながらさらに姿勢を変更するような複雑な動作を生成することが可能となり、従来にない動作生成が容易に生成できるため、ユーザの使いやすさが格段に向上する。
すなわち、ツール部分のキャリブレーション以外の作用による外力分を考慮した力制御を行う際に、ある動作中に力制御には関与しない一定外力として取り扱うことできるようになるので、外部との接触を伴う力制御をする場合の設定が非常に容易になり、ユーザの使いやすさを向上させることができる。
なお、以上のような発明は、産業用のロボットまたは位置制御が可能な機械装置に適用することができる。

Claims (5)

1. 先端に取り付けられ、作業対象に対して作用するツール部分の力制御を行う機械装置において、前記作業対象との接触により、前記ツール部分に生じた外力のみを抽出するキャリブレーション装置であって、
   前記ツール部分の位置情報を取得する位置情報取得部と、
   前記ツール部分に作用する力情報を取得する力情報取得部と、
   前記力情報を取得するセンサのセンサ座標系の原点を通過する任意の回転軸を指定する回転軸指定部と、
   前記回転軸周りに前記ツール部分を回転させる姿勢指令値を生成する姿勢生成部と、
   前記力情報からバイアス値および手先負荷の重力作用分を差し引いて、前記作業対象との接触により生じた外力のみを抽出するキャリブレーション処理を実施するキャリブレーション部と、を備え、
   前記キャリブレーション部は、
   前記姿勢指令値に応じて前記ツール部分を回転させた際の前記位置情報および前記力情報に基づいて、近似曲線を生成する近似曲線生成部と、
   前記近似曲線、前記位置情報および前記力情報に基づいて、前記力情報のバイアス値を推定するバイアス値推定部と、
   前記力情報から前記バイアス値を除去し、前記バイアス値が除去された前記力情報を用いて、前記手先負荷の質量および重心位置ベクトルを算出する質量・重心位置推定部と、
   推定された前記バイアス値、前記手先負荷の質量および重心位置ベクトルを用いて、前記力情報から前記バイアス値および前記手先負荷の重力作用分を差し引く外力成分演算部と、を有する
   キャリブレーション装置。
2. 前記バイアス値推定部は、前記ツール部分に取り付けられるケーブルおよび配線の少なくとも一方による外力の作用により生じる前記力情報の増加分が設定値を超えた場合に、当該力情報を除去する
   請求項１に記載のキャリブレーション装置。
3. 前記キャリブレーション部は、
   ユーザが前記ツール部分の位置姿勢の指令値を指定するオフセット位置姿勢指定部と、
   前記ユーザが指定した位置姿勢に前記ツール部分を移動した際に演算される作用外力の演算値を記憶するオフセット保持部と、を有し、
   前記オフセット保持部が動作した次の演算周期のキャリブレーションから、前記オフセット保持部に保持された作用外力の演算値分をさらに差し引く
   請求項１または請求項２に記載のキャリブレーション装置。
4. 先端に取り付けられ、作業対象に対して作用するツール部分の力制御を行う機械装置において、前記作業対象との接触により、前記ツール部分に生じた外力のみを抽出するキャリブレーション装置であって、
   前記ツール部分の位置情報を取得する位置情報取得部と、
   前記ツール部分に作用する力情報を取得する力情報取得部と、
   前記力情報を取得するセンサのセンサ座標系の原点を通過する任意の回転軸を指定する回転軸指定部と、
   前記回転軸周りに前記ツール部分を回転させる姿勢指令値を生成する姿勢生成部と、
   前記姿勢指令値に応じて前記ツール部分を回転させた際の前記位置情報および前記力情報に基づいて、前記力情報からバイアス値および手先負荷の重力作用分を差し引いて、前記作業対象との接触により生じた外力のみを抽出するキャリブレーション処理を実施するキャリブレーション部と、
   を備えた、キャリブレーション装置。
5. 請求項１から請求項４までの何れか１項に記載されたキャリブレーション装置が適用されたロボットシステム。

Images