JPS5973289A

JPS5973289A - Method of controlling multi-joint robot

Info

Publication number: JPS5973289A
Application number: JP18170882A
Authority: JP
Inventors: 均久保田
Original assignee: Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Holdings Corp
Priority date: 1982-10-15
Filing date: 1982-10-15
Publication date: 1984-04-25

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

【発明の詳細な説明】産業上の利用分野この発明は、多関節ロボットにおいて、動作時に、アー
ム間の干渉のおそれのある場合、そのロボットの特性た
とえばアーム長さ、アーム太さに合わせて、自動的にロ
ボットを停止させる方法に関するものである。DETAILED DESCRIPTION OF THE INVENTION Field of Industrial Application This invention provides an articulated robot that, when there is a risk of interference between arms during operation, adjusts the robot's characteristics, such as arm length and arm thickness, to This relates to a method for automatically stopping a robot.

従来例の構成とその問題点従来、多関節ロボットのアーム間、相互干渉を防ぐ方法
として、隣り合うアーム同志は、リミットＳＷ等を用い、構造的
に相互干渉を起こさない様にして、干渉を防いでいたが
、隣り合わないアーム同志の干渉については、ある定めら
れた点から、所定の点まで、ロボットを、人間が注意深
く、動かすことにより、干渉を防ぎながら、動作プログ
ラムを作成していた。Conventional configuration and its problems Traditionally, as a method to prevent mutual interference between the arms of an articulated robot, adjacent arms use limit switches, etc. to structurally prevent mutual interference. However, in order to prevent interference between arms that are not adjacent to each other, humans had to carefully move the robot from one predetermined point to another to prevent interference while creating a motion program. .

しかし、この様な方法では、動作プログラムの作成に手
間がかかるのみならず、オフラインで、教示のための動
作プログラムを作成して、実際にロボットを動かす時、
隣り合わないアーム同志の干渉の発生する危険性を回避
することは、出来なかった。However, with this method, not only is it time-consuming to create a motion program, but also the motion program for teaching must be created offline, and when the robot is actually moved,
It was not possible to avoid the risk of interference between non-adjacent arms.

発明の目的この発明は、隣り合うアーム同志の干渉は、構進上、相
互干渉を起こさない様にし、隣り合わないアーム同志の
相互干渉については、動作時に、アーム同志の干渉の可
能性を自動判定し、干渉するおそれのある場合は、自動
的に停止する手段を提供し、アーム同志が互いに干渉し
ないようにすることを目的とする。Purpose of the Invention This invention prevents interference between adjacent arms from causing mutual interference during construction, and automatically prevents interference between arms that are not adjacent to each other during operation. The purpose of this invention is to provide a means to automatically stop the arms if there is a risk of interference, thereby preventing the arms from interfering with each other.

発明の構成この発明のロボットは、アーム長さ、太さを入力する手
段と、動作時、各関節の回転角を測定する手段と、アー
ム先端座標、及び、各関節中心座標を計算する手段と、
それらから、隣り合わないアーム間の最小距離を計算し
、アーム相互干渉の可能性を判定する手段と、判定結果
より、ロボットへ停止信号を出す手段より成り立ってい
る。Structure of the Invention The robot of the present invention has means for inputting the arm length and thickness, means for measuring the rotation angle of each joint during operation, and means for calculating the arm tip coordinates and the center coordinates of each joint. ,
It consists of means for calculating the minimum distance between non-adjacent arms from these and determining the possibility of mutual interference between the arms, and means for issuing a stop signal to the robot based on the determination result.

実施例の説明以下、この発明を第３図の実施例に基づいて詳述する。Description of examples This invention will be described in detail below based on the embodiment shown in FIG.

第２図において、１は、５関節よりなる多関節ロボット
本体である。In FIG. 2, reference numeral 1 denotes a multi-joint robot body consisting of five joints.

２は、多関節ロボットの第１関節で、Ｚ軸回りに回転し
、３，４．５は、それぞれ、第２関節。2 is the first joint of the multi-joint robot, which rotates around the Z axis, and 3 and 4.5 are the second joints, respectively.

第３関節、第４関節で、ｙ軸回りに回転する。６は、第
６関節で、Ｘ軸回りに、回転する。７は、ロボット制御
装置であり、８は、各アームの長さ。The third and fourth joints rotate around the y-axis. 6 is the sixth joint, which rotates around the X axis. 7 is a robot control device, and 8 is the length of each arm.

太さを入力する手段、たとえば押ボタンＳＷの様なもの
であり、それらを操作することにより、多関節ロボット
のアーム長、アーム太さを、随時インタフェイス９を介
して小形計算機（以下Ｃｐｕと呼ぶ。）１０へ入力、記
憶出来る様になっている。There is a means for inputting the thickness, such as a pushbutton SW, and by operating these, the arm length and arm thickness of the articulated robot can be input to a small computer (hereinafter referred to as CPU) via the interface 9. ) 10, it can be input and stored.

又、多関節ロボットの関節部２〜６には、回転検出器が
、それぞれついており、回転角に対応する電気信号が、
インターフェイス９を介して、ｃｐｕｌｏへ取り込まれ
る。In addition, each of the joints 2 to 6 of the articulated robot is equipped with a rotation detector, and an electric signal corresponding to the rotation angle is transmitted.
It is imported into cpulo via interface 9.

Ｃｐｕｌｏでは、上記により、記憶されている各アーム
長、各アームの太さ、又、周期的に入力されてくる回転
角を基にしてアーム同志の相互干渉を判定する演算がな
される。In Cpulo, as described above, calculations are performed to determine mutual interference between arms based on the stored lengths of each arm, the thickness of each arm, and rotation angles that are periodically input.

次にアーム同志の干渉を判定する計算手続きについて、
ステップを追って、詳述する。Next, regarding the calculation procedure for determining interference between arms,
Step by step and explain in detail.

く第１ステツプ〉あらかじめ、各アームの長さ、太さを入力する手段ｑ（
たとえば、押ボタンＳＷの様なもの）を操作することよ
り入力されているアーム長さと、周期的に回転検出器よ
り入力されてくる回転角を基にして、アーム先端座標、
及び各関節中心座標を求める。1st step〉 A means of inputting the length and thickness of each arm in advance q (
For example, based on the arm length input by operating a push button SW (for example, a push button SW) and the rotation angle periodically input from a rotation detector, the arm tip coordinates,
and determine the center coordinates of each joint.

θ、を第ｉ関節の回転角、ｔ、をそれに対応するアーム
の長さとし、Ｔ、（Ｘ）、　Ｊｉ（Ｙ）、　Ｊｉ（Ｚ）
を第ｉ関節のＸ座標、Ｙ座標、Ｘ座標とすると各関節の
中心座標及びアーム先端座標は、（関節１中心座標）Ｊｌ（Ｘ）　＝　。Let θ be the rotation angle of the i-th joint, t be the length of the corresponding arm, and T, (X), Ji (Y), Ji (Z)
Let be the X coordinate, Y coordinate, and X coordinate of the i-th joint, then the center coordinates of each joint and the arm tip coordinates are (joint 1 center coordinates) Jl(X) = .

Ｊｌ（Ｙ）−〇 ■１（Ｚ）−ｔｌ（関節２中心座標）Ｊｌ（Ｘ）＝ＯＪ２（Ｙ）−〇 ■２（Ｚ）＝１１（Ｚ）＋４（関節３中心座標）１３（ｘ）＝ｔ２ＳＩｎθ２ｃＯｓθ１Ｊｓ（Ｙ）　＝
Ｚ２　ｓｉｎθ２　ｓｉｎθ１■３（Ｚ）二１２（Ｚ）
十ｌ！、２部θ２（関節４中尼・座標）Ｊ４（Ｘ）　＝　Ｔ。（Ｘ）　＋　ｔ３ｓｉｎθ３ＣＯ
３θ１Ｊ４（”）　＝Ｊ３（Ｙ）　＋　／−３ｓｉｎθ
３　ｓｉｎθ１■４（Ｚ）＝１３（Ｚ）＋ｔ３ｃｏｓθ
３（関節５中七・座標）Ｊ５（Ｘ）　＝　Ｊ４（Ｘ）　＋　ｔ４ｓｉｎθ４部θ
１■、（Ｙ）＝■４（Ｙ）＋ｔ４ＳＩｒＩ０４ＳＩｒｌ
θ１■５（Ｚ）−’４（Ｚ）　”ｌ’１４　ｃｏｓθ４
（アーム先端座標）Ｊ６（ｘ）　＝７５（Ｘ）＋Ａ５（ｃｏｓ（９１ｃｏｓ
０５ｓ＋ｎθ４−５１ｎθ伸θ５）Ｉｓ（Ｙ）”ｌ６（
Ｙ）＋ｔ５（ｃｏｓθ１Ｓ石０５　”！＋１ｎｏＩＣＱ
Ｓ０６ｓｌｎθ４）工。（Ｚ）＝Ｊｓ（Ｚ　）　十Ｚｓ
α３θ６α３θ４となる。Jl(Y)-〇■1(Z)-tl (Joint 2 center coordinates) Jl(X)=O J2(Y)-〇■2(Z)=11(Z)+4 (Joint 3 center coordinates) 13( x)=t2SInθ2cOsθ1Js(Y)=
Z2 sinθ2 sinθ1■3(Z)212(Z)
Ten liters! , 2 part θ2 (joint 4 middle/coordinates) J4(X) = T. (X) + t3sinθ3CO
3θ1J4(”) = J3(Y) + /-3sinθ
3 sinθ1■4(Z)=13(Z)+t3cosθ
3 (7 out of 5 joints, coordinates) J5 (X) = J4 (X) + t4sinθ4 part θ
1■, (Y)=■4(Y)+t4SIrI04SIrl
θ1■5(Z)-'4(Z) "l'14 cosθ4
(Arm tip coordinates) J6(x) =75(X)+A5(cos(91cos
05s+nθ4-51nθ elongation θ5)Is(Y)”l6(
Y)+t5(cosθ1S stone 05”!+1noICQ
S06slnθ4) Eng. (Z)=Js(Z) 10Zs
α3θ6α3θ4.

く第２ステツプ〉アーム先端座標および各関節中心座標より、隣り合わな
いアームのセンター間の最小距離を求める０隣り合わない２つのアームを、Ａアーム、Ｂアームとす
る。2nd step> Find the minimum distance between the centers of non-adjacent arms from the arm tip coordinates and each joint center coordinate.0 Let the two non-adjacent arms be A-arm and B-arm.

Ａアーム中心線上の任意の点をＡ　（ｘＡ−ＶＡＴＺＡ
）、Ｂアーム中心線上の任意の点をＢ　（ｘＢ、　ＶＢ
。Any point on the A-arm center line is A (xA-VATZA
), any point on the B arm center line is B (xB, VB
.

ＺＢ）とすると、ＡＢ間の距離の２乗は、Ａ　Ｂ２＝　
（ｘｐ、−ＸＢ　）　２＋　（ｙＡ−ｙＢ　）　＋　（
ｚｐ、−ＺＢ　）　　となる。ZB), then the square of the distance between AB is A B2=
(xp, -XB) 2+ (yA-yB) + (
zp, -ZB).

一方、Ａアームの中心線の一方の端点の座標を、（ｚａ
、ｙａ、ｚａ）、そこからのＡアーム中心線の方向余弦
を、’−ＡＩ　”ＡＩ　”Ａとする。On the other hand, the coordinates of one end point of the center line of the A-arm are (za
, ya, za), and let the direction cosine of the A-arm center line from there be '-AI ``AI ''A.

他方、Ｂアームのそれを、（ｘｌ）＋　Ｙｂ＋　”ｂ）
、ＬＢ、　ｍＢ、　ｎＢとすると、 ”Ａ　＝　ｘａ＋ノ’ＡｋＡ、　　３’Ａ＝Ｙａ−Ｉ−
ｍＡｋＡ＋　　　ｚＡ＝　ｚａ”ＡｋＡ（０≦ｋＡ≦１
）ＸＢ＝Ｘｂ＋ｔＢｋＢ、　ｙＢ＝＝ｙｂ＋ｍＢｋＢ、　
ＺＢ’：Ｚｂ＋ｎＢｋＢ（０≦ｋＢ≦１）となり、距離は、ｋＡ、ｋＢの関数となる。On the other hand, that of the B arm is (xl) + Yb + "b)
, LB, mB, nB, ``A = xa+ノ'AkA, 3'A=Ya-I-
mAkA+ zA= za”AkA (0≦kA≦1
) XB=Xb+tBkB, yB==yb+mBkB,
ZB': Zb+nBkB (0≦kB≦1), and the distance is a function of kA and kB.

Ａ′Ｂ２＝ｆ（ｋＡ、ｋＢ） ””　（ｘａ−”ｂ”（’ＡｋＡ−ｔＢｋＢ）’＋ｉ　
（ｙａ−３’ｂ）”（ｍＡｋＡ−”ｂｋＢ）　１２＋　
ｉ　（ｚａ−ｚｂ）＋（ｎＡｋＡ−ｎＢｋＢ）１２Ａア
ーム、Ｂアームの２つのアームの中心線間の最小距離は
、○≦ｋＡ≦１，０≦ｋＢ≦１でｆ　（ｋＡ、　ｋＢ）
を最小にすればよい。A′B2=f(kA,kB) ”” (xa-”b”('AkA-tBkB)'+i
(ya-3'b)"(mAkA-"bkB) 12+
i (za-zb) + (nAkA-nBkB) 12 The minimum distance between the center lines of the two arms, A arm and B arm, is f (kA, kB) with ○≦kA≦1, 0≦kB≦1
should be minimized.

ｋＡ（４２Ｂ　＋ｍＡｍＢ　＋　ｎＡｎＢ　）　ｋＢ　
−−ＣｔＡ（ｘ　ａ−ｘ　ｂ）　＋ｍＡ（ｙａ−ｙＢ）
＋ｎＡ（ｚａ−２ｂ）〕−（４４＋ｒｎＡｍＢ　＋　ｎ
ＡｎＢ　）ｋｐ、＋ｋＢ　＝＝　ｔＢ、（Ｘ　ａ−Ｘ　
ｂ）　＋ｒｎＢ（ｙａ　ＹＢ）”Ｂ（ｚａ−２ｂ）上記、連立方程式の解をｋＡ、　ｋＢ　　とする。kA (42B + mAmB + nAnB) kB
--CtA(x a-x b) +mA(ya-yB)
+nA(za-2b)]-(44+rnAmB + n
AnB ) kp, +kB == tB, (X a-X
b) +rnB(ya YB)”B(za-2b) Let kA and kB be the solutions of the above simultaneous equations.

又、前記、ｆ（ｋＡ、ｋＢ）は、凹関数であることより
、ｍｉｎ　（ＬＢ２）は、ｋＮ　、　ｋＢ”　の範囲に
より、次表の如くなる。Furthermore, since f(kA, kB) is a concave function, min (LB2) becomes as shown in the following table depending on the range of kN and kB.

次に、相互のアームの干渉の判定を行なう０１１Ｊす合
わない２つのアームＡ、Ｂのセンター間の最小距離をｔ
ＡＢ＊とし、アームＡ、Ｂの半径をｒＡｌ　”Ｂとする
と々Ｂ　≦ｒＡ＋　ｒＢの時、２つのアームは、干渉するおそれがある０全ての
隣り合わないアームについてｌＡＢ≦ｒＡ＋ｒＢ＋ε の時、干渉するおそれありと見なす。。Next, the minimum distance between the centers of two arms A and B that do not meet each other is determined as t.
Let AB* be the radius of arms A and B, and let the radius of arms A and B be rAl "B. When B ≤ rA + rB, there is a risk of interference between the two arms. For all non-adjacent arms, when lAB ≤ rA + rB + ε, there will be interference. It is considered to be a risk.

前記の如く、ｃｐｕｌｏより、干渉する可能性があると
見なされた場合、ｃｐｕｌｏより、ロボット制御装置７
に停止信号を送り、ロボットを止める〇発明の効果この発明によれば、１　ティーチング時、ロボットアー人間、相互の干渉を
気にせず動作プログラムが作成出来る０２　外部系で、
動作プログラムを作成しても、実際にロボットを動作さ
せる場合、アーム相互干渉の危険性がない。As mentioned above, if cpulo determines that there is a possibility of interference, cpulo will
Sends a stop signal to stop the robot〇Effects of the invention According to this invention, 1. During teaching, operation programs can be created without worrying about mutual interference between the robot and humans. 02. With the external system,
Even if a motion program is created, there is no risk of arms interfering with each other when actually operating the robot.

３　アームの長さ、太さを変えても、それに対応して、
アーム間の相互干渉が回避出来る。3 Even if you change the arm length and thickness,
Mutual interference between arms can be avoided.

４　アームが、他の断面形状であっても、円筒で、近似
することにより、容易に拡張が出来る。4 Even if the arm has a different cross-sectional shape, it can be easily expanded by approximating it to a cylinder.

[Brief explanation of the drawing]

第１図は本発明の制御方法を示す説明図、第２図は本発
明の一実施例における多関節ロボットの構成および制御
系統を示す系統図、第３図は本発明の方法および演算手
順を示すフローチャートである。Ａ・・・・・アーム長さ、太さの入力手段、Ｂ・・・・
・・アーム回転角測定手段、Ｃ・・・・・・アーム先端
座標および関節中心座標計算手段、Ｄ・・・・アーム間
最小距離計算手段およびアーム干渉判定手段、Ｅ・・・
・・ロボット動作停止手段。代理人の氏名　弁理士　中　尾　敏　男　ほか１名第１
図第２図FIG. 1 is an explanatory diagram showing the control method of the present invention, FIG. 2 is a system diagram showing the configuration and control system of an articulated robot in an embodiment of the present invention, and FIG. 3 is a diagram showing the method and calculation procedure of the present invention. FIG. A: Input method for arm length and thickness, B...
... Arm rotation angle measuring means, C ... Arm tip coordinate and joint center coordinate calculation means, D ... Inter-arm minimum distance calculation means and arm interference determination means, E ...
...Means for stopping robot operation. Name of agent: Patent attorney Toshio Nakao and 1 other person No. 1
Figure 2

Claims

[Scope of Claims] Means for inputting the length and thickness of each arm of an articulated robot, a rotation detector for detecting the rotation angle of each joint, and an input signal for the rotation angle detected by the rotation detector. The robot has means for calculating the arm tip coordinates and the center coordinates of each joint, means for calculating the minimum distance between each arm, and means for determining interference between the arms, and from the means for determining interference between the arms, the robot A method for controlling an articulated robot configured to control movement commands to the robot and avoid mutual interference between the arms.