JP2681035B2 - 力検出補償を行う力制御ロボット - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、鋳物のバリ等を除去す
るときに使用する力制御ロボットに係り、特に工具の重
力成分の力検補償を行う力制御ロボットに関する。

【０００２】

【従来の技術】産業用ロボットの力制御においては、一
般的にはロボット先端と工具の間に６軸力センサを介在
させ、工具に作用する力とモーメントを力センサで検出
し、この力とモーメントの検出値をもとにロボットの力
制御を行なっている。前記した力センサが検出する力と
モーメントは、加工による負荷（以下負荷という）に工
具重量が重畳されたものの結果として検出される。従っ
て、精度の高い力制御を行おうとするときは、前記した
ような負荷と工具重量との重畳されたものの結果から工
具重量による力およびモーメントを除去したもの（以下
力検出補償という）で力制御を行う必要がある。特に工
具重量が負荷に対して無視できないほどの大きさになる
と、ことさら必要となる。特願昭６２−７４５９４号公
報には、ロボット装置の力センサ構成方法として次のよ
うに記載されている。即ち、ハンドに重力のみがかかる
状態で、ロボットに複数のハンド姿勢を取らせ、各ハン
ド姿勢において力センサ出力を測定する。そしてハンド
の重量がＷで、ハンドの重心位置がハンド座標系のＧｈ
で表され、ドリフトをＤとして、全て未知数とし、これ
を前記した各ハンド姿勢の出力値を基にして計算により
求めて、これを記憶しておく。ロボットが作業するとき
に、操作対象物とハンドとの間に作用する力を検出、測
定するときは、記憶されたＷ，Ｇｈ，Ｄを用いて計算を
行うことにより、ハンド重力の寄与と力センサのドリフ
トが除去された力・モーメントを検出することができ
る。

【０００３】

【発明が解決しようとする課題】しかし、上記した先行
技術においては、次のような問題点があった。即ち、
（１）記載はしてないが、零点調整は常に所定の位置で
行うものと推定されるが、ロボットが制御情報として認
識している力センサ姿勢と実際の力センサ姿勢とは偏差
をもっており、零点調整した時からの力センサの姿勢変
化が大きくなるに従って累積誤差が大きくなる、（２）
ハンド姿勢を複数回変えての力センサの出力を測定し
て、その結果により工具の重量および重心位置を計算し
て求めるため、工具を交換する毎に前記した操作を行わ
ねばならない、というものである。本発明は、上記した
問題点を改善して、取扱いが容易で、しかも誤差が小さ
く精度の高い力制御を行うことのできる力検出補償を行
う力制御ロボットを提供することを目的とする。

【０００４】

【課題を解決するための手段】本発明は力検出補償を行
う力制御ロボットにおいて、ロボットの先端部に力セン
サと工具を備え、力センサからの力およびモーメントの
検出値を入力する力・モーメント入力手段と、予め、所
定工具の重量と重心位置とを記憶する重量記憶手段と、
ロボット基準座標に対する力センサの姿勢を入力する力
センサ姿勢入力手段と、任意姿勢の力センサの検出値を
零にする指令をするバイアス指令手段と、バイアス指令
手段の指令時のみの力センサ姿勢入力手段からの力セン
サ姿勢をバイアス姿勢として記憶するバイアス姿勢記憶
手段と、時々刻々の力センサ姿勢入力手段からの力セン
サ姿勢を現在姿勢として記憶する現在姿勢記憶手段と、
バイアス姿勢記憶手段からのバイアス姿勢を基準とし
て、現在姿勢記憶手段からの現在姿勢の姿勢変化を演算
する姿勢変化演算手段と、姿勢変化演算手段からの姿勢
変化と重量記憶手段からの所定工具の重量とで工具の重
力成分による重力補償値を演算する重力補償値演算手段
と、力・モーメント入力手段の検出値から、重力補償値
演算手段からの重力補償値を差し引く演算を行う重力補
償手段とを有し、重力補償手段にて演算した力・モーメ
ントによって力制御を行うことを特徴とするものであ
る。

【０００５】

【作用】本発明での力制御ロボットの力検出補償方法の
概要は、ｆｒ＝ｆｓ−ｆｗ（θ）である。ここでｆｒは
正味の外部負荷、ｆｓは力センサ出力値、ｆｗ（θ）は
重力補償項であり、（θ）はバイアス姿勢からの力セン
サの姿勢変化を示している。実際の外部負荷Ｆが作用し
ていない場合を説明する。まず、力センサの出力値ｆｓ
を任意の姿勢で零点調整する。この時、力センサの出力
値ｆｓ＝０となる。同時に、零点調整した時点のロボッ
ト基準座標からの力センサ姿勢を記憶装置にバイアス姿
勢（θｂ）として記憶する。次に、零点調整直後の現在
の力センサ姿勢（θｃ）はバイアス姿勢（θｂ）と等し
いので力センサの姿勢変化（θ）は零であるため、手先
効果器の重量Ｗが存在しても重力補償項はｆｗ（θ）＝
０となる。この場合、正味外部負荷ｆｒ＝ｆｓ−ｆｗ
（θ）＝０となり、前提条件である外部負荷Ｆが作用し
ていないことと一致する。

【０００６】次に、力センサの姿勢がバイアス姿勢から
（θ）だけ変化した場合を説明する。 まず、姿勢が変
化したので手先効果器の重量Ｗによって力センサ出力値
ｆｓは零とならず、バイアス姿勢（θｂ）からの姿勢変
化（θ）に比例したｆｓ（θ）となる。ここでｆｓ
（θ）は力センサ姿勢が（θ）だけ変化したときの、力
センサの出力値を示す。従ってｆｓ（θ）＝ｆｓであ
り、この値は０ではない。一方、力センサの姿勢変化
（θ）は、｛バイアス姿勢（θｂ）｝−｛力センサ姿勢
（θｃ）｝≠０であるため、手先効果器の重量Ｗとの演
算で重力補償項ｆｗ（θ）≠０となり、演算上の正味外
部負荷ｆｒ＝ｆｓ−ｆｗ（θ）＝０となり、前提条件の
正味の外部負荷Ｆは作用していないことと一致する。

【０００７】次に、力センサに外部負荷Ｆが作用した場
合を説明する。この時は、外部負荷Ｆが作用し、正味の
外部負荷ｆｒは０でなくなる。一方、姿勢変化で手先効
果器の重量Ｗによる力センサの力・モーメントの出力値
はｆｓ（θ）≠０で、同時に外部負荷Ｆが作用している
ので、ｆｓ（Ｆ）≠０で、ｆｓ＝｛外部負荷ｆｓ
（Ｆ）｝＋｛姿勢変化（θ）に比例したｆｓ（θ）｝≠
０となる。姿勢変化後の現在の力センサ姿勢（θｃ）は
前記したバイアス姿勢（θｂ）と比較すると等しくな
く、力センサの姿勢変化は｛バイアス姿勢（θｂ）｝−
｛力センサ姿勢（θｃ）｝≠０となり、手先効果器の重
量Ｗとの演算で重力補償項ｆｗ（θ）≠０となり、前記
同様ｆｓ（θ）−ｆｗ（θ）＝０であるため、ｆｒ＝ｆ
ｓ−ｆｗ（θ）＝｛ｆｓ（Ｆ）＋ｆｓ（θ）｝−ｆｗ
（θ）＝ｆｓ（Ｆ）≠０となり、前提条件の正味の外部
負荷ｆｒは、ｆｒ＝ｆｓ（Ｆ）≠０となり、外部負荷の
作用状況を把握できる。

【０００８】

【実施例】本発明の実施例を図１、図２、図３に基づい
て説明する。図３は、本発明に供する力制御ロボットの
システムの構成を示す。図３において、１はロボット、
２はロボット１の手先先端に取り付けた力センサ、３は
力センサ２に取り付けた手先効果器であって、本例の場
合工具３Ｂと工具ホルダ３Ａである。４はロボット１
と、力センサ２と電気的に接続したロボット制御盤であ
る。５はロボット基部に設定したロボット基準座標で静
止座標である。６は力センサ２に設定した力センサ座標
で、ここでは説明を容易にするため手先座標と同一の座
標とする（同一である必要はない）。７は手先効果器の
作業点であってＴＣＰ（Tool Center Point）で、８は
ＴＣＰ７に設定した工具座標である。９は力制御対象ワ
ーク（以下、単にワークと言う）である。

【０００９】図１は、本発明の力制御ロボットの力検出
補償方法の構成を説明するための制御盤４内に設けたマ
イクロコンピュータ等のシステム（図示せず）での機能
ブロック図である。図１において、１０は力センサ２へ
零点調整３１を行わせるバイアス指令手段である。１１
は通常ロボット１が制御している工具座標８から変換で
きる手先座標（ここでは力センサ座標６に同じ）の位置
と姿勢からロボット基準座標５と力センサ座標６との関
係を示す力センサ２の姿勢入力手段である。１５は力セ
ンサ２が検出している力・モーメント３５の入力手段で
ある。１６は予め通常のＭＤＩ等でのデータ設定による
入力手段で風袋重量、重心位置を記憶する自重・重心記
憶手段である。

【００１０】１２は力センサ姿勢入力手段１１から得ら
れた力センサ２の姿勢３２を記憶するバイアス姿勢記憶
手段である。１３は力センサ姿勢入力手段１１から得ら
れた力センサ２の現在の姿勢３３を記憶する現在姿勢記
憶手段である。

【００１１】１４はバイアス姿勢記憶手段１２の力セン
サ２のバイアス姿勢３４から現在姿勢記憶手段１３の力
センサ２の現在姿勢４０を差し引く姿勢変化演算手段で
ある。１７は、自重・重心記憶手段１６の自重または重
心位置４１と姿勢変化演算手段１４の姿勢変化分３９を
演算し、重力補償値３８を求める重力補償演算手段であ
る。１８は力・モーメントの入力手段１５の力・モーメ
ント３７から重力補償値演算手段１７の重力補償値３８
を差し引き、正味の外部負荷３６を求める重力補償演算
手段である。１９はこの一連の重力補償を行う重力補償
手段である。

【００１２】図３において、１２Ａは力センサ座標６の
力制御開始直前のバイアス姿勢で、１３Ａは力センサ座
標６の力制御開始の現在姿勢で、１３Ｂは力センサ座標
６の力制御終了の現在姿勢である。１３Ｘは力センサ座
標６の力制御姿勢の１３Ａから１３Ｂの区間の平均的姿
勢である。

【００１３】次に、上記構成の重力補償手段１９を用い
て力制御ロボットの力検出の補償を行う方法について説
明する。図２は、本発明の力制御ロボットの力検出補償
方法の作用を説明するためのフローチャートである。ま
ず、予め教示してあるロボット１の動作プログラム３０
（手先動作位置・姿勢、動作速度等）に従って、ロボッ
ト１を動作させる。この動作プログラム３０には、ワー
ク９に手先効果器３の工具３ＢのＴＣＰ７が接触して力
制御する区間を開始姿勢１３Ａから終了姿勢１３Ｂへ動
作するようにしてあるものとする。また、工具３ＢのＴ
ＣＰ７がワーク９と接触していない状態、すなわち、力
センサ座標６に外部負荷Ｆが作用していない力制御開始
直前の位置Ｐ１でバイアス姿勢１２Ａを教示しておく。
従ってロボット１の動作はバイアス調整位置（Ｐ１）１
２Ａから力制御開始位置（Ｐ２）１３Ａ、力制御終了位
置（Ｐ３）１３Ｂへと動作することになる。このバイア
ス姿勢１２Ａは、実際の力センサ座標６とロボット１が
制御情報として認識している内部力センサ座標６との姿
勢誤差による力制御誤差を小さくするため、力制御開始
姿勢１３Ａから力制御終了姿勢１３Ｂの間の平均的姿勢
１３Ｘと同等の姿勢とした方が好ましい。また、予め通
常のＭＤＩ等でのデータ設定による入力手段で風袋重量
Ｗ、その重心位置（ｇｘ、ｇｙ、ｇｚ）４１を入力設定
し、自重・重心記憶手段１６に記憶しておく。

【００１４】ロボット制御盤４からコントロールされて
ロボット１が動作を開始すると、まず、ロボット１がバ
イアス位置（Ｐ１）１２Ａに位置決めされる。

【００１５】次に力センサ２をバイアス調整するため、
バイアス指令手段１０を介し、バイアス指令３１が出力
される。

【００１６】これが実行されると、手先効果器３の重力
・ドリフト等を含め、力センサ２の力・モーメント検出
値３５は零となり零点調整が行われる。従って、力・モ
ーメントの入力手段１５への入力値ｆｓ３５は、ｆｓ＝
（ｆｘｓ、ｆｙｓ、ｆｚｓ、ｍｘｓ、ｍｙｓ、ｍｚｓ）
T＝０で全て零となる。

【００１７】同時に、ロボット基準座標５と力センサ座
標６との関係を示す力センサ２の姿勢を姿勢入力手段１
１で得、バイアス時の力センサ２のバイアス姿勢３２を
記憶するバイアス姿勢記憶手段１２へ格納する。バイア
ス姿勢（θｂ）３２は次式で表される。

【数１】

【００１８】次に所定のサンプリングタイム毎に、ロボ
ット基準座標５と力センサ座標６との関係を示す力セン
サ２の現在の姿勢を姿勢入力手段１１から得、現在の力
センサ２の現在姿勢を３３記憶する現在姿勢記憶手段１
３へ格納する。現在姿勢（θｃ）３３は次式で表され
る。

【数２】

【００１９】そして、重力補償を行う重力補償手段１９
で一連の演算を行い、外部から作用する外部負荷Ｆすな
わち重力補償した正味の力・モーメント３６を監視し、
力制御にリアルタイムのデータとして供するのである。
この演算式はｆｒ＝ｆｓ−ｆｗ（θ）である。ここでｆ
ｒは正味の力・モーメント３６で、ｆｒ＝（ｆｘｒ、ｆ
ｙｒ、ｆｚｒ、ｍｘｒ、ｍｙｒ、ｍｚｒ）Tである。ｆ
ｓは力センサ出力３７で、ｆｓ＝（ｆｘｓ、ｆｙｓ、ｆ
ｚｓ、ｍｘｓ、ｍｙｓ、ｍｚｓ）Tである。ｆｗ（θ）
は重力補償項３８で、（θ）はバイアス姿勢３４からの
力センサの姿勢変化３９を示している。重力補償のため
の重量Ｗに対する姿勢変化は重力方向のみであるから、
次式で示される。

【数３】 従って、次式で正味の力とモーメントｆｒを求める。

【数４】

【００２０】まず、バイアス直後を説明する。力センサ
２の姿勢はバイアス姿勢１２Ａのままで、バイアス直後
のため力・モーメント３５の入力手段１５への入力値３
７は、ｆｓ＝（ｆｘｓ、ｆｙｓ、ｆｚｓ、ｍｘｓ、ｍｙ
ｓ、ｍｚｓ）T＝０で、全て零となる。また同時に、バ
イアス直後のため現在の力センサ２の現在姿勢３３は、
（θｃ）＝（ｎｃ ｏｃ ａｃ）となり、バイアス姿勢
１２Ａの（θｂ）＝（ｎｂ ｏｂ ａｂ）のバイアス姿
勢３２と等しく、重力に関するバイアス姿勢３４と現在
姿勢４０との差は、ｎｚｂ−ｎｚｃ＝ｏｚｂ−ｏｚｃ＝
ａｚｂ−ａｚｃ＝０である。 従って、式４で右辺の第
１項の力センサ検出値３７、第２項の重力補償項３８
は、ともに零で、正味の外部負荷３６は、ｆｒ＝（ｆｘ
ｒ、ｆｙｒ、ｆｚｒ、ｍｘｒ、ｍｙｒ、ｍｚｒ）T＝０
で全て零となる。これは、力センサ座標６にＴＣＰ７か
らの外部負荷Ｆが作用せず力制御開始直前の位置Ｐ１で
バイアス姿勢１２Ａを教示して置いたため、外部負荷Ｆ
＝０と一致し、正しい結果が得られた。

【００２１】次に、力制御開始姿勢１３Ａに行くまでの
状況を説明する。まず、バイアス姿勢（θｂ）１２Ａと
力制御する開始姿勢１３Ａの力センサ２の姿勢は違うも
のとする。従って、力センサ２の姿勢はバイアス姿勢
（θｂ）１２Ａから時々刻々変化し、最後に現在の力セ
ンサ２の姿勢（θｃ）１３Ａに等しくなる。任意の位置
での現在の力センサ２の姿勢（θｃ）１３Ａは、前述の
現在姿勢３３の（θｃ）＝（ｎｃ ｏｃ ａｃ）で表さ
れる。

【００２２】バイアス姿勢（θｂ）１２Ａも同様に、前
述のバイアス姿勢３２の（θｂ）＝（ｎｂ ｏｂ ａ
ｂ）で表される。（θｃ）≠（θｂ）であるのでバイア
ス姿勢１２Ａからの重力方向の姿勢変化（θ）は次式で
表せる。 ｎｚｂ−ｎｚｃ＝ｄｎｚ≠０ ｏｚｂ−ｏｚｃ＝ｄｏｚ≠０ ａｚｂ−ａｚｃ＝ｄａｚ≠０

【００２３】重力補償項３８のｆｗ（θ）は次式で表さ
れる。

【数５】

【００２４】一方、力センサ２の姿勢（θ）が変化した
ので手先効果器３の重量Ｗによって力センサ２の力・モ
ーメント検出値３５は零とならず、バイアス姿勢１２Ａ
からの姿勢変化（θ）に比例し、力・モーメント検出値
３７はｆｓ＝ｆｓ（θ）≠０で次式で示す出力が得られ
る。

【数６】 従って、力センサ２の力・モーメント検出値３７のｆｓ
＝ｆｓ（θ）は、重力補償項３８のｆｗ（θ）と同一と
なる。

【００２５】このように、力センサ２の姿勢（θ）が変
化しても手先効果器の自重Ｗによる力センサ２の力・モ
ーメント検出値３７のｆｓ（θ）は重力補償項３８のｆ
ｗ（θ）で補償され、力センサ２の姿勢がバイアス姿勢
（θｂ）１２Ａから力制御開始姿勢１３Ａになるまで正
味の外部負荷３６はｆｒ＝ｆｓ−ｆｗ（θ）＝０と無負
荷の状況を示すのである。

【００２６】次に、力センサ２の姿勢が力制御開始姿勢
１３Ａとなる前後において、ワーク９に手先効果器３の
工具３ＢのＴＣＰ７が接触し始めるとワーク９から外部
負荷Ｆで力・モーメントを受けるようになる。力センサ
２がバイアス姿勢（θｂ）から力制御開始姿勢１３Ａに
なるまでの力・モーメント検出値ｆｓ（θ）に外部負荷
ｆｓ（ｒ）が複合し、力・モーメント検出値３７はｆｓ
＝ｆｓ（θ）＋ｆｓ（ｒ）となり、次式で示される。

【数７】 これを数４で、重力補償すると力センサ２の姿勢（θ）
が変化しても手先効果器３の自重Ｗによる力センサの力
・モーメント検出値３７のｆｓ（θ）は前述のとおり重
力補償項３８のｆｗ（θ）で補償され、正味の外部負荷
３６のｆｒはｆｒ＝ｆｓ（ｒ）＋ｆｓ（θ）−ｆｗ
（θ）＝ｆｓ（ｒ）となり、正しい正味の外部負荷Ｆに
よるのみの結果が得られる。

【００２７】こうして、ロボット１がＰ１（姿勢１２
Ａ）からＰ２（姿勢１３Ａ）へ移動していくと、ＴＣＰ
７とワーク９が接触し、力制御のために予め設定した力
・モーメントｆに正味の外部負荷ｆｒが到達するとワー
ク９への接触動作を終了し、次の力センサ座標６の力制
御終了の現在姿勢１３Ｂへ、力制御のために予め設定し
た力・モーメントｆに正味の外部負荷ｆｒが追従するよ
うにロボット１が力制御されながら移動して所望の力制
御が行われる。この結果、ロボットの動作はバイアス調
整位置１２Ａから力制御開始位置１３Ａ、力制御終了位
置１３Ｂへと移動して行くのである。この時、実際に力
制御される力・モーメントｆは、ＴＣＰ７における工具
座標８におけるものであるが、力センサ座標６と工具座
標８との関係は座標変換により可能である。

【００２８】

【発明の効果】以上説明したように、本発明の力制御ロ
ボットの力検出補償方法よれば、バイアス調整したバイ
アス姿勢からの力センサの姿勢変化に応じて、力センサ
の姿勢が変化しても手先効果器の自重Ｗによる力センサ
の力・モーメント検出値ｆｓ（θ）を重力補償項ｆｗ
（θ）で補償するようにしたため、正味の外部負荷ｆｒ
＝ｆｓ（ｒ）＋ｆｓ（θ）−ｆｗ（θ）＝ｆｓ（ｒ）と
なり、正しい正味の外部負荷のみの結果が得られた。本
発明によって、力センサを任意の姿勢でバイアス調整で
き、外部負荷による正味の力とモーメントを得ることで
きるようになり、バイアス調整のためにロボット基準座
標と力センサの座標を重力方向で一致させる余分なロボ
ット動作が無くなり、所定作業のサイクルが伸びない。
また、力制御途中の力センサ姿勢とほぼ同一姿勢で、力
制御開始直前にバイアス調整すれば姿勢変化｛バイアス
姿勢（θｂ）−力センサ姿勢（θｃ）｝は小さく手先効
果器の重量との演算で重力補償項ｆｗ（θ）が小さくな
る。この結果、実際の力センサ座標とロボット１が制御
情報で認識している内部力センサ座標との姿勢誤差に起
因する力制御精度の低下を防ぎ、基本機能である力制御
の信頼性を高めることができる。

【図面の簡単な説明】

【図１】本発明の力制御ロボット実施するための構成を
説明するための機能ブロック図

【図２】本発明を用いてロボットの運転時の作用を説明
するための図

【図３】本発明を実施するための力制御ロボットのシス
テムの構成を示す図

【符号の説明】

１ ロボット ２ 力センサ ３ 手先効果器 ５ ロボット基準座標 ６ センサ座標 １０ バイアス指令手段 １１ 力センサ姿勢入力手段 １２ バイアス姿勢記憶手段 １３ 現在姿勢記憶手段 １４ 姿勢変化演算手段 １５ 力・モーメント入力手段 １６ 自重・重心記憶手段 １７ 重力補償値演算手段 １８ 重力補償演算手段 １９ 重力補償手段

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 ロボットの先端部に力センサと工具を備
   え、 力センサからの力およびモーメントの検出値を入力する
   力・モーメント入力手段と、予め所定工具の重量と重心
   位置とを入力する重量・重心位置記憶手段と、 ロボットの基準座標に対する力センサの姿勢を入力する
   力センサ姿勢入力手段と、所定の力制御作業に対しする
   力センサの基準とするべく力センサの検出値を零にする
   ようにバイアス指令するバイアス指令手段と、 バイアス指令時の力センサ姿勢である力センサバイアス
   姿勢を記憶するバイアス姿勢記憶手段と、 所定の時刻の力センサ姿勢を現在姿勢として記憶する現
   在姿勢記憶手段と、 力センサバイアス姿勢を基準とした現在姿勢までの姿勢
   変化を演算する姿勢変化演算手段と、 前記姿勢変化と重量記憶手段からの所定工具の重量と重
   心とで工具の重量成分による重力補償値を演算する重力
   補償値演算手段と、 力・モーメント入力手段の検出値から重力補償値演算手
   段からの重力補償値を差し引く演算を行う重力補償演算
   手段とを有し、 重力補償演算手段にて演算した力・モーメントによって
   力制御を行うことを特徴とする力検出補償を行う力制御
   ロボット。