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

Abstract

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

Description

【発明の詳細な説明】 産業上の利用分野 この発明は多関節ロボットにおいて、動作時に凸平面よ
り構成される障害物（以下、凸障害物という）との干渉
のおそれのある場合、そのロボットの特性たとえばアー
ム長さ・アーム太さに合わせて、自動的にロボットを停
止させる方式に関するものである。

従来例の構成とその問題点 従来は多関節ロボットと、凸障害物の干渉を防ぐ方法と
して、ある定められた点から所定の点まで、ロボットを
人間が注意深く動かすことにより干渉を防きながら、動
作プログラムを作成していた。しかし、この様な方法で
は動作プログラムの作成に手間かかかるのみならず、オ
フラインで教示のだめの動作プログラムを作成して、実
際にロボットを動かす時、凸障害物との干渉の発生する
危険性を回避することは出来なかった。

発明の目的。

そこでこの発明は、凸障害物との干渉を動作時に自動判
定し、干渉のおそれのある場合は自動的にロボットを停
止させる手段を提供し、凸障害物とロボットが互いに干
渉しないようにすることを目的とする。

発明の構成 この発明のロボットに示す様に、ロボットの特性、たと
えばアーム長さ、太さを入力する手段と、６障害物の端
点を入力する手段と、動作時各関節の回転角を測定する
手段と、アーム先端座標および各関節中心座標を計算す
る手段と、それらから６障害物への最小距離を計算し、
６障害物との干渉の可能を判定する手段と、判定結果よ
り、ロボットへ停止信号を出す手段より成り立っている
。

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

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

第１軸は、Ｚ軸回りに回転し、第２．３．４軸はｙ軸回
りに回転し、第５軸は、Ｘ軸回りに回転する。

各アーム共、形状は、概略、円筒形状をしている。

７はロボット制御装置である。８は押ボタンＳＷの様な
入力装置であり、それらを押すことにより、多関節ロボ
ットの特性（アーム長・アーム太さ）が、又９は押ボタ
ンＳＷの様な入力装置であり、それらを押すことにより
、６障害物の端点が随時インタフェイス１０を介して、
ＣＰＵ１１へ入力。

記憶出来る様になっている。又、多関節ロボットの関節
部２〜６には、回転検出器がそれぞれついており、回転
角に対応する電気信号がインタフェイス１０を介してＣ
ＰＵＩ　１へ取り込まれる。

ＣＰＵ１１では、上記により記憶されているロボット特
性（アーム長・アーム太さ）、父周期的に入力されてく
る回転角を基にして、６障害物との干渉を判定する演算
がなされる。

次に６障害物との干渉を判定する割算手続きについて、
ステップを追って詳述する。

（第１ステツプ） あらかじめ押ボタ／ＳＷｓより入力されているロボット
の特性（アーム長さ・太さ）と、周期的に回転検出器２
〜６より入力されてくる回転角を基にして、アーム先端
座標及び各関節中心座標を求める。

θｌ　を第ｉ関節の回転角、ρｌ　をそれに対応するア
ームの長さとし、Ｊｉ（Ｘ）、　Ｊｉ（Ｙ）、　Ｊｉ（
Ｚ）を第１関節のＸ座標、Ｙ座標、２座標とすると、各
関節の中心座標及びアーム先ψ１１．；座標は次の様に
なる。

〔関節１中心座標〕 Ｊｌ（ｘ）＝○ Ｊｌ（Ｙ）二〇 Ｊｌ（ｚ）＝ｐ１ 〔関節２中心座標〕 ■２（Ｘ）−〇 ｌ２（Ｙ）二〇 Ｊ２　（Ｚ’）　−Ｊｌ（Ｚ）十ρ２ 〔関節３中心座標〕 １３（Ｘ）　＝　１２ｓｉｎ　０２ｔ：ｏｓσ１Ｔ、　
（Ｙ）　＝　Ｉｔ２ｓｉｎ　θ２ｓｉｌ＋　０１１３（
Ｚ　）　二Ｊ２　（Ｚ　）　＋　Ｒ２ｃｏｓｏ　２〔関
節４中心座標〕 Ｊ　４　（Ｘ　）　＝Ｊ３（Ｘ　）　”　１３ｓｉｎθ
３ｃｏｓθ１１４（Ｙ）＝　　Ｊ３（Ｙ）　　＋　ｆ１
３　ｓｉｎ　　θ３　ｓｉｎ　　０１Ｊ４（Ｚ）＝Ｊ３
（Ｚ）　＋ｆｉ３ｃｏｓθ３〔関節５中心座標〕 Ｊ　５　（Ｘ）＝　　Ｊ４（Ｘ）　　＋　４２４　ｓｉ
ｎ　　０４　ＣＯｓ　　θ１Ｊ６（Ｙ）二Ｊ　４（Ｙ　
）　＋１！、４＼団／／４ｓｉｎ１Ｊ　ｓ　（Ｚ　）　
＝Ｊ　４（Ｚ　）　＋１Ａ　ｃｏｓ　０４〔アーム先端
座標〕 Ｊ６（Ｘ）＝Ｊ５（Ｘ）十ρ５（ＣＯ５θ１ＣＯ５θ５
ｓｉｎθ４−８ｉｎθ１ｓｉｎθ５）Ｊ６（Ｙ）＝　Ｊ
５（Ｙ）＋ｆ１５（ｃｏｓｏ１ｓｉｎθ５＋ｓｉｎθ１
ｒｏｓθｓ　Ｓ　Ｉ　ｎθ４）Ｊ　６（Ｚ　）　＝Ｊ　
５（Ｚ　）　十Ｑ　５ｃｏｓθ５ｃｏｓθ４となる。

（第２ステツプ） あらかじめ、押ボタンＳＷ９より入力されている６障害
物の端点と、（第１ステツプ）により求められたアーム
先端座標および関節中心座標よりアームと凸障害物間の
距離を求める。アームと凸障害物間の距離を求めるに当
り、６障害物の一つの面と、アームとの距離算出方法を
述べる。６障害物より１つの凸子面を取り出し、その面
の−っの端点をＳとし、Ｓを起点とする２つの稜を、ア
ームの直径長さくＬ）づつで区切り、２つの稜に対応す
る区分点をそれぞれ、ｎ４　（ｉ＝１．２　、・・・）
。

ｎｉ／（ｉ＝１．２．・・・）とする。

ｎｉ・Ｌ（ｎ□′・Ｌ）が、それぞれの稜の長さを超え
る時、ｎ工（ｎ□′）に対応する区分点を、他方の端点
とし、区分を中止する。両方の稜の区分が中止された時
、それぞれの他方の端点を起点として、それぞれの稜を
前記と同様の手続きでアーム直径の長さ向づつで区切る
。

この様な手続きを繰り返すことにより、対応する２つの
区分点が最終的には１つの端点（均あるいは２つの端点
（Ｅ　、　Ｅ／）に集約され、凸障害物の１つの面が順
次三角形および四角形・・・・・。

Δ５ｎ１ｎ、’、ロｎ　ｉ　ｎ　１’　ｎ　２’　　　
口ｎ２　ｎ２’　ｎ３　ｎ３　、　”１’１２１ △ｎＥｎＥ／　Ｅと、分割される。

この様にして、分割された三角形および四角形とアーム
の干渉を順次検証して行くことにより、１つの凸障害物
の面とアームの干渉が検証出来る。

次に計算の基本となる前記分割された四角形と１つのア
ームの干渉の具体的！１痺手段を１例として説明する。

前記手続により分割されたある四角形の区分点を、ｎ４
．ｎ４’　　”ｉ＋１＋”ｉ＋１とし、２つの区分点ｎ
□＋　”ｉ＋Ｉ　Ｑ中点をＷ、ｎ／、＋”ｉ＋１　　の
中点をＷ、′とする。線分ＷＬＷ工′上の任意の点を、
Ｗ（ＸＷ、ＶＷ。

ＺＷ）、１つのアーム中心線分の任意の点をＡ（ＭＡ、
ＹＡＩＺＡ　）とする。

ＷＡ間の距離は、 一方、アーム中心線の一方の端点の座標を、（ＸＡＳ、
）’ＡＳ＋ｚＡＳ　）ｌそこからのアーム中心線の方向
余弦を、（Ｑ　Ａ、　ｒｎＡ、　ｎ　Ａ）とする。他方
、四角形　”ｉ　＋　”ｉ＋１の中点Ｗ０の座標を（ｘ
ｗｓ。

５’ＷＳ、ｚＷＳ）　　とし、”　ｉ’　＋　”　ｉ’
＋１の中点Ｗｉ′への方向余弦を（Ｑｗ、ｍｗ、ｎｗ）
とすると、ＸＡ＝ｘＡＳ”Ａ６ｋＡ！ＹＡ＝ＹＡＳ　　
Ａ　　ＡｌｚＡ””Ａｓ　　　Ａ　　　Ａ （Ｏ≦ｋＡ≦１） ｘＷ””　ｘＷＳ　＋’Ｗ’　ｋＷ　＋ｙｗ＝ｙｗｓ　
＋ｍｗ”Ｗｚｗ−２ｗｓ＋ｎｗＯｋｗ （〇−、、、、ｋｗ；（、，１） となり、距離は、ｋｗ、ｋＡの関数となる。

ＷＡ　　をｆ（ｋＷ、ｋＡ）とおくと、１つのアーム中
心線と、前記区分された四角形口ｎｌｎ工”’＝＋＊　
”ｉ＋１の最／」・距離は、○＝≦ｋｗ≦ｉ、ｏ≦ｋＡ
≦１で、／（ｋＷ、ｋＡ）を最小にすればよい。

ｋ　Ａ−（ｆｌ　Ａｊ２ｗ＋”ＡｍＷ＋ｎｐ、ｎｗ）ｋ
ｗ＝　　　［ＲＡ（ｘＡ−ｘｗ）十ｍＡｃｙＡ−ｙＷ”
ｎＡ（ｚＡ−ＺＷ）］−（ｌ　ＡＲｗ＋ｍＡｍｗｌｎＡ
”Ｗ）ｋＡ＋　ｋｗ−”Ｗ（ｘＡ−ＸＷ）　十ｍＷ（Ｖ
Ａ−ＶＷ）”　ｎＡ（ＺＡ−２Ｗ）上記、連立方程式の
解を、ｋＡ、ｋｗとする。

又、前記、ｆ（ｋｗ、ｋＡ）は、凹関数で、あることに
より、ｍｍ（ＷＡ）は、ｋＡ、ｋｗ（Ｄ範囲により、次
表の如くなる。

以下余白 次に、前記手続きにより分割された四角形と、１つのア
ームの干渉の判定の手続きを行なう。

前記、手続きにより、分割された四角形口Ｓと、１つの
アームＡの間の］゛し小距離を”ＡＳ　　とし、アーム
の半径をｒＡ　とすると、 ＱＩＳ≦　・Ａ の時、分割された四角形口Ｓと、１つのアームＡは、干
渉するおそれがある。

全てのアームと、全ての分割された四角形について、 ＱＡＢ　＝　ｘＡ＋　ｅ の時、干渉するおそれありと見なす。

前記の手続きよｐ、ＣＰＵ１１より干渉する可能がある
と見なされた場合、ＣＰＵ１１よりロボット制御装置７
へ停止信号を送り、ロボットを停止させる。

発明の効果 この発明によれば、 （１）　　ティーチング時、ロボットアームと、凸障害
物との干渉を気にせず、動作プログラムが作成できる。

（２）外部系で、動作プログラムを作成しても、実際に
ロボットを動作させる場合、アームと凸障害物の干渉の
危険性か少ない。

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

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

（５）障害物が凸・Ｖ−而より構成されていなくても凸
子面より構成されている様、近似することにより、容易
に他の障害物への拡張ができる。

【図面の簡単な説明】

第１図は本発明の一実施例における多関節ロボットの制
御方法を示す説明図、第２図は同多関節ロボットの装置
の構成および制御系統を示す説明図、第３〜４図は凸障
害物の４角形および３角形への分割分法を示す説明図、
第５図は本発明方法および演算手段を示すフローチャー
ト図である。 １　・・・多関節ロボット本体、２・・・・・・第１関
節、３・・・・・第２関節、４・・・・・第３関節、５
・・・・第４関節、６・・・・第６関節、７・・・・ロ
ボット制御装置、８−　アームの長さ、太さの入力装置
、９・−・・・凸障害物の端点の入力装置、１０・・・
・・インターフェイス、１１・・　小形計算機（ＣＰＵ
）。 代理人の氏名　弁理士　中　尾　敏　男　ほか１名ｆ！
Ｈ１園 第　２　図 第３図 ４図

Claims (1)

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