JP5941083B2 - 外部環境との接触を検知するロボット制御装置 - Google Patents

Description

本発明は、外部環境との接触を検知する、特に産業用ロボットのための制御装置に関する。

外部からの力がロボットアームに加わるのを検出するために、力センサがロボットアームに備えられている。特許文献１においては、外部環境に接触しない状態でロボットアームが動作したときに力センサに作用する内力を推定している。ロボットアームが外部環境に接触したときには、力センサの出力から内力の推定値を減算して、その偏差を接触力として求めている。そして、偏差に基づいて、ロボットアームが外部環境に接触したか否かを判定している。ロボットアームが外部環境に接触した場合には、ロボットアームを外部環境から接触回避させたり、ロボットアームを停止させ、それにより、ロボットの安全な動作を達成している。

特開2006-21287号公報

ところで、図４Ａは、一つの姿勢におけるロボットと線条体との関係を示す図である。図４Ａに示されるように、天井から吊られた線条体１０９が固定具１０８によりロボット１のロボットアーム１０１の複数の箇所に取付けられている。そして、図４Ｂに示されるように、ロボットアーム１０１がその姿勢を変えるように動作する場合がある。

このようにロボットアーム１０１が姿勢を変えるよう動作するときには、ロボットアーム１０１などに取付けられた線条体１０９が引張られることによる力が生じる。そして、ロボット１の下方に取付けられた力センサ１０３は、このような線条体１０９の引張に起因する力を検出する。また、通常、線条体１０９はロボットシステムの内容に応じて取付けられるので、ロボットシステム毎に線条体１０９の種類および数が異なる。このため、ロボットアーム１０１の構成から推定できる内力とは異なり、線条体１０９の引張に起因する力を事前に推定するのは不可能である。

それゆえ、推定する内力に線条体１０９の引張に起因する力を含むことができないため、ロボットアームが外部環境に接触したか否かを正確に判定することはできない。このため、ロボットアーム１０１が外部環境に接触していない場合であっても、ロボットアーム１０１が外部環境に接触したと判断したり、また、ロボットアーム１０１が外部環境に接触してる場合であっても、ロボットアーム１０１が外部環境に接触していないと判断することもある。

従って、線条体１０９の引張に起因する力を補正する必要があり、そのために以下の方法が考えられる。はじめに、実空間を複数の小領域に分割し、分割されたそれぞれの小領域において、小領域内にロボットアーム１０１の先端部が含まれるときの前述した力を測定して力補正量として記憶する。そして、内力の推定値と、ロボットアーム１０１の先端部の現在の位置を含む領域に対応する力補正量とを、力センサの出力から減算して、接触力としての偏差を求める。

しかしながら、ロボットアーム１０１の先端部の位置が同一の領域に在る場合であっても、ロボットアーム１０１の姿勢が異なると、力補正量が適切でない事態が生じうる。例えば、図４Ａと図４Ｂとにおいてロボットアーム１０１の先端部の位置は同一である。しかしながら、ロボットのそれぞれの関節の角度が異なるので、ロボットの姿勢は図４Ａと図４Ｂとで異なる。

このため、図４Ａおよび図４Ｂにおいては、ロボットアーム１０１が線条体を引張る力の大きさや、そのような力の方向が互いに異なる。このような場合にロボットアーム１０１の先端部の位置のみに応じて力補正量を決定したとしても、前述した力の大きさの違いおよび力の方向の違いまで補正することはできない。

本発明はこのような事情に鑑みてなされたものであり、ロボットアームなどに取付けられた線条体が引張られることにより力が生じる場合であっても、そのような力の影響を受けることなしに、外部環境との接触を高精度で検知することのできるロボット制御装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、力センサが取付けられた多関節ロボットを制御するロボット制御装置において、前記ロボットのロボットアームおよび該ロボットアームの先端に取付けられたハンドのそれぞれの自重と、前記ロボットアームおよび前記ハンドのそれぞれの動作により生じる慣性力とに基づいて、前記力センサの取付け箇所に生じる力を内力として推定する内力推定部と、前記ロボットアームの複数の関節のそれぞれの回転動作領域を角度に応じて複数の小領域に分割して、それぞれの小領域において、前記ロボットアームが外部環境に接触していない状態における前記力センサの出力から、前記ロボットアームが外部環境に接触していない状態において前記内力推定部により推定された内力を減算して、力補正量として記憶する記憶部と、前記ロボットアームの前記複数の関節の現在の角度に対応する力補正量を前記記憶部から決定する力補正量決定部と、前記力センサの現在の出力から、前記ロボットの動作時において前記内力推定部により推定された内力と、前記力補正量決定部により決定された前記力補正量とを減算して、前記ロボットアームが外部環境に接触する接触力を算出する接触力算出部と、を具備する、ロボット制御装置が提供される。
２番目の発明によれば、１番目の発明において、前記接触力算出部により算出された前記接触力が所定値を超えた場合には、前記ロボットアームを停止させる停止指令部（１６）をさらに具備する。

１番目の発明においては、ロボットアームが外部環境に接触していない状態において力センサの出力から、内力推定部により推定された内力を減算して、力補正量を求めている。この力補正量は、ロボットアームなどに取付けられた線条体がロボットアームの姿勢が変化するときに引張られることにより生じる力を補正する。従って、ロボットアームなどに取付けられた線条体が引張られることにより力が生じる場合であっても、そのような力の影響を受けることなしに、外部環境との接触を高精度で検知することができる。
２番目の発明においては、ロボットアームが外部環境に接触したときの接触力が所定値を超えた場合には、ロボットアームを停止させる。このため、ロボットをより安全に動作させられる。

本発明に基づくロボット制御装置の機能ブロック図である。 記憶部に記憶される力補正量テーブルの一つの例を示す図である。 一つの関節の回転動作領域を示す図である。 一つの姿勢におけるロボットと線条体との関係を示す図である。 他の姿勢における図４Ａと同様な図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボット制御装置の機能ブロック図である。図１に示されるように、複数の関節を備えたロボット１、例えば六軸垂直多関節ロボットがロボット制御装置１０に接続されている。ロボット１のロボットアーム１０１には六個の関節が設けられており、ロボットアーム１０１は各関節Ｊ１〜Ｊ６で回動または捩り動作することができる。

図面には示さないものの、各関節Ｊ１〜Ｊ６には、それぞれ、モータ、減速機、回転角検出器などが設置され、ロボット制御装置１０からの指令で各関節Ｊ１〜Ｊ６の回転角度を制御している。また、ロボットアーム１０１の先端にはハンド１０７が設置されており、ワークを把持する。

図１に示されるように、天井から吊られた線条体１０９がロボット１のハンド１０７まで延びている。線条体１０９は、ロボットアーム１０１の複数箇所において固定具１０８により固定されている。線条体１０９は、ハンド１０７を駆動するために、動力または信号を供給するためのケーブル、および／もしくは冷却水または駆動用エアを供給するチューブを含む。なお、ハンド１０７の代わりに、溶接トーチなどがロボットアーム１０１の先端に取付けられていてもよい。

ロボットアーム１０１の根元部にはロボットベース１０２が設置されている。さらに、力センサ１０３がロボットベース１０２の下方に設置されている。力センサ１０３はＸ方向、Ｙ方向、およびＺ方向の三成分の力をそれぞれ検出することができる。ロボットベース１０２よりも上方に配置されたロボットアーム１０１に加わる力が変化した場合には、力センサ１０３の出力が変化する。

ここでロボットアーム１０１に加わる力は、ロボットアーム１０１が外部環境に接触してロボットアーム１０１に加えられる接触力と、ロボットアーム１０１自体が動作することによって生じる力（以下、「内力」と呼称する）と、およびロボットアーム１０１が姿勢を変えるよう動作するときに線条体１０９が引張られることにより生じる力とを含む。

なお、力センサ１０３はロボットアーム１０１に内蔵されていてもよい。この場合におけるロボットアーム１０１に加わる力のうちの接触力は、力センサ１０３の内蔵箇所からロボットアーム１０１の先端までの部分が外部環境に接触して、その部分に加えられる力を意味する。

ロボット制御装置１０はデジタルコンピュータであり、ロボット１の動作を制御する。図１に示されるように、ロボット制御装置１０は、ロボット１の動作プログラムなどを含む動作計画部１７と、動作計画部１７に基づいてロボット１の動作指令を作成する動作指令作成部１８とを含んでいる。

さらに、ロボット制御装置１０は、ロボットアーム１０１が外部環境に接触したときの接触力をロボット１の所定の制御周期毎に算出する接触力算出部１５を含んでいる。図２に示されるように、接触力算出部１５は、前述した内力を推定する内力推定部１１と、力補正量テーブルを記憶する記憶部１２と、力補正量テーブルに基づいて力補正量を決定する力補正量決定部１３とを含んでいる。

内力推定部１１は、ロボットアーム１０１が外部環境に接触していないときの、ロボットアーム１０１およびハンド１０７のそれぞれの自重とそれぞれの動作による慣性力とによって力センサ１０３の取付箇所に作用する力を内力として公知の手法で推定する。内力を推定する際に、内力推定部１１は、回転角検出器が検出したモータの回転角、動作計画部１７におけるモータの回転角指令値等を適宜利用するものとする。内力推定部１１により推定されたＸ方向、Ｙ方向、およびＺ方向における内力をそれぞれ、ＲＦＸ、ＲＦＹ、ＲＦＺと呼ぶ。

図２は記憶部に記憶された力補正量テーブルの一つの例を示す図である。図２に示されるように、力補正量テーブルにおいては、ロボットアーム１０１の複数の関節Ｊ１〜Ｊ６のうちの一つの関節、例えば関節Ｊ１に関し、関節Ｊ１の回転動作領域をｎ個の小領域Ｊ１１〜Ｊ１ｎに分割している。

図３は一つの関節の回転動作領域を示す図である。図３に示される中心は関節、例えば関節Ｊ１の中心軸を示している。関節Ｊ１の回転動作領域は３６０°である。図３においては、回転動作領域を関節Ｊ１回りの角度に応じて等間隔でｎ個の小領域Ｊ１１〜Ｊ１ｎに分割している。図面には示さないものの、他の関節Ｊ２〜Ｊ６についてもｎ個の小領域を同様に設定する。

そして、図２を再び参照すると、関節Ｊ２〜関節Ｊ６をそれぞれの第一の小領域Ｊ２１〜Ｊ６１に固定しつつ、関節Ｊ１を小領域Ｊ１１〜Ｊ１ｎに割振っている。そして、図２においては、関節Ｊ１の小領域Ｊ１１〜Ｊ１ｎのいずれかと、関節Ｊ２〜関節Ｊ６のそれぞれの第一の小領域Ｊ２１〜Ｊ６１との組み合わせについて、力補正量が設定されている。図２から分かるように、Ｘ方向、Ｙ方向、およびＺ方向について力補正量ΔＦＸ、ΔＦＹ、ΔＦＺが設定されている。

力補正量は、ロボットアーム１０１が外部環境に接触しない状態において、内力推定部１１により推定された内力を力センサ１０３の出力から減算することにより求められる。具体的には、ロボット１の各関節Ｊ１〜Ｊ６が図２に示される小領域になるような姿勢にロボット１を動作させる。このとき、ロボットアーム１０１が外部環境に接触しないことが必要とされる。

そして、そのような姿勢において、Ｘ方向、Ｙ方向、およびＺ方向における力センサ１０３の出力ＳＦＸ、ＳＦＹ、ＳＦＺを読取る。次いで、以下の式（１）〜（３）に示されるように、力センサ１０３の出力ＳＦＸ、ＳＦＹ、ＳＦＺから内力推定部１１により推定された内力ＲＦＸ、ＲＦＹ、ＲＦＺを減算して力補正量ΔＦＸ、ΔＦＹ、ΔＦＺを算出する。
ΔＦＸ＝ＳＦＸ−ＲＦＸ （１）
ΔＦＹ＝ＳＦＹ−ＲＦＹ （２）
ΔＦＺ＝ＳＦＺ−ＲＦＺ （３）

このようにして算出される力補正量は、ロボットアーム１０１などに取付けられた線条体１０９がロボットアーム１０１の姿勢が変化するときに引張られることにより生じる力を補正する。

図２に示される力補正量テーブルにおいては関節Ｊ２〜関節Ｊ６をそれぞれの第一の小領域Ｊ２１〜Ｊ６１に固定しつつ、関節Ｊ１を小領域Ｊ１１〜Ｊ１ｎに割振っている。次いで、関節Ｊ２〜関節Ｊ６をそれぞれの第二の小領域Ｊ２２〜Ｊ６２に固定した状態で、同様に力補正量を求める。そのような作業を順次繰返し、関節Ｊ１〜Ｊ２の全ての小領域についての組み合わせが網羅されるよう、力補正量テーブルを作成する（以下の式（４）を参照されたい）。力補正量は一般的な補間方法で作成してもよい。

なお、回転動作領域を必ずしも等間隔で分割する必要はない。例えば、回転動作領域のうち力補正量の変化が大きい場所では、小さい角度毎に等間隔で小領域を設定し、力補正量の変化が小さい場所では、大きい角度毎に残りの小領域を設定してもよい。この場合には、詳細に後述する補正の効果を維持しつつ、記憶部１２の記憶容量を抑えることが可能となる。

また、力補正量に影響の少ない要素は図２の力補正量テーブルから排除してもよい。例えば、関節Ｊ４〜Ｊ６の姿勢が力補正量に与える影響が小さいことが明らかである場合には、関節Ｊ４〜Ｊ６の小領域の設定を排除してもよい。この場合にも、補正の効果を維持しつつ、記憶部１２の記憶容量を抑えることができる。

力補正量決定部１３は、ロボット１の動作時に所定の制御周期毎に力補正量ΔＦＸ、ΔＦＹ、ΔＦＺを決定する。具体的には、力補正量決定部１３は、回転角検出器により検出された各関節Ｊ１〜Ｊ６回りの角度を取得し、それら角度に対応する力補正量ΔＦＸ、ΔＦＹ、ΔＦＺを記憶部１２の力補正量テーブルから決定する。

そして、図１から分かるように、接触力算出部１５は力センサ１０３の出力ＳＦＸ、ＳＦＹ、ＳＦＺから力補正量ΔＦＸ、ΔＦＹ、ΔＦＺと内力ＲＦＸ、ＲＦＹ、ＲＦＺとを減算して、接触力ＣＦＸ、ＣＦＹ、ＣＦＺをそれぞれ算出する。
ＣＦＸ＝ＳＦＸ−ΔＦＸ−ＲＦＸ （５）
ＣＦＹ＝ＳＦＹ−ΔＦＹ−ＲＦＹ （６）
ＣＦＺ＝ＳＦＺ−ΔＦＺ−ＲＦＺ （７）

また、ロボット制御装置１０の停止指令部１６は、接触力ＣＦＸ、ＣＦＹ、ＣＦＺの大きさを基準値と比較し、接触力の大きさが基準値よりも大きい場合には、ロボット１の停止指令を作成して、動作指令作成部１８に供給する。動作指令作成部１８は停止指令をロボット１に供給して、ロボット１を停止させる。

なお、停止指令部１６は、接触力ＣＦＸ、ＣＦＹ、ＣＦＺの全てが基準値よりも大きい場合にロボット全体の停止指令を作成してもよく、接触力ＣＦＸ、ＣＦＹ、ＣＦＺのうちの少なくとも一つが基準値よりも大きい場合にロボット全体の停止指令を作成してもよい。あるいは、停止指令部１６は少なくとも一つの接触力、例えば接触力ＣＦＸが基準値よりも大きい場合に、その方向についてのみの停止指令を作成することもできる。また、Ｘ方向、Ｙ方向およびＺ方向について異なる基準値を採用してもよい。また、各方向の接触力を合成した力の大きさが基準値を超える場合に停止指令を作成してもよい。

ロボット１の動作時にロボットアーム１０１が外部環境に接触しない場合には、力センサ１０３の出力から力補正量と内力とを減算すると、接触力はゼロとなる。従って、接触力は基準値よりも大きくならず、それゆえ、動作指令作成部１８は、動作計画部１７に基づいてロボット１に動作指令を供給し続ける。

これに対し、ロボット１の動作時にロボットアーム１０１が外部環境、例えば周辺機器または作業者に接触した場合に、力センサ１０３の出力から力補正量と内力とを減算すると、接触力がゼロでない値になる。そして、接触力の大きさが基準値よりも大きい場合には、停止指令部１６から停止指令が出力され、それにより、ロボット１が停止されるようになる。従って、ロボット１を安全に動作させられる。

このように本発明においては、力センサ１０３の現在の出力から、内力推定部１１により推定された内力と、力補正量決定部１３により決定された力補正量とを減算して、接触力を算出している。そして、力補正量は、ロボットアーム１０１が外部環境に接触していない状態において力センサ１０３の出力から、内力を減算することにより予め求められている。また、力補正量は、ロボット１の各関節Ｊ１〜Ｊ６の角度の組み合わせ毎に定められている。このため、本発明においては、ロボットアーム１０１などに取付けられた線条体１０９が引張られることにより力が生じる場合であっても、そのような力の影響を受けることなしに、外部環境との接触を高精度で検知することが可能となっている。

１ 多関節ロボット
１０ ロボット制御装置
１１ 内力推定部
１２ 記憶部
１３ 力補正量決定部
１５ 接触力算出部
１６ 停止指令部
１７ 動作計画部
１８ 動作指令作成部
１０１ ロボットアーム
１０２ ロボットベース
１０３ 力センサ
１０８ 固定具
１０９ 線条体

Claims (2)

1. 力センサ（１０３）が取付けられた多関節ロボット（１）を制御するロボット制御装置（１０）において、
   前記ロボットのロボットアーム（１０１）および該ロボットアームの先端に取付けられたハンド（１０７）のそれぞれの自重と、前記ロボットアームおよび前記ハンドのそれぞれの動作により生じる慣性力とに基づいて、前記力センサの取付け箇所に生じる力を内力として推定する内力推定部（１１）と、
   前記ロボットアームの複数の関節のそれぞれの回転動作領域を角度に応じて複数の小領域に分割して、それぞれの小領域において、前記ロボットアームが外部環境に接触していない状態における前記力センサの出力から、前記ロボットアームが外部環境に接触していない状態において前記内力推定部により推定された内力を減算して、力補正量として記憶する記憶部（１２）と、
   前記ロボットアームの前記複数の関節の現在の角度に対応する力補正量を前記記憶部から決定する力補正量決定部（１３）と、
   前記力センサの現在の出力から、前記ロボットの動作時において前記内力推定部により推定された内力と、前記力補正量決定部により決定された前記力補正量とを減算して、前記ロボットアームが外部環境に接触する接触力を算出する接触力算出部（１５）と、を具備する、ロボット制御装置。
2. 前記接触力算出部により算出された前記接触力が所定値を超えた場合には、前記ロボットアームを停止させる停止指令部（１６）をさらに具備する請求項１に記載のロボット制御装置。

Images