JPH0429517B2 - - Google Patents

Description

【発明の詳細な説明】 産業上の利用分野 ロボツトに力センサをとりつけたシステムにお
いて使用される力センサは、ストレインゲージを
利用した構成となつたものが多い。多くの場合、
力センサ出力にはドリフトがあり、温度等の使用
環境の変化や使用時間の経過にしたがつてドリフ
トは変動する。力の検出を行う際に作用している
力の絶対値を求めるためには、力センサ出力から
変動するドリフトを除去しなければならない。本
発明は、ロボツトにとりつけられた力センサのド
リフト補償方法に関するものである。

従来の技術 力センサのドリフトを補償するために、はじめ
に一度ドリフトを何らかの手段によつて求めて、
そのドリフトの一定値を、力センサ出力から差し
引くという方法がとられていた。

発明が解決しようとする問題点 力センサのドリフトが時間経過とともに変化す
る場合、ドリフトを一定値として力センサ出力か
ら差し引くという方法では、ドリフトの変化分だ
けの誤差が生じる。力センサのドリフトは、短時
間では大きく変化しないが、産業用ロボツトのよ
うに長時間連続運動するシステムでは、ドリフト
を求めるために運転を停止させることは稼動率の
点で不利である。また、ドリフトは毎日同じパタ
ーンで変化するとは限らないので、システムを稼
動させながら変動するドリフトを補償する必要が
ある。

問題点を解決するための手段 上記問題点を解決するために、本発明のドリフ
ト補償方法では、予め正確なドリフトを求めて初
期ドリフトとして記憶すると同時に、ロボツト運
転中の作業においてあらわれる動作点における力
センサ出力を、ロボツトの作業中と同一負荷条件
でロボツトに動作させて測定して記憶した後、作
業中、前記動作点を動作する時に力センサ出力を
測定し、この力センサ出力と前記記憶された力セ
ンサ出力の差をドリフト変化として記憶し、力の
検出を行う時に、その時点の力センサ出力から前
記初期ドリフトおよび前記ドリフト変化を差し引
くことにより、力センサ出力からドリフトを除去
するという方法をとつている。

作 用 初期ドリフトを測定すると同時に較正用測定点
で測定する力センサ出力は、ロボツトの作業中
の較正用測定点で力センサ出力を測定する時と
同一負荷条件で測定されるので、力センサ出力
と力センサ出力の差は、力センサのドリフト変
化と考えられる。すなわち、 ｄ（t_c）＝ｄ（t_p）＋｛ｃ（t_c）−ｃ（t_p）｝ ただし ｄ（ｔ）：時刻ｔのドリフト ｃ（ｔ）：時刻ｔの較正用測定点で力センサ出力 t_p：初期ドリフト測定時刻 t_c：較正用測定点の力センサ出力測定時刻 力の検出時は、次の計算でドリフト除去を行
う。

ｆ（t_c＋Δt）＝ｓ（t_c＋Δt）−ｄ（t_c） ただし ｆ（ｔ）：時刻ｔのドリフト除去後の力センサ出
力 ｓ（ｔ）：時刻ｔの力センサ出力 Δt：較正用測定点での力センサ出力を測定し
ドリフト変化を最後に更新した時刻から、力の検
出のための力センサ出力の測定時刻までの経過時
間。

初期ドリフト測定時から長時間経過しても、較
正用測定点で行うドリフト変化の更新からの経過
時間Δtが短かければ、力を検出する時に誤差の
少ないドリフト補償ができる。

逆に、較正用測定点での力センサ出力測定とド
リフト変化の更新や十分に短時間毎に行うように
すれば、誤差は少なくすることができる。

実施例 第１図は、本発明の一実施例のおける力センサ
付きロボツトシステムのブロツク図、第２図はロ
ボツトの斜視図である。ロボツト１は垂直多関節
型ロボツトで６自由度の動きができ、許容動作範
囲内でハンド１０の位置、姿勢を任意に指定して
動作させることができる。前記ロボツト先端９と
ハンド１０の間に力センサ２が取り付けられてい
る。力センサ２は６軸力センサであつて、ハンド
１０にかかる力およびモーメントを、第３図に示
すようなセンサ座標系の直交する３軸方向の力
f_x、f_y、f_zおよび前記３軸のまわりのモーメント
m_x、m_y、m_zとして検出し、測定できるものであ
る。ロボツト１は制御装置８によつて動作を制御
され、力センサ出力は制御装置８により測定され
る。制御装置８は内蔵する計算機のプログラムに
したがつて動作し、制御装置内に教示・設定され
たプログラムおよびデータにしたがつてロボツト
は作業に応じた動作を繰り返す。

動作の中の適当な一点または複数点を選び、較
正用測定点とする。教示・設定プログラムおよび
データによつて、作業動作実行中の較正用測定点
において、次の手続きを実行するようにしてお
く。すなわち、力センサ出力を測定し、較正用測
定点初期力センサ出力との差をとりドリフト変化
とし、これを初期ドリフトに加えたものをその時
点のドリフトとして制御装置８内の記憶素子７に
記憶更新する。この手続きのフローチヤートを第
４図に示す。

次に第５図をもとに、本発明におけるドリフト
補償方法について説明する。ロボツトの作業動作
をはじめる前に、先ず後述する手段（初期ドリフ
ト決定手段）３によりドリフトを求め、初期ドリ
フトとして制御装置８の記憶素子７に格納し、同
時に、前記較正用測定点における力センサ出力
を、教示プログラム実行時と同一負荷条件（速
度、荷重など）でロボツト１を動作させた条件の
下で測定（較正用測定点力センサ出力測定手段
４）し、較正用測定点初期力センサ出力として制
御装置８内の記憶素子７に格納する。作業動作開
始時でのドリフト変化はゼロとして制御装置８内
の記憶素子７に格納しておく。その後、ロボツト
に作業を行わせる。この作業は繰り返し動作によ
つて構成され、動作１サイクルの中には前記較正
用測定点が含まれ、設定されたプログラムおよび
データにしたがつて、第４図のフローチヤートに
示される手続きが各較正用測定点において実行さ
れる。この作業の中で力の検出を行う時には、力
センサ出力からドリフトdriftを差し引いてドリ
フトを除去する（ドリフト除去手段５）。このよ
うに第４図のフローチヤートの手続きが実行され
る毎にドリフトdriftが更新されるために、ロボ
ツトが連続運転される間にドリフトが変化して
も、ドリフト変化による力検出時の誤差は小さく
することができる。なお、第１図において、６は
ロボツト動作制御手段である。

次に、初期ドリフトを求める手段について説明
する。まず、適当なハンド姿勢を３つ選び、各ハ
ンド姿勢における力センサ出力を測定する。そし
て、以下の計算を行う。

次の式により、作用する力と力センサ出力の関
係を考える。

F_O＝Ｆ＋F_W（θ）＋Ｄ ただし F_O：力センサの出力ベクトル Ｆ：重力以外でハンドに作用する力のベクト
ル F_W（θ）：ハンド姿勢θにおいてハンドにか
かる重力による因子の力センサ座標系ベクトル Ｄ：力センサのドリフトベクトル ハンドに重力のみがかかる状態では、 F_O＝F_W（θ）＋Ｄ となる。

ハンドにかかる重力は絶対座標系において常に
一定の大きさと向きを持ち、F_W（θ）はハンドに
かかる重力にハンド姿勢θにしたがつた変換を施
したものになる。

ハンド座標系とセンサ座標系は一致していると
する。一致していなければ、ハンド座標系と力セ
ンサ座標系は一定の相対位置関係にあるので座標
交換を行う。ハンド姿勢θ_i（ｉ＝１、２、３）に
おけるハンド座標系のＸ軸、Ｙ軸、Ｚ軸各正方向
の単位ベクトルが絶対座標系においてそれぞれ N_i＝（n_xi，n_yi，n_zi）^T O_i＝（o_xi，o_yi，o_zi）^T A_i＝（a_xi，a_yi，a_zi）^T と表わされるとする。ハンドにかかる重量がＷ
で、ハンドの重心位置がハンド座標系の Gr＝（g_hx，g_hy，g_hz）^T にあるとする。この時、F_W（θ）は F_W（θ）＝F_x〓 F_y〓 F_z〓 M_x〓 M_y〓 M_z〓＝−Ｗn_zi o_zi a_zi a_zig_hy−o_zig_hz n_zig_hz−a_zig_hx o_zig_hx−n_zig_hy となり、ハンド姿勢θ_i（ｉ＝１、２、３）におけ
る力センサ出力がf_pi（f_xi，f_yi，f_zi，m_xi，m_yi，m_zi
）
とすると、例えば次のような計算によりドリフト Ｄ＝（d_fx，d_fy，d_fz，d_nx，d_ny，d_nz）^Tを求める。

Ｗ＝（f_x1＋f_y1＋f_z1−f_x2−f_y2−f_z2）／（n_z1＋o_z1
＋a_z1−n_z2−o_z2−a_z2） d_fx＝１／２｛f_x1＋f_x2＋ｗ・（n_z1＋n_z2）｝ d_fy＝１／２｛f_y1＋f_y2＋ｗ・（o_z1＋o_z2）｝ d_fz＝１／２｛f_zi＋f_z2＋ｗ・（a_z1＋a_z2）｝ Ｈ＝θ −a_z1＋a_z2 o_z1−o_z2 θ −a_z1＋a_z3 o_z1−o_z3 a_z1−a_z2 θ −n_z1＋n_z2 a_z1−a_z3 θ −n_z1＋n_z3 −o_z1＋o_z2 n_z1−n_z2 θ −o_z1＋o_z3 n_z1−n_z3 θ q_hx q_hy q_hz＝H⁺m_x1−m_x2 m_y1−m_y2 m_z1−m_z2 m_x1−m_x3 m_y1−m_y3 m_z1−m_z3 d_nx＝１／３｛₃ Σⁱ⁼¹ m_xi＋ｗ₃ 〓ⁱ⁼¹ （a_zig_hy−o_zig_hz）｝ d_ny＝１／３｛₃ Ζⁱ⁼¹ m_yi＋ｗ₃ 〓ⁱ⁼¹ （n_zig_hz−a_zig_hx）｝ d_nz＝１／３｛₃ Σⁱ⁼¹ m_xi＋ｗ₃ 〓ⁱ⁼¹ （o_zig_hx−n_zig_hy）｝ ただしH⁺はＨの擬似逆行列（Moore−
Penroseの逆行列）である。

発明の効果 以上のように本発明では、力検出の時に差し引
くべきドリフトの値を、力センサをロボツトに組
み込んだままの状態でロボツトの作業中に更新し
ていく方法をとつているため、力センサのドリフ
トが時間変化しても許容誤差内でドリフトを除去
することができる。

【図面の簡単な説明】

第１図は本発明の構成の説明図、第２図は本発
明の一実施例における力センサ付きロボツトシス
テムの斜視図、第３図は６軸力センサの説明図、
第４図および第５図は本発明の一実施例のフロー
チヤート図である。 １……ロボツト、２……力センサ、３……初期
ドリフト決定手段、４……較正用測定点力センサ
出力測定手段、５……ドリフト除去手段、６……
ロボツト動作制御手段、７……記憶素子、８……
制御装置、９……６軸力センサ、１０……ハン
ド。

Claims (1)

1. 【特許請求の範囲】 １ ロボツトと、前記ロボツトに取り付けられた
   力センサと、制御装置とから構成される力センサ
   付きロボツトシステムにおいて、予め、力センサ
   の正確なドリフトを求めて初期ドリフトとして制
   御装置内の記憶素子に格納し、同時に、作業中の
   ロボツトが動作または停止する一点または複数点
   を較正用測定点として選び、作業中と同一の負荷
   条件でロボツトを動作または停止させて前記較正
   用測定点における力センサ出力を測定して制御装
   置内の記憶素子に格納した後、実際のロボツトの
   作業中、前記較正用測定点にロボツトが動作また
   は停止する際に前記較正用測定点における力セン
   サ出力を測定し、この力センサ出力と前記記憶素
   子に格納された力センサ出力の差をドリフト変化
   として前記素子に格納更新し、力の検出を行う時
   に、その時点の力センサ出力から前記初期ドリフ
   トおよひ前記ドリフト変化を差し引くことによ
   り、時間変化するドリフトを力センサ出力から除
   去することを特徴とするロボツトの力センサドリ
   フト補償方法。 ２ ロボツトが動作許容範囲内でハンド姿勢を任
   意に指定して動作させることのできるものであ
   り、力センサが前記ロボツトの先端とハンドの間
   に取り付けられたものであり、初期ドリフトはロ
   ボツトに複数のハンド姿勢をとらせ、前記各ハン
   ド姿勢における力センサ出力を測定し、その出力
   値をもとに計算することを特徴とする特許請求の
   範囲第１項記載のロボツトの力センサドリフト補
   償方法。