JP5633623B2 - ロボットシステム - Google Patents

Description

本発明は、ロボットに対する衝突をセンサレスで検出する手段を備えたロボットシステムに関する。

ロボットに対する衝突を検出する方法としては、各種センサを用いて行う方法や、各種センサを用いずに（センサレスで）ソフトウェアにより行う方法がある。このうち、センサレスでロボットに対する衝突を検出する方法としては、例えば特許文献１に開示されている方法が挙げられる。この特許文献１では、外乱オブザーバを使ってロボットの各軸に外部から加わる負荷（外乱負荷トルク）をそれぞれ求め、いずれか１つの軸に加わる外乱負荷トルクの値が予め定めたしきい値を超えると衝突が発生したと判断するようになっている。

特許第３２１２５７１号公報

特許文献１記載の上記方法によれば、ロボットに対する衝突の有無を検出することは可能である。しかしながら、特許文献１記載の方法では、例えば軸と軸の間に存在するリンクに対する衝突が生じた場合、その衝突がどのリンクに対するものであるのかなど、衝突箇所を特定することはできない。また、特許文献１記載の方法では、外乱オブザーバによる演算誤差（モデル値と現実値との差に起因した誤差）の影響により衝突の検出感度を十分に高めることができない。すなわち、特許文献１記載の方法では、衝突の検出感度を高めるためにしきい値を下げると、上記演算誤差が衝突の判定に与える影響が大きくなり、衝突を誤検出する可能性が高くなるため、衝突の検出感度を十分に高めることができなかった。

本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットに設けられる軸と軸の間に存在するリンクに対する衝突が生じた場合にその衝突箇所を特定できるとともに、衝突の誤検出を抑制しつつ検出感度を高めることができるロボットシステムを提供することにある。

請求項１記載の手段によれば、駆動制御手段は、外部から与えられる指令値に従って、ロボットに設けられた複数の軸を駆動するための複数のモータの駆動を制御する。そして、トルク値推定手段は、指令値を用いて複数の軸のうちの検出対象軸に生じる推定トルク値を推定する。また、トルク値演算手段は、モータに流れる電流を検出する電流検出手段の検出値を用いて上記検出対象軸に生じる発生トルク値を演算する。外部からロボットに対して負荷が加えられない限り、検出対象軸にはモータが発生する指令値に従ったトルクが生じることになるので、推定トルク値と発生トルク値とが一致することになる。これに対し、衝突が発生したことなどにより外部からロボットに対して所定の負荷が加えられると、推定トルク値と発生トルク値との間には、その負荷に応じた分だけの差が生じる。そこで、外乱演算手段は、推定トルク値と発生トルク値との差分から外部より検出対象軸に加わる外乱負荷トルクの値を演算する。

また、検出対象軸は、互いに同一方向の軸心を持つものであり、これらの間にはリンクが存在している。このリンクに対する衝突が発生した場合、これら検出対象軸の一方に加わる外乱負荷トルクの値と、他方に加わる外乱負荷トルクの値とは必ず符号が反転する。その理由は次のとおりである。すなわち、一般に、ロボットにおける種々の演算は、上記リンクが剛体であると仮定した上で行われる。剛体であるリンクに対して所定の負荷が加わったとしてもリンクは曲がることがないとされるため、必然的にリンクの両端部に位置する各検出対象軸において、互いに反対向きの（符号が異なる）外乱負荷トルクが生じることになる。本手段では、このような点を利用して、ロボットのリンクに対する衝突を以下のようにして検出する。

すなわち、衝突判断軸抽出手段は、外乱負荷トルク値の符号が異なる任意の２つの検出対象軸を衝突判断軸として抽出する。そして、衝突判断手段は、衝突判断軸に対応した２つの外乱負荷トルク値の差分の絶対値を求め、その絶対値が所定のしきい値を超えた場合には、これら衝突判断軸間に存在するリンクに対する衝突が発生していると判断する。衝突判断軸間のリンクに対する衝突が生じた場合、これら衝突判断軸に加わる外乱負荷トルクは、いずれも高い値を示すとともに互いに符号が異なる。従って、これらの差分の絶対値は、非常に高い値となる。これにより、各軸単位の外乱負荷トルクとしきい値とを比較する従来の方法と比べ、しきい値を高く設定することができる。このため、外乱負荷トルクの演算に伴う誤差の影響による衝突の誤検出を防止することができる。しかも、しきい値の比較対象である上記絶対値は、衝突が生じたリンクに対応する部分で非常に高い値を示すことになるので、衝突の検出感度についても高めることができる。また、本手段によれば、衝突が生じた箇所は、衝突判断軸間に存在するリンクのいずれかであるという判断ができるので、衝突箇所を所定の範囲内に特定することが可能となる。

また、衝突判断軸抽出手段は、外乱負荷トルク値の符号が互いに異なる任意の２つの検出対象軸のうち、互いの距離が最も遠いものを衝突判断軸として抽出する。２つの検出対象軸の外乱負荷トルク値は、てこの原理に基づいて考えれば、これらの距離が遠くなるほど高くなる。このことは、各検出対象軸の間に複数のリンクが存在する場合であっても、これら複数のリンクが１つの剛体であると仮定することで成り立つ。従って、このように互いの距離が最も遠いものを衝突判断軸とすることで、外乱負荷トルク値の差分の絶対値は最も高い値を示すことになる。このため、衝突が発生した場合、直ちにその衝突を検出することができる。

本発明の第１の実施形態を示すロボットシステムの構成を概略的に示す図 ロボットシステムの電気構成図 モータ制御の内容を等価的に示すブロック図 ２つの軸およびそれらの間に存在するリンクを示す図 第３軸と第５軸の間に外力が加えられたロボットの構成を示す図 第３軸と第５軸の間に外力が加えられた場合の各軸の外乱負荷トルク値と２つの外乱負荷トルク値の差分とを絶対値として示す図 第２軸と第３軸の間に外力が加えられたロボットの構成を示す図 第２軸と第３軸の間に外力が加えられた場合の図６相当図 総合衝突検出制御の内容を示すフローチャート 本発明の第２の実施形態を示す図９相当図 本発明の第３の実施形態を示す図１相当図

（第１の実施形態）
以下、本発明の第１の実施形態について図１〜図９を参照しながら説明する。
図１は、一般的な産業用ロボットのシステム構成を示している。この図１に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。

図１に示すように、ロボット２は、６軸の垂直多関節型ロボットとして構成されている。すなわち、ベース５上には、Ｚ方向の軸心を持つ第１軸Ｊ１を介してショルダ部６が水平方向に回転可能に連結されている。ショルダ部６には、Ｙ方向の軸心を持つ第２軸Ｊ２を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸Ｊ３を介して第１の上アーム８が垂直方向に回転可能に連結されている。第１の上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸Ｊ４を介して第２の上アーム９が捻り回転可能に連結されている。第２の上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸Ｊ５を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸Ｊ６を介してフランジ１１が捻り回転可能に連結されている。

ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アーム先端であるフランジ１１には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット２に設けられる複数の軸（Ｊ１〜Ｊ６）はそれぞれに対応して設けられるモータ（図２に符号Ｍを付して示す）により駆動される。各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている。

本実施形態では、下アーム７、第１の上アーム８および第２の上アーム９がリンクに相当する。また、第２軸Ｊ２、第３軸Ｊ３および第５軸Ｊ５を、ロボット２の上記リンクに対する衝突を検出するための検出対象軸としている。これら検出対象軸（Ｊ２、Ｊ３、Ｊ５）は、同一方向（Ｙ方向）の軸心を持つとともに、互いの間にリンク（７、８、９）が存在している。

ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。

図２は、ロボットシステム１の電気構成を概略的に示すブロック図である。ロボット２には、第１軸Ｊ１〜第６軸Ｊ６をそれぞれ駆動するための複数のモータＭ（図２では１つのみ示す）が設けられている。モータＭは、例えばブラシレスＤＣモータである。コントローラ３には、交流電源２１より供給される交流を整流および平滑して出力する直流電源装置２２、モータＭを駆動するインバータ装置２３、電流検出部２４、位置検出部２５およびこれら各装置の制御などを行う制御部２６が設けられている。

直流電源装置２２は、整流回路２７と平滑用のコンデンサ２８とから構成されている。整流回路２７は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば３相２００Ｖの交流電源２１の各相出力は、整流回路２７の交流入力端子に接続されている。整流回路２７の直流出力端子は、それぞれ直流電源線２９、３０に接続されている。これら直流電源線２９、３０間にはコンデンサ２８が接続されている。

インバータ装置２３は、直流電源線２９、３０間に６つのスイッチング素子例えばＩＧＢＴ（図２には２つのみ示す）を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを６組備えている（図２には１組のみ示す）。ＩＧＢＴのコレクタ・エミッタ間には還流ダイオードが接続されている。また、ＩＧＢＴのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部２６から与えられる指令信号（通電指令Ｓｃ）に基づいてパルス幅変調されたゲート信号を出力して各ＩＧＢＴを駆動する。

制御部２６（駆動制御手段に相当）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、Ｉ／Ｏなどを備えたマイクロコンピュータを主体として構成されている。電流検出部２４（電流検出手段に相当）は、モータＭに流れる電流を検出する電流検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。位置検出部２５は、モータＭの回転位置を検出する位置検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。制御部２６は、電流検出部２４から出力されるデータを元にモータＭに流れる電流の値を取得するとともに、位置検出部２５から出力されるデータを元にモータＭの回転位置および回転速度を取得する。詳細は後述するが、制御部２６は、このようにして取得した電流値、回転位置および回転速度を用いてインバータ装置２３によるモータＭの駆動をフィードバック制御する。

図３は、ロボットシステム１におけるモータ制御の内容を等価的に示したブロック図である。この図３に示すように、制御部２６は、位置制御部４１、速度制御部４２、電流制御部４３、トルク値推定部４４、発生トルク値演算部４５および外乱トルク値演算部４６を備えている。なお、図３では、１つのモータＭの制御に係る構成のみを示しているが、実際には全てのモータＭのそれぞれに対応して同様の構成が設けられている。さて、一般に産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。図示しない上位制御部は、その動作プログラムを解釈し、ロボット２に動作プログラムに従った動作を行わせるように各モータＭを制御するための指令値（位置指令ｐｃ）を位置制御部４１に出力する。

位置制御部４１は、上位制御部から与えられる位置指令ｐｃに対する現在の回転位置ｐ＊の偏差を求める減算器４７と、この減算器４７の出力（偏差）をゼロに近づけるように速度指令ｖｃ（回転速度指令）を出力する位置制御アンプ４８とから構成されている。この位置制御アンプ４８のゲインはＫｐとなっている。

速度制御部４２は、微分器４９、減算器５０および速度制御アンプ５１により構成されている。微分器４９は、現在の回転位置ｐ＊を微分して現在の回転速度ｖ＊に変換する。減算器５０は、速度指令ｖｃに対する現在の回転速度ｖ＊の偏差を求める。速度制御アンプ５１は、この減算器５０の出力（偏差）をゼロに近づけるように電流指令ｉｃを出力する。この速度制御アンプ５１のゲインはＫｖとなっている。

電流制御部４３は、電流指令ｉｃに対する現在のモータＭに流れる電流ｉ＊の偏差を求める減算器５２と、この減算器５２の出力（偏差）をゼロに近づけるようにインバータ装置２３に対する指令信号（通電指令Ｓｃ）を出力する電流制御アンプ５３とから構成されている。この電流制御アンプ５３のゲインはＫｉとなっている。このような構成により、制御部２６は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータＭの駆動をフィードバック制御してロボット２のアームの動作制御を行う。

トルク値推定部４４（トルク値推定手段に相当）は、ロボット２が所定の動作プログラムに基づいて行う動作の開始から終了までの間にモータＭに発生させる必要があるトルクの値（推定トルク値）を、位置指令ｐｃを用いて一括して推定する。この推定方法としては、例えば運動方程式を用いて、ロボット２の軸を所定の回転位置からその次の回転位置まで回転させるためにモータＭに発生させる必要があるトルク値を推定するという方法が挙げられる。なお、トルク値推定部４４は、ロボット２が現在の指令位置からその次の指令位置まで移動するためにモータＭに発生させる必要があるトルク値を逐次推定するように構成してもよい。発生トルク値演算部４５（トルク値演算手段に相当）は、モータＭに流れる現在の電流ｉ＊に対し、モータＭのトルク定数を乗算することにより、モータＭが発生する現在のトルクの値（発生トルク値）を求める。

外乱トルク値演算部４６（外乱演算手段に相当）は、推定トルク値および発生トルク値の差分から、モータＭが駆動する対象となる軸に外部から加わるトルク値（外乱負荷トルク値）を求める。すなわち、外部からロボット２に対して負荷が加えられない限り、ロボット２の各軸には、位置指令ｐｃに従いモータＭが発生するトルクがそれぞれ生じることになるので、推定トルク値と発生トルク値とは一致する。これに対し、衝突が発生したことなどにより外部からロボット２に対して所定の負荷が加えられると、推定トルク値と発生トルク値との間には、その負荷に応じた分だけの差が生じる。そこで、外乱トルク値演算部４６は、推定トルク値Ｔｅおよび発生トルク値Ｔｇを用いて下記（１）式に基づき外乱負荷トルク値Ｔｄを演算する。
Ｔｄ＝Ｔｇ−Ｔｅ …（１）

制御部２６（衝突判断軸抽出手段および衝突判断手段に相当）は、検出対象軸（Ｊ２、Ｊ３、Ｊ５）に加わる外乱負荷トルク値Ｔｄに基づいて以下のようにロボット２のリンクに対する衝突を検出する。例えば、第２軸Ｊ２および第３軸Ｊ３の間に存在するリンクである下アーム７に対する衝突が発生した場合、第２軸Ｊ２および第３軸Ｊ３のそれぞれには、互いに符号の異なる外乱負荷トルクが発生する。このように、２つの検出対象軸間に存在するリンクに外部から所定の負荷が加えられたときに、２つの検出対象軸の一方に加わるトルクの値と他方に加わるトルクの値とにおいて符号が反転する理由は次のとおりである。

すなわち、一般に、ロボット２における種々の演算は、各リンクが剛体であると仮定した上で行われる。図４は、２つの軸およびそれらの間に存在するリンクを模式的に示している。この図４に示すように、リンクＬに対して所定の外力（負荷）が加わった場合、リンクＬは剛体であるため、曲がることはない。このため、必然的にリンクＬの両端部に位置する軸Ｊａ、Ｊｂにおいて、互いに反対向きの（符号が異なる）トルクが生じることになる。なお、このことは、各検出対象軸の間に複数のリンクが存在する場合であっても、これら複数のリンクが１つの剛体であると仮定することで成り立つ。

本実施形態の制御部２６は、このような点を利用して、ロボット２のリンクに対する衝突を検出するリンク衝突検出制御を行う。すなわち、制御部２６は、外乱負荷トルク値Ｔｄの符号が互いに異なる任意の２つの検出対象軸を衝突判断軸として抽出する。制御部２６は、抽出した衝突判断軸に対応する２つの外乱負荷トルク値の差分の絶対値を求める。そして、制御部２６は、上記絶対値が所定のしきい値を超えた場合に、２つの衝突判断軸間に存在するリンクに対する衝突が発生していると判断する。

次に、本実施形態の作用および効果について図５〜図９も参照して説明する。
図５は、ロボット２の構成を模式的に示す図であり、検出対象軸（Ｊ２、Ｊ３、Ｊ５）以外の軸は省略している。図５に示すように、第３軸Ｊ３および第５軸Ｊ５の間のリンク（第１の上アーム８および第２の上アーム９）に対し、図示の外力（負荷）が加えられると、各検出対象軸には図示の外乱負荷トルクが生じる。すなわち、第２軸Ｊ２には＋１５０［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じ、第３軸Ｊ３には＋１００［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じ、第５軸Ｊ５には−１００［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じる。

この際、検出対象軸に生じる外乱負荷トルクの値のうち、互いに符号の異なる２つの組み合わせ（Ｊ２およびＪ５の組み合わせと、Ｊ３およびＪ５の組み合わせ）が存在する。この場合、本実施形態の制御部２６は、互いの距離が近いほうの組み合わせを採用する。すなわち、制御部２６は、外乱負荷トルク値の符号が互いに異なる任意の２つの検出対象軸のうち、互いの距離が最も近いものを衝突判断軸として抽出する。従って、制御部２６は、第３軸Ｊ３および第５軸Ｊ５を衝突判断軸として抽出し、それらの外乱負荷トルク値の差分の絶対値を求める。

図６は、各検出対象軸の外乱負荷トルク値と、衝突判断軸の外乱負荷トルク値の差分とをいずれも絶対値で示している。この図６に示すように、上記差分の絶対値は、２００［ｋｇｆ・ｃｍ］であり、各検出対象軸の外乱負荷トルク値と比較して高い値を示している。また、本実施形態においては、しきい値Ｔｔｈを例えば１５０［ｋｇｆ・ｃｍ］としている。制御部２６は、この差分の絶対値がしきい値Ｔｔｈを超えているため、衝突判断軸（Ｊ３、Ｊ５）間のリンク（８、９）に衝突が生じたと判断する。そして、制御部２６は、ロボット２の動作を停止させる停止制御を行うとともに、衝突が生じたことをユーザに対して報知する報知制御を行う。この報知制御としては、衝突を表すメッセージを表示させたり、衝突を表す警告音を発生させたりするなどが考えられる。本実施形態では、制御部２６は、報知制御として、ティーチングペンダント４の画面上に「○○○において衝突発生！」といったメッセージを表示させる。なお、この○○○には、衝突が生じたと判断した箇所（リンク）の名称を表示すればよく、上記ケースでは「上アームにおいて衝突発生！」となる。

続いて、上記ケースとは異なるリンクに対する衝突が生じた場合について説明する。図７および図８は、それぞれ図５および図６に相当する。この図７に示すように、第２軸Ｊ２および第３軸Ｊ３の間のリンク（下アーム７）に対し、図示の外力（負荷）が加えられると、各検出対象軸には図示の外乱負荷トルクが生じる。すなわち、第２軸Ｊ２には−１００［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じ、第３軸Ｊ３には＋１５０［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じ、第５軸Ｊ５には＋２００［ｋｇｆ・ｃｍ］の外乱負荷トルクが生じる。

このケースでも、検出対象軸に生じる外乱負荷トルクの値のうち、互いに符号の異なる２つの組み合わせ（Ｊ２およびＪ３の組み合わせと、Ｊ２およびＪ５の組み合わせ）が存在する。この場合、制御部２６は、互いの距離が近い第２軸Ｊ２および第３軸Ｊ３を衝突判断軸として抽出し、それらの外乱負荷トルク値の差分の絶対値を求める。

図８に示すように、上記差分の絶対値は、２５０［ｋｇｆ・ｃｍ］であり、各検出対象軸の外乱負荷トルク値と比較して高い値を示している。制御部２６は、この差分の絶対値がしきい値Ｔｔｈを超えているため、衝突判断軸（Ｊ２、Ｊ３）間のリンク（７）に衝突が生じたと判断する。そして、制御部２６は、停止制御を行うとともに報知制御を行う。このケースではティーチングペンダント４の画面上に「下アームにおいて衝突発生！」という表示がなされる。

このように、制御部２６は、外乱負荷トルク値の符号が互いに異なる任意の２つの検出対象軸を衝突判断軸として抽出し、これら衝突判断軸に加わる外乱負荷トルク値の差分の絶対値に基づいてロボット２のリンクに対する衝突を検出するリンク衝突検出制御を行う。上記差分の絶対値は、任意の２つの検出対象軸に加わる外乱負荷トルク値の絶対値を加算したものと等しくなるので、各検出対象軸にそれぞれ加わる外乱負荷トルク値と比べて非常に高い値となる。

一方、各軸の外乱負荷トルク値のそれぞれとしきい値とを比較してロボット２に対する衝突を検出する従来の方法では、そのしきい値を各軸単位の外乱負荷トルク値に合わせて設定する必要があった。例えば、図５および図７のケースでは、最も小さい外乱負荷トルク（１００［ｋｇｆ・ｃｍ］）が加わる軸についての衝突を検出するためには、しきい値Ｔｔｈ’を例えば５０［ｋｇｆ・ｃｍ］程度に設定しなければならない。これに対し、本実施形態のリンク衝突検出制御では、各軸の外乱負荷トルク値よりも十分に高い上記絶対値に合わせてしきい値Ｔｔｈを設定すればよいため、従来のしきい値Ｔｔｈ’に比べて高い値に設定することができる。

このしきい値が高いほど、外乱負荷トルクの演算に伴う誤差の影響を受け難くなり、その結果、衝突の誤検出が生じる可能性を低減できる。本実施形態のリンク衝突検出制御によれば、上記しきい値を高く設定できるため、外乱負荷トルクの演算に伴う誤差の影響による衝突の誤検出の発生を抑制することができる。また、その高いしきい値Ｔｔｈは、上記差分の絶対値に合わせて設定しており、その差分の絶対値は各軸の外乱負荷トルク値に比べて非常に高い値となる。すなわち、上記差分の絶対値は、衝突が生じたときに、際立って高い値となるため、リンクに対する衝突の検出感度についても高めることができる。

上記リンク衝突検出制御によれば、衝突が生じた箇所は、衝突判断軸間に存在するリンクのいずれかであるという判断ができる。このため、衝突箇所を所定の範囲内に特定することが可能となる。加えて、外乱負荷トルク値の符号が互いに異なる検出対象軸のうち、互いの距離が近いものを衝突判断軸として抽出した。検出対象軸同士の距離が近いということは、それらの間に存在するリンクの数が少ないことを意味する。従って、衝突箇所をより狭い範囲内に特定することができる。また、制御部２６は、特定した衝突箇所をユーザに報知する報知制御を実行する。各軸単位での衝突判断を行う従来の方法では、衝突位置が分かり難かったが、本実施形態によれば、衝突位置を所定の範囲内に特定することが可能となり、その衝突の原因特定作業において非常に有益な情報（おおよその衝突位置）をユーザに提供することができる。

このように、上記リンク衝突検出制御によれば、検出対象軸間のリンクに対して生じた衝突に関しては、誤検出の発生を抑制しつつ、高精度に検出することができる。しかし、上記リンク衝突判断制御では、検出対象軸間のリンク以外の部分（例えば手先など）に対して生じた衝突を検出することができない。なぜなら、検出対象軸間のリンク以外の部分（手先）に対して衝突が生じた場合、検出対象軸に同じ方向の外乱負荷トルクが加わる（検出対象軸に加わる外乱負荷トルク値の符号が全て同じになる）からである。そこで、本実施形態の制御部２６は、上記リンク衝突検出制御と、従来の各軸単位での衝突検出制御（各軸衝突検出制御）とを以下のように組み合わせた総合衝突検出制御を実行する。

図９は、制御部２６による総合衝突検出制御の内容を示すフローチャートである。
まず、図９のステップＳ１では、ロボット２の全ての軸、すなわち第１軸Ｊ１〜第６軸Ｊ６のそれぞれに加わる外乱負荷トルク値を求める。このステップＳ１の後は、ステップＳ２のリンク衝突検出制御と、ステップＳ３の各軸衝突検出制御とを並行して行う。なお、これらステップＳ２、Ｓ３は、いずれかを先に実行するようにしてもよい。

ステップＳ２のリンク衝突検出制御は、前述したとおりの内容である。このリンク衝突検出制御において衝突が検出された場合（検出有り）にはステップＳ４に進む。また、リンク衝突検出制御において衝突が検出されなかった場合（検出無し）にはステップＳ１に戻る。一方、ステップＳ３の各軸衝突検出制御は、従来の制御方法であり、例えば、各軸の外乱負荷トルク値の絶対値としきい値Ｔｔｈ’とを比較し、少なくとも１つの外乱負荷トルク値がしきい値Ｔｔｈ’を超えた場合（検出有り）には、その外乱負荷トルク値に対応する軸において衝突があったと判断し、ステップＳ４に進む。また、いずれの外乱負荷トルク値もしきい値Ｔｔｈ’以下であった場合（検出無し）にはステップＳ１に戻る。

ステップＳ４のロボット停止制御では、ロボット２の動作を直ちに停止させ、ステップＳ５に進む。なお、これらステップＳ４、Ｓ５は、並行して行ってもよいし、ステップＳ５を先に行ってもよい。続くステップＳ５では、衝突が生じたことをユーザに報知する報知制御を行う。ステップＳ５は、ステップＳ２、Ｓ３における衝突検出状況によって以下のように制御内容が変化する。すなわち、ステップＳ２のリンク衝突検出制御において衝突が検出された場合には、前述したように衝突が発生したリンク名称をユーザに報知する。これに対し、ステップＳ２では衝突が検出されず、ステップＳ３においてのみ衝突が検出された場合には、リンク以外の部分で衝突が生じているため、その旨を報知する。この場合、ティーチングペンダント４の画面上に「×××軸において衝突発生！」といったメッセージが表示されることになる。なお、この×××には、各軸のうち最も高い外乱負荷トルク値を示した軸の名称を表示するとよい。

このように、本実施形態の総合衝突検出制御によれば、検出対象軸間のリンクに対する衝突については、リンク衝突検出制御によって誤検出を抑制し且つ高精度に検出し、また、上記リンク以外の部分に対する衝突については、各軸衝突検出制御によって従来と同等の検出精度でもって検出することができる。

また、本実施形態では、Ｙ方向の軸心を持つ第２軸Ｊ２、第３軸Ｊ３および第５軸Ｊ５を検出対象軸とした。ロボット２においては、Ｙ方向の軸心を持つ軸の数が３つであり、最も多い。このように検出対象軸の数を多くすることで、リンクに対する衝突について、その衝突箇所を細かく特定することが可能となる。

なお、本実施形態では、Ｘ方向の軸心を持つ第４軸Ｊ４および第６軸Ｊ６を検出対象軸とすることは次のような理由からできない。すなわち、第４軸Ｊ４および第６軸Ｊ６は、同一軸上に互いの軸心が存在している。言い換えると、第４軸Ｊ４と第６軸Ｊ６との間にはリンクが存在しないことになる。このため、衝突が生じた場合には、第４軸Ｊ４および第６軸Ｊ６には、互いに同じ方向に同じ大きさのトルクが加わる。同じ方向に同じ大きさのトルクが加わるということは、それらの差がゼロになるということである。従って、これら軸に加わる外乱負荷トルク値の差分の絶対値が必ずしきい値Ｔｔｈ以下となり、衝突を検出することができない。このような理由から、第４軸Ｊ４および第６軸Ｊ６を検出対象軸とすることはできない。また、Ｚ方向の軸心を持つ軸は第１軸Ｊ１しかないため、それを検出対象軸とすることもできない。

（第２の実施形態）
以下、第１の実施形態に対し、総合衝突検出制御の内容を変更した第２の実施形態について図１０を参照しながら説明する。
図１０は、第１の実施形態における図９相当図である。図１０に示す総合衝突検出制御は、図９に示した第１の実施形態の総合衝突検出制御に対し、ステップＳ３の各軸衝突検出制御に代えてステップＳ１３の手先衝突検出制御を行う点と、ステップＳ５の報知制御に代えてステップＳ１５の報知制御を行う点とが異なっている。

従来より、各軸の外乱負荷トルク値から、ヤコビ行列を用いてロボット２の手先に加わる外乱負荷トルク値を求めることが行われている（例えば、特開２００３−２４５８８１号公報参照）。ステップＳ１３では、上記手法を用いて手先に加わる外乱負荷トルク値を求め、その外乱負荷トルク値の絶対値としきい値Ｔｔｈ’とを比較し、外乱負荷トルク値がしきい値Ｔｔｈ’を超えた場合（検出有り）には、手先に対する衝突があったと判断し、ステップＳ４に進む。また、外乱負荷トルク値がしきい値Ｔｔｈ’以下であった場合（検出無し）にはステップＳ１に戻る。

ステップＳ１５は、ステップＳ５に対し、ステップＳ１３でのみ衝突が検出された場合の報知方法が異なる。ステップＳ１３においてのみ衝突が検出された場合には、ロボット２の手先に対する衝突が生じているため、その旨を報知する。この場合、ティーチングペンダント４の画面上に「手先において衝突発生！」といったメッセージが表示されることになる。

このように、本実施形態の総合衝突検出制御によれば、検出対象軸間のリンクに対する衝突については、リンク衝突検出制御によって誤検出を抑制し且つ高精度に検出し、また、上記リンク以外の部分である手先に対する衝突については、手先衝突検出制御によって従来と同等の検出精度でもって検出することができる。

（第３の実施形態）
以下、上記各実施形態に対し、対象とするロボットを変更した第３の実施形態について図１１を参照しながら説明する。
図１１は、第１の実施形態における図１相当図である。この図１１に示すロボットシステム６１は、第１の実施形態におけるロボットシステム１に対し、６軸の垂直多関節型ロボットとして構成されたロボット２に代えて、４軸の水平多関節型ロボットとして構成されたロボット６２を用いている点が異なっている。

この図１１に示すように、ロボット６２は、設置面に固定されるベース６３と、このベース６３上にＺ方向の軸心を持つ第１軸Ｊ６１を中心に回転可能に連結された第１のアーム６４と、第１のアーム６４の先端部上にＺ方向の軸心を持つ第２軸Ｊ６２を中心に回転可能に連結された第２のアーム６５と、第２のアーム６５の先端部に上下動可能（矢印Ａ方向）で且つＺ方向の軸心を持つ第３軸Ｊ６３を中心に回転可能に設けられたシャフト６６とから構成されている。シャフト６６の先端部（下端部）には、ツールなどを取り付けるためのフランジ６７が位置決めされて着脱可能に取り付けられるようになっている。

本実施形態では、第１のアーム６４および第２のアーム６５がリンクに相当する。また、第１軸Ｊ６１、第２軸Ｊ６２および第３軸Ｊ６３を、ロボット６２の上記リンクに対する衝突を検出するための検出対象軸としている。これら検出対象軸（Ｊ６１、Ｊ６２、Ｊ６３）は、同一方向（Ｚ方向）の軸心を持つとともに、互いの間にリンク（６４、６５）が存在している。

上記構成においても、上記各実施形態と同様のリンク衝突検出制御、各軸衝突検出制御、手先衝突検出制御およびこれらを含む総合衝突検出制御を行うことが可能である。従って、本実施形態の構成によっても、検出対象軸間のリンクに対する衝突については、誤検出を抑制し且つ高精度に検出し、また、上記リンク以外の部分に対する衝突については、各軸衝突検出制御または手先衝突検出制御によって従来と同等の検出精度でもって検出することができる。

（第４の実施形態）
以下、上記各実施形態に対し、リンク衝突制御の内容を変更した本発明の第４の実施形態について説明する。
上記各実施形態のリンク衝突検出制御では、検出対称軸に加わる外乱負荷トルクの値に基づいてリンクに対する衝突を検出していたが、本実施形態では、全ての軸（Ｊ１〜Ｊ６）に加わる外乱負荷トルクの値に基づいて以下のようにリンクに対する衝突を検出する点が異なっている。すなわち、制御部２６は、各軸に加わる外乱負荷トルクをＸ、Ｙ、Ｚ方向の各成分に分割して考え、そのうちの所定方向成分（本実施形態ではＹ方向成分）を抽出し、抽出した所定方向成分としきい値とを比較することで衝突の検出を行う。

ベース座標を基準とした各軸の回転ベクトルをａ_ｎとし、抽出する方向の回転ベクトルをｂとし、各軸の外乱負荷トルクをτ_ｎとすると、外乱負荷トルクの抽出方向成分Ｔ_ｎは、下記（２）式により表される。ただし、＊はベクトルの内積とする。
Ｔ_ｎ＝τ_ｎ＊ａ_ｎ＊ｂ …（２）

例えば、図１に示す６軸のロボット２の第１軸Ｊ１〜第６軸Ｊ６の回転ベクトルａ_１〜ａ_６は、それぞれ下記（３）〜（８）式により表される。
ａ_１＝（０，０，１） …（３）
ａ_２＝（０，１，０） …（４）
ａ_３＝（０，１，０） …（５）
ａ_４＝（１，０，０） …（６）
ａ_５＝（０，１，０） …（７）
ａ_６＝（１，０，０） …（８）

また、抽出するＹ方向の回転ベクトルｂは、下記（９）式により表される。
ｂ＝（０，１，０） …（９）

例えば、ロボット２の第１軸Ｊ１〜第６軸Ｊ６に加わる外乱負荷トルクτ_ｎが、６０、５０、−４０、３０、２０、１０であるとすると、各軸に加わる外乱負荷トルクのＹ方向成分Ｔ_１〜Ｔ_６は、下記（１０）〜（１１）式により表される。
Ｔ_１＝６０＊（０，０，１）＊（０，１，０）＝０ …（１０）
Ｔ_２＝５０＊（０，１，０）＊（０，１，０）＝５０ …（１１）
Ｔ_３＝−４０＊（０，１，０）＊（０，１，０）＝−４０ …（１２）
Ｔ_４＝３０＊（１，０，０）＊（０，１，０）＝０ …（１３）
Ｔ_５＝２０＊（０，１，０）＊（０，１，０）＝２０ …（１４）
Ｔ_６＝１０＊（１，０，０）＊（０，１，０）＝０ …（１５）

上記（１０）〜（１５）式に示すとおり、抽出したＹ方向と同じ方向の軸心を持つ軸（Ｊ２、Ｊ３、Ｊ５）以外の軸（Ｊ１、Ｊ４、Ｊ６）のＹ方向成分Ｔ_１、Ｔ_４、Ｔ_６は、いずれもゼロになる。そして、制御部２６は、各軸（Ｊ１〜Ｊ６）の外乱負荷トルクのＹ方向成分Ｔ_１〜Ｔ_６について、全ての軸の組み合わせの差分の絶対値を求める。従って、６軸のロボット２の場合には、１５（＝６×５÷２）組の差分の絶対値を求めることになる。なお、本実施形態では、上記各実施形態と比べて差分の絶対値を求める数が増えることになるが、この演算は単純な差分計算であるため、それに係る演算時間の増加はほとんど問題とならないレベルである。

制御部２６は、全ての軸の組み合わせの差分の絶対値のうち、最も大きい値を抽出する。この場合、第２軸Ｊ２と第３軸Ｊ３の組み合わせの値が９０であり、最も大きい値となる。制御部２６は、差分の絶対値のうち最も大きい値と、しきい値とを比較し、差分の絶対値がしきい値を超えている場合には、その差分の絶対値に対応する第２軸Ｊ２および第３軸Ｊ３間のリンク（下アーム７）に対する衝突が生じたと判断する。

なお、抽出した成分と同方向（Ｙ方向）の軸心を持つ軸以外の上記外乱負荷トルクの所定方向成分は、上記（１０）、（１３）、（１５）式に示すとおり、全てゼロになる。このため、差分の絶対値が大きくなる組み合わせは、抽出した成分と同方向の軸心を持つ軸（Ｊ２、Ｊ３、Ｊ５）同士の組み合わせとなる。すなわち、抽出した成分と同方向の軸心を持つ軸が、上記各実施形態における検出対象軸および衝突判断軸に相当することになる。言い換えれば、リンクに対する衝突を検出するための軸を、第２軸Ｊ２、第３軸Ｊ３おおよび第５軸Ｊ５に限定したことにもなる。

上記構成によれば、全ての軸の外乱負荷トルクの所定方向成分を抽出し、その抽出した成分の差分の絶対値を全軸について求める。そして、その差分の絶対値のうち最も大きい値がしきい値を超えた場合には、その差分の絶対値に対応する軸間のリンクに衝突が生じたと判断するようになっている。このように上記構成によっても、ロボット２の軸間に存在するリンクに対する衝突が検出されるので、第１〜第３の実施形態と同様の作用および効果が得られる。

さて、第１、第２の実施形態におけるロボット２では、各軸の回転方向（軸心の方向）が互いに直交していたため、Ｙ方向の軸心を持つ検出対象軸の外乱負荷トルクを演算する際、他の軸（Ｘ方向の軸心を持つ軸、Ｚ方向の軸心を持つ軸）の影響を受けることがなかった。また、第３の実施形態におけるロボット６２では、全ての軸（直動軸は除く）の回転方向（軸心の方向）が一致していたため、検出対象軸の外乱負荷トルクを演算する際に他の方向の軸心を持つ軸（そもそも存在しない）の影響を受けることがなかった。

しかし、第１〜第３の実施形態のリンク衝突検出制御を用いた場合、各軸の回転方向が互いに直交していないロボットを対象として衝突検出を行うと、検出対象軸の外乱負荷トルクが他の軸の外乱負荷トルクの影響を受けて変化する可能性があった。この場合、検出対象軸の外乱負荷トルクの演算に誤差が生じてしまい、ひいては衝突の検出精度の低下を招く可能性があった。本実施形態のリンク衝突検出制御によれば、外乱負荷トルクの所定方向成分のみを抽出して、その抽出した値に基づいてリンクに対する衝突を検出するための演算を行うので、上記した演算誤差が生じることがない。このため、本実施形態によれば、各軸の回転方向が互いに直交していないロボットを対象とした場合であっても、リンクに対する衝突の検出精度を高めることができる。

（その他の実施形態）
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
トルク値推定部４４は、ロボット２に所定の動作を行わせるためにモータＭに発生させる必要があるトルクの値（推定トルク値）を、モータ制御に用いられる所定の指令値を用いて推定する構成であればよい。
検出対象軸に生じる外乱負荷トルク値のうち、互いに符号の異なる組み合わせが２つ以上存在する場合、制御部２６は、互いの距離が最も遠いものを衝突判断軸として抽出してもよい。２つの検出対象軸の外乱負荷トルク値は、てこの原理に基づいて考えれば、これらの距離が遠くなるほど高くなる。従って、このような構成によれば、外乱負荷トルク値の差分の絶対値は最も高い値を示すことになり、衝突の検出感度が向上する。
上記各実施形態では、本発明を６軸の垂直多関節型のロボット２または４軸の水平多関節型ロボット６２に適用した例を説明したが、本発明は、各軸をモータにより駆動する構成であって、互いに同一方向の軸芯を持つとともに互いの間にリンクが存在する軸を備えたロボット全般に適用可能である。

図面中、１、６１はロボットシステム、２、６２はロボット、７は下アーム（リンク）、８は第１の上アーム（リンク）、９は第２の上アーム（リンク）、２４は電流検出部（電流検出手段）、２６は制御部（駆動制御手段、衝突判断軸抽出手段、衝突判断手段）、４４はトルク値推定部（トルク値推定手段）、４５は発生トルク値演算部（トルク値演算手段）、４６は外乱トルク値演算部（外乱演算手段）、６４は第１のアーム（リンク）、６５は第２のアーム（リンク）、Ｊ１〜Ｊ６、Ｊ６１〜Ｊ６３は軸、Ｍはモータを示す。

Claims (1)

1. ロボットに設けられ、互いに同一方向の軸心を持つとともに互いの間にリンクが存在する検出対象軸を含む複数の軸と、
   前記複数の軸をそれぞれ駆動するための複数のモータと、
   前記モータに流れる電流を検出する電流検出手段と、
   外部から与えられる指令値に従って前記モータの駆動を制御する駆動制御手段と、
   前記指令値を用いて前記検出対象軸に生じる推定トルク値を推定するトルク値推定手段と、
   前記電流検出手段の検出値を用いて前記検出対象軸に生じる発生トルク値を演算するトルク値演算手段と、
   前記推定トルク値と前記発生トルク値との差分から外部より前記検出対象軸に加わる外乱負荷トルクの値を演算する外乱演算手段と、
   前記外乱負荷トルク値の符号が互いに異なる任意の２つの前記検出対象軸を衝突判断軸として抽出する衝突判断軸抽出手段と、
   前記衝突判断軸に対応した２つの外乱負荷トルク値の差分の絶対値を求め、その絶対値が所定のしきい値を超えた場合には当該衝突判断軸間に存在するリンクに対する衝突が発生していると判断する衝突判断手段と、
   を備え、
   前記衝突判断軸抽出手段は、前記外乱負荷トルク値の符号が互いに異なる任意の２つの前記検出対象軸のうち、互いの距離が最も遠いものを衝突判断軸として抽出することを特徴とするロボットシステム。

Images