JP5618066B2 - 力制御ロボットのキャリブレーション装置と方法 - Google Patents

Description

本発明は、力制御を使用するロボットシステムにおけるロボットの設置位置誤差等に関わらず接触力を計測するキャリブレーション装置と方法に関する。

ロボットのツール位置較正手段に関連して、特許文献１〜４が既に開示されている。

特許文献１、２は治具などを用いて較正するものである。また図面情報などをベースに数値入力により設定することも知られている。
さらに特許文献３は視覚センサなどを利用して撮像したデータから特定点の認識を行うことなどで、較正・設定するものである。
また、特許文献４はロボットに複数の姿勢をとらせ、各姿勢おける力センサ出力から力センサを較正するものである。

特開昭６１−０２５２０６号公報、「ロボットの工具位置設定方式」 特開昭６１−０２５２０７号公報、「ツール座標系の設定方式」 特許第４０２０９９４号公報、「ロボットのツール座標系補正設定方法並びに該方法に使用するエンドエフェクタ」 特公平０６−３９０７０号公報、「ロボット装置の力センサ較正方法」

従来は、手先に力センサを設け、作業中のツール反力を計測しながら接触作業するロボットの重力補償において、作業前にツールの重量と重心位置を計測し、作業中の重心位置の移動、変動を計測してツールの重力成分を差し引くことが行われている。
また、重心位置などの計測は、従来は、（ａ）ツールの３ＤＣＡＤモデルを作成し、手先の重心位置を算出する手段や、特許文献４のように、（ｂ）ロボットに複数の姿勢を取らせ、計測した力センサの値からツールの重量と重心位置を算出する手段が提案されている。

しかし、上述した従来の技術では、ロボットの設置姿勢の誤差を考慮されていないため、微小なツール押し付け力での制御が行えないといった問題点があった。

すなわち、従来、ロボットを所定の位置・姿勢に設置する際に、正確に設計値どおりに設置することは困難であり、設置誤差が生じることがある。この場合、従来の技術では、この誤差を考慮せず、ロボット座標系における重力方向に誤差を含んだ状態でツール重心等をキャリブレーションするため、正確な重力補償ができない。
なお、力覚センサの計測データは、ツールに加わる重力と、ツールに加わる外力との合力である。この計測データから、ツール重心位置やツールの姿勢等からツールに加わる重力を差し引くことで、ツールに加わる外力を算出することを重力補償と呼ぶ。
このため、例えばバリ取りロボットなどでは、微小なツール押し付け力での制御が行えない問題点があった。

本発明はかかる問題点を解決するために創案されたものである。すなわち、本発明の目的は、ロボットの設置精度が低い場合でもロボット座標系における重力方向、ツール重量、ツール重心位置を含むパラメータをキャリブレーションすることができる力制御ロボットのキャリブレーション装置と方法を提供することにある。

本発明によれば、３次元動作するロボットアームの手先に力センサを介してツールが取り付けられている力制御ロボットのキャリブレーション装置であって、
前記ロボットアームを複数の姿勢に動作させて前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データを取得するロボット制御装置と、
前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データと、を記憶する記憶装置と、
前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データから、重力方向ベクトル、ツール重量、ツール重心位置ベクトル、及び力センサのバイアス値を算出する演算装置とを備える、ことを特徴とする力制御ロボットのキャリブレーション装置が提供される。

また本発明によれば、３次元動作するロボットアームの手先に力センサを介してツールが取り付けられている力制御ロボットのキャリブレーション方法であって、
前記ロボットアームを複数の姿勢に動作させて前記力センサの計測値と、力センサの姿勢データと、を取得し、
前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データとから、重力方向ベクトル、ツール重量、ツール重心位置ベクトル、及び力センサのバイアス値を算出する、ことを特徴とする力制御ロボットのキャリブレーション方法が提供される。

上記本発明の装置と方法によれば、ロボット座標系における重力方向ベクトルを含む複数のパラメータと力センサの検出値との関係を示す計算モデルと、ロボットアームを複数の姿勢に動作させて取得した力センサの計測値とから、前記複数のパラメータを算出するので、パラメータであるロボット座標系における重力方向ベクトル、力センサ座標系におけるツール重心位置ベクトル、力センサ座標系におけるツール重量、及び力センサのバイアス値を同時にキャリブレーションすることができる。

従って、ロボットの設置精度が低い場合でも、その誤差を考慮して、必要なパラメータをキャリブレーションすることができるので、ツール重心位置ベクトルやツール重量のキャリブレーション精度を向上することができる。
そのため、これらのパラメータを使った重力補償の精度が向上し、高精度での力制御が可能になる。

特にバリ取りロボットでは位置・力のハイブリッド制御で一定の加工反力を得られるよう制御しており、力センサの誤差はツールの押し付け力の誤差に直結しているため仕上げに大きな影響を与える。また、精度よく加工反力が得られることで微小な加工押し付け力で制御する必要のある３次元曲面加工が可能になる。

本発明による力制御ロボットのキャリブレーション装置を備えたロボットの全体構成図である。 本発明による力制御ロボットのキャリブレーション装置の構成図である。 本発明の力制御ロボットのキャリブレーション方法を示すフローチャートである。 本発明の力制御ロボットのキャリブレーション方法を示す別のフローチャートである。

以下、本発明の好ましい実施例を図面を参照して説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明による力制御ロボットのキャリブレーション装置を備えたロボットの全体構成図である。この図において、（Ａ）は全体図、（Ｂ）は（Ａ）のＢ部説明図である。

図１（Ａ）において、ロボット１は、その手先に力センサ３を介してツール４（工具）が取り付けられている。
本発明による力制御ロボットのキャリブレーション装置１０は、ロボット座標系Σ_Ｒにおけるロボット１の力センサ３のキャリブレーション装置である。
ここでロボット座標系Σ_Ｒとは、ロボット１の土台に固定された座標系であり、Ｘ，Ｙ，Ｚの座標軸を有する。

また図１（Ｂ）に示すように、力センサ３に固定された座標系を力センサ座標系Σ_Ｓと呼ぶ。力センサ座標系Σ_Ｓも、Ｘ，Ｙ，Ｚの座標軸を有する。

ロボット座標系Σ_Ｒ基準での力覚センサ座標系Σ_Ｓの位置・姿勢は、Ｘ，Ｙ，Ｚの座標軸に沿った並進量と、各軸周りの回転角度、ヨー角Ａ（Ｚ軸回り）、ピッチ角Ｂ（Ｙ軸回り）、ロール角Ｃ（Ｘ軸回り）で表現する。ただし、姿勢の表現方法はこの限りではなく、オイラー角を用いた方法などもある。

本発明でキャリブレーションするパラメータ５は、ロボット座標系Σ_Ｒ基準での重力方向ベクトルｎ_Ｇ、力センサ座標系Σ_Ｓにおけるツール重心位置ベクトル^Ｓｒ_Ｇ、ツール重量ｆ_Ｔ、及び力センサ３のバイアス値^ＳＦ_ｂｉａｓである。
なお、力センサのバイアス値とは、力センサ３に負荷を加えない状態での力センサ３の計測値を意味する。力センサのバイアス値は、理想的には０だが、実際には誤差が生じる。

図２は、本発明による力制御ロボットのキャリブレーション装置の構成図である。
この図に示すように、本発明の力制御ロボットのキャリブレーション装置１０は、記憶装置１２、ロボット制御装置１４、及び演算装置１６を備える。

記憶装置１２は、例えばハードディスク、メモリカード、ＲＯＭ、ＲＡＭ等であり、力センサ計測値と、前記計測値を取得するときの力覚センサの姿勢データとを記憶する。
この計算モデル６は、図示しない入力装置、例えばキーボード、メモリリーダから記憶装置１２に入力されている。

本発明でキャリブレーションするパラメータ５は、ロボット座標系Σ_Ｒ基準での重力方向ベクトルｇ、力センサ座標系Σ_Ｓにおけるツール重心位置ベクトル^Ｓｒ_Ｇ、ツール重量ｆ_Ｔ、及び力センサ３のバイアス値^ＳＦ_ｂｉａｓである。

ロボット制御装置１４は、例えばコンピュータであり、ロボットアーム１を複数の姿勢に動作させて力センサ３の計測値と、前記計測値を取得するときの力覚センサの姿勢データと、を取得する。取得された計測値と、前記計測値を取得するときの力覚センサの姿勢データとは、記憶装置１２に記憶される。
ロボット制御装置１４は、ロボットアーム１を制御するロボットコントローラの機能を備えていることが好ましい。

演算装置１６は、例えばコンピュータのＣＰＵであり、記憶した計算モデル６と記憶した計測値と、前記計測値を取得するときの力覚センサの姿勢データとから、パラメータ５を算出する。
なお、記憶装置１２、ロボット制御装置１４、演算装置１６を１台または複数のコンピュータで構成してもよい。

図３は、本発明の力制御ロボットのキャリブレーション方法を示すフローチャートである。この図に示すように、本発明の方法は、ロボット座標系Σ_Ｒにおけるロボット１の力センサ３のキャリブレーション方法であり、Ｓ１〜Ｓ４の各ステップ（工程）からなる。

Ｓ１では、ロボット制御装置１４によりロボットアーム１を複数の姿勢に動作させ、Ｓ２で力センサの計測値と、前記計測値を取得するときの力覚センサの姿勢データと、を取得する。取得した計測値と、前記計測値を取得するときの力覚センサの姿勢データとは、記憶装置１２に記憶する。
Ｓ３で、演算装置１６により、記憶した計測値と、前記計測値を取得するときの力覚センサの姿勢データとから、パラメータ５を算出する。
Ｓ４では、Ｓ３で算出したパラメータ５を出力する。パラメータ５の出力先は、例えばＣＲＴ、記憶装置１２、ロボット制御装置１４である。

以下、本発明の実施例を詳細に説明する。

１．本発明では、手先に力覚センサ（以下、「力センサ」という）を搭載した産業用ロボットアームにおいて、手先のツール重心等の計測手段について説明する。また、本発明を適用する装置として、バリ取り・仕上げロボットを想定する。

２．記法について
本発明では、左上の添え字はその座標系を表す。例えば、座標系Σ_Ｌにおけるツール目標位置・姿勢を^ＬＹ_Ｄと表す。なお、図中の表記などでは一部省略している場合がある。

座標系Σ_Ｐと座標系Σ_Ｑとの位置関係が、座標系Σ_ＰのＺ軸周りに^Ｐａ_Ｑだけ回転させた座標系がΣ_ＰＰであり、座標系Σ_ＰＰのＹ軸周りに^Ｐｂ_Ｑだけ回転させた座標系がΣ_ＰＰＰであり、座標系Σ_ＰＰＰのＸ軸周りに^Ｐｃ_Ｑだけ回転させた座標系がΣ_Ｑである場合、座標系Σ_Ｑ上の位置ベクトル^Ｑｒを座標系Σ_Ｐ上の位置ベクトル^ｐｒに座標変換する回転行列^ＰＲ_Ｑを数１の式（１）（２）と定義する。
ここで、^Ｐａ_Ｑはヨー角、^Ｐｂ_Ｑはピッチ角、^Ｐｃ_Ｑはロール角である。

回転行列には、^ＰＲ_Ｑ＝^ＱＲ_Ｐ ^Ｔ・・・（２．１）の性質があり、ここでの添字Ｔは転置を表す。
位置ベクトルｒ＝［ｘ ｙ ｚ］^Ｔ・・・（２．２）について、［ｒ×］は外積行列を表し、数１の式（３）で表される。

３．キャリブレーションするパラメータ
本発明で使用する座標系を図１に示す。キャリブレーションするパラメータ５は、以下の通り。
（１）ロボット座標系Σ_Ｒにおける重力方向ベクトル^Ｒｎ_Ｇ
（長さ１の単位ベクトルとする）
（２）力センサ座標から先のツール重量ｆ_Ｔ
（３）力センサ座標系Σ_Ｓにおけるツール４の重心位置ベクトル^Ｓｒ_Ｇ
（４）力センサ３の各軸ごとの零点からのバイアス値^ＳＦ_ｂｉａｓ

ロボット座標系Σ_Ｒと力センサ座標系Σ_Ｓとの相対位置は、ロボットコントローラにおけるロボット１の姿勢データを使って算出する。

４．キャリブレーション方法
ロボット１の姿勢を複数変えて、力センサ３の計測値と、前記計測値を取得するときの力覚センサの姿勢データとを取得することで、前述のパラメータ５をキャリブレーションする。

５．力センサ３の計測値の取得
ロボット１の姿勢をｎ通りに変えて力センサ３の計測値を取得する。ｎ番目のロボット姿勢［ａ_ｎ ｂ_ｎ ｃ_ｎ］^Ｔ・・・（３．１）を式（２）に代入して算出される回転行列を^ＳＲ_ｎＲとし、その時の力センサ３の計測値を^ＳＦ_ｎ＝［^ＳＦ_ｎｘ ^ＳＦ_ｎｙ ^ＳＦ_ｎｚ ^ＳＴ_ｎｘ ^ＳＴ_ｎｙ ^ＳＴ_ｎｚ］^Ｔ・・・（３．２）とすると、力センサ３の計測値の計算モデル６は、数２の式（４）となる。

ここでの未知数は、力センサ３のバイアス値^ＳＦ_ｂｉａｓ＝［^ＳＦ_ｂｘ ^ＳＦ_ｂｙ ^ＳＦ_ｂｚ ^ＳＴ_ｂｘ ^ＳＴ_ｂｙ ^ＳＴ_ｂｚ］^Ｔ・・・（４．１）、力センサ３から先のツール４の質量ｍ_Ｔ、重力方向ベクトル^Ｒｎ_Ｇ＝［ｎ_ｘ ｎ_ｙ ｎ_ｚ］^Ｔ・・・（４．２）、ツール４の重心位置ベクトル^Ｓｒ_Ｇ＝［^Ｓｘ_Ｇ ^Ｓｙ_Ｇ ^Ｓｚ_Ｇ］^Ｔ・・・（４．３）である。

６．重力方向ベクトルの算出
数３の式（５）に示すように、２つの計測値の差を取ることでバイアス値^ＳＦ_ｂｉａｓを消去する。

ｎ個の計測値からのペアを作って同様に、バイアス値^ＳＦ_ｂｉａｓを消去し、それぞれの１〜３行目を使って、重力方向ベクトルｎ_Ｇについての連立方程式を、式（６）とおく。

通常、式（６）の連立方程式は解を持たないが、｜ｆ_ＴＡ・ｎ_Ｇ−ｂ｜・・・（６．１）を最小にする近似解の一般解ｆ_Ｔｎ_Ｇは、式（７）（８）と表される。ここで、Ａ^＋はＡの擬似逆行列、ｋは１×３の任意ベクトルである。

さらに、この解のうちで解自身のノルム｜ｆ_Ｔｎ_Ｇ｜・・・（７．１）を最小にするものは、式（９）と表される。以降は、式（９）で算出したｆ_Ｔ，ｎ_Ｇを使用する。

東京における重力加速度｜ｇ｜＝９．７９７６３を使用するなどして，ツール質量を算出することもできる。

７．重心位置の算出
式（５）のそれぞれの４〜６行目を使って、重心位置ベクトルｒ_Ｇに関する連立方程式を数４の式（１０）とおく。重力方向ベクトルｎ_Ｇの算出と同様に、ｒ_Ｇの近似解を式（１２）として算出する。

８．力センサのバイアス値の算出
算出したツール重量ｆ_Ｔ、重力方向ベクトルｎ_Ｇ、重心位置ベクトルｒ_Ｇを、式（４）に代入し、力センサ３のバイアス値^ＳＦ_ｂｉａｓを算出する。

９．計測する姿勢の選択
ロボット１の姿勢や計測する回数は、式（６）、式（１０）においてｒａｎｋＡ＝３かつｒａｎｋＡ_ｒ＝３（ｒａｎｋは行列の階数）・・・（１２．１）となるように選択する。

ロボットの姿勢数が２の場合、ｒａｎｋＡ＝３かつｒａｎｋＡ_ｒ＝３であることの必要十分条件は、ｒａｎｋ（^ＳＲ_２−^ＳＲ_１）＝３かつｒａｎｋ（［ｒ×］・（^ＳＲ_２−^ＳＲ_１））＝３・・・（１２．２）であり、さらにこの必要十分条件は「計測時のロボット姿勢における力センサ座標系Σ_１、Σ_２のＸ・Ｙ・Ｚ軸が一致しない」かつ「２つの座標系の回転軸がｒ_Ｇと平行でない」となる。このときＡ、Ａ_ｒは６×３の行列となる。ここで、Ａ，Ａ_ｒの階数を減らさないように適当な行のみを選択して、３×３の行列Ａ^’，Ａ_ｒ ^’を再度構成すれば、Ａ^’，Ａ_ｒ ^’には逆行列が存在するため、重力方向ベクトルｎ_Ｇ、重心位置ベクトルｒ_Ｇは解を持ち、逆行列を使った演算によって解を算出できる。
また、計測データの数を多くして最小二乗法の近似解を使用すると、｜ｆ_ＴＡ・ｎ_Ｇ−ｂ｜や｜ｆ_ＴＡ_ｒ・ｒ_Ｇ−ｂ_ｒ｜によって、解の妥当性を定量的に判定できるため、計測の失敗によって計測値中に異常値を含んだ場合に、エラー判定ができる。

近似解の算出方法はこの限りではなく、擬似逆行列を逐次最小二乗法を使って算出する方法では、計測データを取得するたびごとにパラメータを算出・更新していき、パラメータの算出結果は次第に収束していく。処理のフローは図４のようになる。

上述した本発明の装置と方法によれば、ロボットアーム１を複数の姿勢に動作させて取得した力センサ３の計測値と、前記計測値を取得するときの力覚センサの姿勢データとから、複数のパラメータ５を算出するので、パラメータ５であるロボット座標系Σ_Ｒにおける重力方向ベクトルｎ_Ｇ、力センサ座標系Σ_Ｓにおけるツール重心位置ベクトル^Ｓｒ_Ｇ、力センサ座標系Σ_Ｓにおけるツール重量ｆ_Ｔ及び力センサ３のバイアス値^ＳＦ_ｂｉａｓを同時にキャリブレーションすることができる。

従って、ロボット１の設置精度が低い場合でも、その設置誤差を考慮して、重力方向ベクトルｎ_Ｇ、ツール重心位置、ツール重量を含むパラメータ５を同時にキャリブレーションすることができるので、ツール重心位置ベクトルやツール重量のキャリブレーション精度を向上することができる。
そのため、これらのパラメータを使った重力補償の精度が向上し、高精度での力制御が可能になる。

特にバリ取りロボットでは位置・力のハイブリッド制御で一定の加工反力を得られるよう制御しており、力センサ３の誤差はツール４の押し付け力の誤差に直結しているため仕上げに大きな影響を与える。また、精度よく加工反力が得られることで微小な加工押し付け力で制御する必要のある３次元曲面加工が可能になる。

なお、本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々の変更を加え得ることは勿論である。

１ ロボット（バリ取りロボット）
３ 力センサ（力覚センサ）、４ ツール（工具）、
５ パラメータ、６ 計算モデル、
１０ 力制御ロボットのキャリブレーション装置、
１２ 記憶装置、
１４ ロボット制御装置、
１６ 演算装置

Claims (2)

1. ３次元動作するロボットアームの手先に力センサを介してツールが取り付けられている力制御ロボットのキャリブレーション装置であって、
   前記ロボットアームを複数の姿勢に動作させて前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データを取得するロボット制御装置と、
   前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データと、を記憶する記憶装置と、
   前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データから、重力方向ベクトル、ツール重量、ツール重心位置ベクトル、及び力センサのバイアス値を算出する演算装置とを備える、ことを特徴とする力制御ロボットのキャリブレーション装置。
2. ３次元動作するロボットアームの手先に力センサを介してツールが取り付けられている力制御ロボットのキャリブレーション方法であって、
   前記ロボットアームを複数の姿勢に動作させて前記力センサの計測値と、力センサの姿勢データと、を取得し、
   前記力センサの計測値と、前記計測値を取得するときの力センサの姿勢データとから、重力方向ベクトル、ツール重量、ツール重心位置ベクトル、及び力センサのバイアス値を算出する、ことを特徴とする力制御ロボットのキャリブレーション方法。