JPS5973289A - 多関節ロボツトの制御方法 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 産業上の利用分野 この発明は、多関節ロボットにおいて、動作時に、アー
ム間の干渉のおそれのある場合、そのロボットの特性た
とえばアーム長さ、アーム太さに合わせて、自動的にロ
ボットを停止させる方法に関するものである。

従来例の構成とその問題点 従来、多関節ロボットのアーム間、相互干渉を防ぐ方法
として、 隣り合うアーム同志は、リミットＳＷ等を用い、構造的
に相互干渉を起こさない様にして、干渉を防いでいたが
、 隣り合わないアーム同志の干渉については、ある定めら
れた点から、所定の点まで、ロボットを、人間が注意深
く、動かすことにより、干渉を防ぎながら、動作プログ
ラムを作成していた。

しかし、この様な方法では、動作プログラムの作成に手
間がかかるのみならず、オフラインで、教示のための動
作プログラムを作成して、実際にロボットを動かす時、
隣り合わないアーム同志の干渉の発生する危険性を回避
することは、出来なかった。

発明の目的 この発明は、隣り合うアーム同志の干渉は、構進上、相
互干渉を起こさない様にし、隣り合わないアーム同志の
相互干渉については、動作時に、アーム同志の干渉の可
能性を自動判定し、干渉するおそれのある場合は、自動
的に停止する手段を提供し、アーム同志が互いに干渉し
ないようにすることを目的とする。

発明の構成 この発明のロボットは、アーム長さ、太さを入力する手
段と、動作時、各関節の回転角を測定する手段と、アー
ム先端座標、及び、各関節中心座標を計算する手段と、
それらから、隣り合わないアーム間の最小距離を計算し
、アーム相互干渉の可能性を判定する手段と、判定結果
より、ロボットへ停止信号を出す手段より成り立ってい
る。

実施例の説明 以下、この発明を第３図の実施例に基づいて詳述する。

第２図において、１は、５関節よりなる多関節ロボット
本体である。

２は、多関節ロボットの第１関節で、Ｚ軸回りに回転し
、３，４．５は、それぞれ、第２関節。

第３関節、第４関節で、ｙ軸回りに回転する。６は、第
６関節で、Ｘ軸回りに、回転する。７は、ロボット制御
装置であり、８は、各アームの長さ。

太さを入力する手段、たとえば押ボタンＳＷの様なもの
であり、それらを操作することにより、多関節ロボット
のアーム長、アーム太さを、随時インタフェイス９を介
して小形計算機（以下Ｃｐｕと呼ぶ。）１０へ入力、記
憶出来る様になっている。

又、多関節ロボットの関節部２〜６には、回転検出器が
、それぞれついており、回転角に対応する電気信号が、
インターフェイス９を介して、ｃｐｕｌｏへ取り込まれ
る。

Ｃｐｕｌｏでは、上記により、記憶されている各アーム
長、各アームの太さ、又、周期的に入力されてくる回転
角を基にしてアーム同志の相互干渉を判定する演算がな
される。

次にアーム同志の干渉を判定する計算手続きについて、
ステップを追って、詳述する。

く第１ステツプ〉 あらかじめ、各アームの長さ、太さを入力する手段ｑ（
たとえば、押ボタンＳＷの様なもの）を操作することよ
り入力されているアーム長さと、周期的に回転検出器よ
り入力されてくる回転角を基にして、アーム先端座標、
及び各関節中心座標を求める。

θ、を第ｉ関節の回転角、ｔ、をそれに対応するアーム
の長さとし、Ｔ、（Ｘ）、　Ｊｉ（Ｙ）、　Ｊｉ（Ｚ）
を第ｉ関節のＸ座標、Ｙ座標、Ｘ座標とすると各関節の
中心座標及びアーム先端座標は、 （関節１中心座標） Ｊｌ（Ｘ）　＝　。

Ｊｌ（Ｙ）−〇 ■１（Ｚ）−ｔｌ （関節２中心座標） Ｊｌ（Ｘ）＝Ｏ Ｊ２（Ｙ）−〇 ■２（Ｚ）＝１１（Ｚ）＋４ （関節３中心座標） １３（ｘ）＝ｔ２ＳＩｎθ２ｃＯｓθ１Ｊｓ（Ｙ）　＝
Ｚ２　ｓｉｎθ２　ｓｉｎθ１■３（Ｚ）二１２（Ｚ）
十ｌ！、２部θ２（関節４中尼・座標） Ｊ４（Ｘ）　＝　Ｔ。（Ｘ）　＋　ｔ３ｓｉｎθ３ＣＯ
３θ１Ｊ４（”）　＝Ｊ３（Ｙ）　＋　／−３ｓｉｎθ
３　ｓｉｎθ１■４（Ｚ）＝１３（Ｚ）＋ｔ３ｃｏｓθ
３（関節５中七・座標） Ｊ５（Ｘ）　＝　Ｊ４（Ｘ）　＋　ｔ４ｓｉｎθ４部θ
１■、（Ｙ）＝■４（Ｙ）＋ｔ４ＳＩｒＩ０４ＳＩｒｌ
θ１■５（Ｚ）−’４（Ｚ）　”ｌ’１４　ｃｏｓθ４
（アーム先端座標） Ｊ６（ｘ）　＝７５（Ｘ）＋Ａ５（ｃｏｓ（９１ｃｏｓ
０５ｓ＋ｎθ４−５１ｎθ伸θ５）Ｉｓ（Ｙ）”ｌ６（
Ｙ）＋ｔ５（ｃｏｓθ１Ｓ石０５　”！＋１ｎｏＩＣＱ
Ｓ０６ｓｌｎθ４）工。（Ｚ）＝Ｊｓ（Ｚ　）　十Ｚｓ
α３θ６α３θ４となる。

く第２ステツプ〉 アーム先端座標および各関節中心座標より、隣り合わな
いアームのセンター間の最小距離を求める０ 隣り合わない２つのアームを、Ａアーム、Ｂアームとす
る。

Ａアーム中心線上の任意の点をＡ　（ｘＡ−ＶＡＴＺＡ
）、Ｂアーム中心線上の任意の点をＢ　（ｘＢ、　ＶＢ
。

ＺＢ）とすると、ＡＢ間の距離の２乗は、Ａ　Ｂ２＝　
（ｘｐ、−ＸＢ　）　２＋　（ｙＡ−ｙＢ　）　＋　（
ｚｐ、−ＺＢ　）　　となる。

一方、Ａアームの中心線の一方の端点の座標を、（ｚａ
、ｙａ、ｚａ）、そこからのＡアーム中心線の方向余弦
を、’−ＡＩ　”ＡＩ　”Ａとする。

他方、Ｂアームのそれを、（ｘｌ）＋　Ｙｂ＋　”ｂ）
、ＬＢ、　ｍＢ、　ｎＢとすると、 ”Ａ　＝　ｘａ＋ノ’ＡｋＡ、　　３’Ａ＝Ｙａ−Ｉ−
ｍＡｋＡ＋　　　ｚＡ＝　ｚａ”ＡｋＡ（０≦ｋＡ≦１
） ＸＢ＝Ｘｂ＋ｔＢｋＢ、　ｙＢ＝＝ｙｂ＋ｍＢｋＢ、　
ＺＢ’：Ｚｂ＋ｎＢｋＢ（０≦ｋＢ≦１） となり、距離は、ｋＡ、ｋＢの関数となる。

Ａ′Ｂ２＝ｆ（ｋＡ、ｋＢ） ””　（ｘａ−”ｂ”（’ＡｋＡ−ｔＢｋＢ）’＋ｉ　
（ｙａ−３’ｂ）”（ｍＡｋＡ−”ｂｋＢ）　１２＋　
ｉ　（ｚａ−ｚｂ）＋（ｎＡｋＡ−ｎＢｋＢ）１２Ａア
ーム、Ｂアームの２つのアームの中心線間の最小距離は
、○≦ｋＡ≦１，０≦ｋＢ≦１でｆ　（ｋＡ、　ｋＢ）
を最小にすればよい。

ｋＡ（４２Ｂ　＋ｍＡｍＢ　＋　ｎＡｎＢ　）　ｋＢ　
−−ＣｔＡ（ｘ　ａ−ｘ　ｂ）　＋ｍＡ（ｙａ−ｙＢ）
＋ｎＡ（ｚａ−２ｂ）〕−（４４＋ｒｎＡｍＢ　＋　ｎ
ＡｎＢ　）ｋｐ、＋ｋＢ　＝＝　ｔＢ、（Ｘ　ａ−Ｘ　
ｂ）　＋ｒｎＢ（ｙａ　ＹＢ）”Ｂ（ｚａ−２ｂ） 上記、連立方程式の解をｋＡ、　ｋＢ　　とする。

又、前記、ｆ（ｋＡ、ｋＢ）は、凹関数であることより
、ｍｉｎ　（ＬＢ２）は、ｋＮ　、　ｋＢ”　の範囲に
より、次表の如くなる。

次に、相互のアームの干渉の判定を行なう０１１Ｊす合
わない２つのアームＡ、Ｂのセンター間の最小距離をｔ
ＡＢ＊とし、アームＡ、Ｂの半径をｒＡｌ　”Ｂとする
と 々Ｂ　≦ｒＡ＋　ｒＢ の時、２つのアームは、干渉するおそれがある０全ての
隣り合わないアームについて ｌＡＢ≦ｒＡ＋ｒＢ＋ε の時、干渉するおそれありと見なす。。

前記の如く、ｃｐｕｌｏより、干渉する可能性があると
見なされた場合、ｃｐｕｌｏより、ロボット制御装置７
に停止信号を送り、ロボットを止める〇 発明の効果 この発明によれば、 １　ティーチング時、ロボットアー人間、相互の干渉を
気にせず動作プログラムが作成出来る０２　外部系で、
動作プログラムを作成しても、実際にロボットを動作さ
せる場合、アーム相互干渉の危険性がない。

３　アームの長さ、太さを変えても、それに対応して、
アーム間の相互干渉が回避出来る。

４　アームが、他の断面形状であっても、円筒で、近似
することにより、容易に拡張が出来る。

【図面の簡単な説明】

第１図は本発明の制御方法を示す説明図、第２図は本発
明の一実施例における多関節ロボットの構成および制御
系統を示す系統図、第３図は本発明の方法および演算手
順を示すフローチャートである。 Ａ・・・・・アーム長さ、太さの入力手段、Ｂ・・・・
・・アーム回転角測定手段、Ｃ・・・・・・アーム先端
座標および関節中心座標計算手段、Ｄ・・・・アーム間
最小距離計算手段およびアーム干渉判定手段、Ｅ・・・
・・ロボット動作停止手段。 代理人の氏名　弁理士　中　尾　敏　男　ほか１名第１
図 第２図

Claims (1)

1. 【特許請求の範囲】 多関節ロボットの各アームの長さ、太さを入力する手段
   と、前記各関節の回転角を検出する回転検出器と、前記
   回転検出器が検出した回転角を入力信号として、 アーム先端座標および各関節中心座標を計算する手段と
   、 各アーム間最小距離を計算する手段と、各アーム間同志
   の干渉の判定手段を持ち、前記アーム間の干渉の判定手
   段から、ロボットへの動作命令を制御し、アーム間の相
   互干渉を回避するよう構成した多関節ロボットの制御方
   法。