JP2000079582A - Controlling method for articulated robot arm - Google Patents
Controlling method for articulated robot armInfo
- Publication number
- JP2000079582A JP2000079582A JP10251361A JP25136198A JP2000079582A JP 2000079582 A JP2000079582 A JP 2000079582A JP 10251361 A JP10251361 A JP 10251361A JP 25136198 A JP25136198 A JP 25136198A JP 2000079582 A JP2000079582 A JP 2000079582A
- Authority
- JP
- Japan
- Prior art keywords
- angle
- robot arm
- shoulder
- calculating
- waist
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Manipulator (AREA)
Abstract
Description
【0001】[0001]
【発明の属する技術分野】この発明は、指先或いは腕に
設置した位置センサにより検出した位置姿勢情報でロボ
ットアームを自在に動作させることができる多関節ロボ
ットアームの制御方法に関するものである。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for controlling an articulated robot arm that can freely operate a robot arm based on position and orientation information detected by a position sensor installed on a fingertip or an arm.
【0002】[0002]
【従来の技術】(1)従来技術の構成の説明 空間の位置姿勢(X、Y、Z方向の距離及び、ロール、
ピッチ、ヨー角)から、ロボットアームの関節角度を求
めることを逆運動という。図12に5軸多関節ロボット
アームの側面図を示す、各アームの長さ及び角度を以下
のように定義する。2. Description of the Related Art (1) Description of Configuration of Prior Art Position and orientation of space (distance in X, Y, and Z directions, roll,
Obtaining the joint angle of the robot arm from the pitch and yaw angle) is called reverse motion. FIG. 12 shows a side view of a five-axis articulated robot arm. The length and angle of each arm are defined as follows.
【0003】 40 Zh:ショルダセンタ高さ 41 Ls:ショルダ長さ 42 Le:エルボ長さ 43 Lw:リスト長さ 44 Lt:ツール長さ 45 Lh:ショルダセンタ オフセット 46 θw :ウエスト角 (水平面55に対する回転角
度、時計方向を正、範囲を+100゜、-100゜とする) 47 θs :ショルダ角 (水平面55からの仰角、上向
を正、範囲を+100゜、 -30゜とする) 48 θe :エルボ角 (水平面55からの仰角、上向
を正、範囲を+0゜、 -110゜とする) 49 θp :ピッチ角 (水平面55からの仰角、上向
を正、範囲を+90゜、 -90゜とする) 50 θr :ロール角 (ピッチ角49との相対角、外
から見て時計方向正、範囲を+180゜、-180゜)40 Zh: Shoulder center height 41 Ls: Shoulder length 42 Le: Elbow length 43 Lw: Wrist length 44 Lt: Tool length 45 Lh: Shoulder center offset 46 θw: Waist angle (rotation with respect to horizontal plane 55 Angle, clockwise direction is positive, range is + 100 °, -100 °) 47 θs: shoulder angle (elevation angle from horizontal plane 55, upward is positive, range is + 100 °, -30 °) 48 θe : Elbow angle (elevation angle from horizontal plane 55, upward is positive, range is + 0 °, -110 °) 49 θp: Pitch angle (elevation angle from horizontal plane 55, upward is positive, range is + 90 °, 50 θr: Roll angle (Relative angle to pitch angle 49, positive clockwise when viewed from the outside, range + 180 °, -180 °)
【0004】ハンド先端位置姿勢54は、(x、y、
z、p、r)で表される。ここで、x、y、zはハンド
先端の位置座標、p(ピッチ)、r(ロール)はハンド
姿勢である。The hand tip position / posture 54 is (x, y,
z, p, r). Here, x, y, and z are the position coordinates of the tip of the hand, p (pitch), and r (roll) are the hand posture.
【0005】(2)従来技術の作用及び動作の説明 次に、従来技術の作用及び動作を図12に示すロボット
アームの側面図に基づいて説明する。 ハンド先端位置
姿勢54を(x、y、z、p、r)、リストセンタ位置
53を(x1、y1、z1)ショルダセンタ位置52を
(x2、y2、z2)とする。(2) Description of the operation and operation of the prior art Next, the operation and operation of the prior art will be described with reference to the side view of the robot arm shown in FIG. The hand tip position / posture 54 is (x, y, z, p, r), the wrist center position 53 is (x1, y1, z1), and the shoulder center position 52 is (x2, y2, z2).
【0006】ハンド先端姿勢の定義から ピッチ角49 θp=p ロール角50 θr=rFrom the definition of the hand tip attitude, pitch angle 49 θp = p roll angle 50 θr = r
【0007】図13はリスト部53を上面から見た場
合、図14はリスト部53を側面から見た場合の図であ
る。 図13よりウエスト角46 θw=tan-1(x/
y)FIG. 13 is a diagram when the wrist unit 53 is viewed from the top, and FIG. 14 is a diagram when the wrist unit 53 is viewed from the side. From FIG. 13, the waist angle 46 θw = tan −1 (x /
y)
【0008】θs、θeを求める前に準備として、まずリ
ストセンタ位置53(x1、y1、z1)を求める。図1
4のL1 60はリストセンタ53からハンド先端54ま
でのX-Y平面へ 投影した長さである。 L1=(Lw+L
t)・cosθp 図13、図14より x1=x−L1・sinθw y1=y−L1・cosθw z1=Z−(Lw+Lt)・sinθpAs a preparation before obtaining θs and θe, a list center position 53 (x1, y1, z1) is first determined. FIG.
L1 60 of 4 is the length projected from the wrist center 53 to the hand tip 54 on the XY plane. L1 = (Lw + L
t) · cos θp From FIGS. 13 and 14, x1 = x−L1 · sin θw y1 = y−L1 · cos θw z1 = Z− (Lw + Lt) · sinθp
【0009】次に、ショルダセンタ位置52(x2、y
2、z2)を求める。 図15にショルダ・エルボ軸部分
の側面図を示す。 x2=Lh・sinθw y2=Lh・cosθw z2=ZhNext, the shoulder center position 52 (x2, y
2, z2). FIG. 15 shows a side view of a shoulder / elbow shaft portion. x2 = Lh · sin θw y2 = Lh · cos θw z2 = Zh
【0010】ショルダ軸、エルボ軸の角度θs、θeにつ
いては、ショルダセンタ52を原点とし、ショルダ軸、
エルボ軸の2軸ロボットと考えれば、水平関節形ロボッ
トと同様の考え方で解くことができる。 θs=θ1 θe=θ1+θ2 ここで、θ2、θ1は θ2=θ2’−π θ1=θ3+θ4Regarding the angles θs and θe of the shoulder axis and the elbow axis, the shoulder center 52 is used as the origin,
If the robot is considered to be a two-axis robot with an elbow axis, it can be solved by the same concept as a horizontal joint type robot. θs = θ1 θe = θ1 + θ2 where θ2 and θ1 are θ2 = θ2'-π θ1 = θ3 + θ4
【0011】ショルダセンタ52からリストセンタ53
までをX-Y平面へ投影した長さR1は以下の式から表せ
る。[0011] From the shoulder center 52 to the wrist center 53
Is projected onto the XY plane, the length R1 can be expressed by the following equation.
【0012】R1=√((x1−x2)2+(y1−y2)2) R=√(R12+(z1−Zh)2) 余弦定理より cosθ2’=(Ls2+Le2−R2)/2・Ls・Le θ2’= cos-1((Ls2+Le2−R2)/2・Ls・L
e) θ3 = tan-1((z1−Zh)/R1) θ4 = cos-1((Ls2+R2−Le2)/2・Ls・
R)[0012] R1 = √ ((x1-x2 ) 2 + (y1-y2) 2) R = √ (R1 2 + (z1-Zh) 2) cosθ2 than the cosine theorem '= (Ls 2 + Le 2 -R 2) / 2 · Ls · Le θ2 ′ = cos −1 ((Ls 2 + Le 2 −R 2 ) / 2 · Ls · L
e) θ3 = tan -1 (( z1-Zh) / R1) θ4 = cos -1 ((Ls 2 + R 2 -Le 2) / 2 · Ls ·
R)
【0013】ショルダ角47は以下の式から求められ
る。The shoulder angle 47 is obtained from the following equation.
【0014】 θs =θ1 =θ3+θ4 =tan-1((z1−Zh)/R1)+cos-1((Ls2+R2−Le2)/2・ Ls・R)[0014] θs = θ1 = θ3 + θ4 = tan -1 ((z1-Zh) / R1) + cos -1 ((Ls 2 + R 2 -Le 2) / 2 · Ls · R)
【0015】エルボ角48は以下の式から求められる。The elbow angle 48 is obtained from the following equation.
【0016】 θe =θ1+θ2 =θ1+(θ2'−π) =tan-1((z1−Zh)/R1)+cos-1((Ls2 +R2−Le2)/2・Ls・R)+(cos-1((Ls2+Le2−R2)/2・Ls・ Le)−π) 以上の計算方法からウエスト角46,ショルダ角47,
エルボ角48、ピッチ角49、ロール角50のそれぞれ
が求められる。Θe = θ1 + θ2 = θ1 + (θ2′−π) = tan −1 ((z1−Zh) / R1) + cos −1 ((Ls 2 + R 2 −Le 2 ) / 2 · Ls · R) + (cos -1 ((Ls 2 + Le 2 −R 2 ) / 2 · Ls · Le) −π From the above calculation method, the waist angle 46, the shoulder angle 47,
Each of the elbow angle 48, the pitch angle 49, and the roll angle 50 is obtained.
【0017】[0017]
【発明が解決しようとする課題】従来技術の課題の説明
位置センサのロール角出力は、±180゜であるため、
±180゜を超えて位置センサを回転させた場合は、角
度の符号が反転し急激にロボットアームのハンドが逆回
転する。また、ロボットアームのウエスト軸は±100
゜の範囲で可動するため、ポジションによりウエスト角
の計算式は異なってくる。The roll angle output of the position sensor is ± 180 °.
When the position sensor is rotated by more than ± 180 °, the sign of the angle is reversed and the hand of the robot arm is rapidly reversely rotated. The waist axis of the robot arm is ± 100
Because it moves in the range of 異 な っ, the formula for calculating the waist angle differs depending on the position.
【0018】さらに、ウエスト角が80゜から100゜及
び、−80゜から−100゜の場合に、ハンド先端はロボ
ットアームのショルダセンタから見て前面と背面の2つ
の解が存在する。したがって、従来の計算方法ではウエ
スト角を特定できない。Further, when the waist angle is between 80 ° and 100 ° and between -80 ° and -100 °, there are two solutions at the front end of the hand when viewed from the shoulder center of the robot arm. Therefore, the waist angle cannot be specified by the conventional calculation method.
【0019】ショルダ角もリストセンタがショルダセン
タから見て前面の場合と背面の場合とでは計算方法を変
えなければならない。さらに、位置センサの位置姿勢情
報でロボットアームを制御するには、位置姿勢情報の入
力範囲の限定、計算角度の範囲の限定、ロボットアーム
の動作範囲の限定などのエラー処理をしなければならな
いなどの問題点があった。The calculation method of the shoulder angle must be changed between the case where the wrist center is the front and the case where the wrist center is the back as viewed from the shoulder center. Furthermore, in order to control the robot arm with the position and orientation information of the position sensor, error processing must be performed, such as limiting the input range of the position and orientation information, limiting the range of calculation angles, and limiting the operation range of the robot arm. There was a problem.
【0020】この発明は、このような問題点を解決する
ためになされたもので、位置センサの位置姿勢情報でロ
ボットアームを自在に動作させることができる多関節ロ
ボットアームの制御方法を提供するものである。The present invention has been made to solve such a problem, and provides a control method of an articulated robot arm that can freely operate a robot arm based on position and orientation information of a position sensor. It is.
【0021】[0021]
【課題を解決するための手段】請求項1の発明に係る多
関節ロボットアームの制御方法は、空間における可動体
の位置姿勢情報を位置センサにより検出する検出工程、
この検出された位置姿勢情報に基づいて制御対象である
多関節ロボットアームの関節角度を演算手段にて演算す
る演算工程、上記位置姿勢情報に沿って前記多関節ロボ
ットを動作させる場合の動作範囲限定値を前記関節角度
に基づいて設定する設定工程とを有するものである。According to a first aspect of the present invention, there is provided a method for controlling an articulated robot arm, comprising the steps of: detecting a position and orientation information of a movable body in a space by a position sensor;
A calculating step of calculating a joint angle of a multi-joint robot arm to be controlled based on the detected position and orientation information by a calculating means, and limiting an operation range when operating the multi-joint robot according to the position and orientation information Setting a value based on the joint angle.
【0022】請求項2の発明に係る多関節ロボットアー
ムの制御方法は、所定の角度閾値と現在のロール角、前
回のロール角よりロール角を計算するロール角演算工程
を有するものである。A method for controlling an articulated robot arm according to a second aspect of the present invention includes a roll angle calculating step of calculating a roll angle from a predetermined angle threshold, a current roll angle, and a previous roll angle.
【0023】請求項3の発明に係る多関節ロボットアー
ムの制御方法は、所定の角度閾値と現在のハンド先端位
置、前回ウエスト角の判断よりロボットアームの方向を
決定し、ウエスト角を計算するウエスト角演算工程を有
するものである。According to a third aspect of the present invention, there is provided a method for controlling an articulated robot arm, wherein the direction of the robot arm is determined based on a predetermined angle threshold value, the current hand tip position, and the previous determination of the waist angle, and the waist angle is calculated. It has an angle calculation step.
【0024】請求項4の発明に係る多関節ロボットアー
ムの制御方法は、リストセンタ位置を計算しリストセン
タ位置によりショルダ角を計算するショルダ角演算工程
を有したものである。A control method for an articulated robot arm according to a fourth aspect of the present invention includes a shoulder angle calculating step of calculating a wrist center position and calculating a shoulder angle based on the wrist center position.
【0025】請求項5の発明に係る多関節ロボットアー
ムの制御方法は、計算した各関節角度からハンド先端位
置、リスト先端位置を計算しロボットアームが設定した
安全動作範囲を超えないように動作範囲を限定する設定
工程を有するものである。According to a fifth aspect of the invention, there is provided a method for controlling an articulated robot arm, wherein a hand tip position and a wrist tip position are calculated from the calculated joint angles so that the robot arm does not exceed the set safe operation range. Is set.
【0026】[0026]
【発明の実施の形態】(1)実施の形態の構成の詳細な
説明 実施の形態1 図1はこの発明による5軸多関節ロボットアームの制御
方法の一実施の形態の全体構成図である。図1におい
て、1は空間における例えば指先、腕等の位置姿勢
(X、Y、Z方向の距離及び、ロール、ピッチ、ヨー
角)を検出可能な位置センサ、2は位置センサ1を制御
し、制御装置へ位置姿勢情報を送るコンピュータ、3は
コンピュータ2からの位置姿勢情報を入力とし、位置姿
勢情報から制御対象であるロボットアームを制御する制
御装置、4は制御装置3内で位置姿勢情報から関節角度
を計算する演算部、5は計算された関節角度に従って制
御装置3により動作制御される5軸多関節ロボットアー
ム(以下、ロボットアームと記載する)である。DESCRIPTION OF THE PREFERRED EMBODIMENTS (1) Detailed Description of Embodiment Configuration Embodiment 1 FIG. 1 is an overall configuration diagram of an embodiment of a control method for a five-axis articulated robot arm according to the present invention. In FIG. 1, reference numeral 1 denotes a position sensor capable of detecting the position and orientation (distance in the X, Y, and Z directions, roll, pitch, and yaw angle) of, for example, a fingertip, an arm, and the like, and 2 controls the position sensor 1; A computer that sends position and orientation information to the control device, 3 receives a position and orientation information from the computer 2, and a control device that controls a robot arm to be controlled from the position and orientation information. The arithmetic unit 5 for calculating the joint angle is a five-axis articulated robot arm (hereinafter, referred to as a robot arm) whose operation is controlled by the control device 3 according to the calculated joint angle.
【0027】以上のような構成により、演算部4はロボ
ットアーム5の関節角度の計算として、位置センサ1の
位置姿勢情報を入力して位置姿勢情報の範囲を限定後、
ある角度閾値と現在のロール角、前回のロール角の判断
結果よりロール角、ある角度閾値と現在のハンド先端位
置、前回ウエスト角の判断結果によりロボットアーム5
の方向を決定しウエスト角、計算したリストセンタ位置
によりショルダ角のそれぞれを計算し、これら計算した
各関節角度からハンド先端位置、リスト先端位置を計算
してロボットアーム5が設定した動作範囲を超えないよ
うに動作範囲を限定する。尚、ロボットアーム5の各
軸、各アーム長の条件は従来技術の構成と同様である。With the above configuration, the calculation unit 4 inputs the position and orientation information of the position sensor 1 to calculate the joint angle of the robot arm 5 and limits the range of the position and orientation information.
The robot arm 5 based on a certain angle threshold value and the present roll angle, the roll angle based on the result of the previous roll angle determination, the certain angle threshold value and the current hand tip position, the previous result on the waist angle determination
Is determined, the shoulder angle is calculated based on the waist angle and the calculated list center position, and the hand tip position and the list tip position are calculated from the calculated joint angles to exceed the operation range set by the robot arm 5. Limit the operating range so that there is no Note that the conditions of each axis and each arm length of the robot arm 5 are the same as those in the configuration of the related art.
【0028】(2)実施の形態の作用、動作の詳細な説
明 次に上記実施の形態1の動作を図1〜図11を参照しな
がら説明する。図2はロボットアーム5を上から見たウ
エスト角の可動範囲を示す図、図3、図4はロボットア
ーム5を横から見たショルダ角を示す図、図5は計算の
初期設定処理を示すフローチャート、図6〜図11は位
置センサ1からの位置姿勢情報をロボットアーム5の動
作範囲に限定して関節角度を計算する処理を示すフロー
チャートである。(2) Detailed Description of Operation and Operation of the Embodiment Next, the operation of the first embodiment will be described with reference to FIGS. FIG. 2 is a diagram showing the movable range of the waist angle when the robot arm 5 is viewed from above, FIGS. 3 and 4 are diagrams showing the shoulder angle when the robot arm 5 is viewed from the side, and FIG. FIGS. 6 to 11 are flowcharts showing processing for calculating the joint angle by limiting the position and orientation information from the position sensor 1 to the operation range of the robot arm 5.
【0029】図1の構成において、例えば位置センサ1
を操縦者の手に取りつけ、操縦者の腕と同様の動きをロ
ボットアーム5にさせる。この時、位置姿勢情報はコン
ピュータ2から制御装置3の演算部4へ送られ、演算部
4で関節角度を計算し、この関節角度でロボットアーム
5を操縦者の腕と同様に動作させる。In the configuration shown in FIG.
Is attached to the operator's hand, and the robot arm 5 makes the same movement as the operator's arm. At this time, the position and orientation information is sent from the computer 2 to the calculation unit 4 of the control device 3, and the calculation unit 4 calculates the joint angle, and operates the robot arm 5 in the same manner as the operator's arm at this joint angle.
【0030】次に制御装置3における演算部4の処理を
説明する。先ず、初めに、図5に示す初期設定フローチ
ャートのブロック10〜12でセンサ入力リミット値
(センサX、Y、Z、センサピッチ角、センサロール角
の最大値及び最小値)、アーム制御角リミット値(ウエ
スト角、ショルダ角、エルボ角、ロール角、ピッチ角の
最大値、最小値)、及び安全範囲リミット値(アーム動
作の安全範囲X、Y、Z)の初期設定をする。Next, the processing of the arithmetic unit 4 in the control device 3 will be described. First, in blocks 10 to 12 of the initial setting flowchart shown in FIG. 5, sensor input limit values (sensor X, Y, Z, sensor pitch angle, maximum value and minimum value of sensor roll angle), arm control angle limit value (The maximum value and the minimum value of the waist angle, the shoulder angle, the elbow angle, the roll angle, and the pitch angle), and the safe range limit values (the safe ranges X, Y, and Z of the arm operation) are initialized.
【0031】即ち、ブロック10ではロボットアーム5
の最大動作範囲から位置センサ入力に対する最大、最小
値を設定する。これはロボ ットアーム5の各腕の長さ
で決まる。ブロック11ではロボットアーム5への制御
角度の最大、最小値を設定する。これはロボットアーム
5の各軸の可動範囲で決まる。ブロック12ではロボッ
トアーム5が安全に動作する安全範囲を設定する。これ
はロボットアーム5と床の高さ、また周囲の環境などに
より決まる。That is, in the block 10, the robot arm 5
The maximum and minimum values for the position sensor input are set from the maximum operation range of. This is determined by the length of each arm of the robot arm 5. In block 11, the maximum and minimum values of the control angle to the robot arm 5 are set. This is determined by the movable range of each axis of the robot arm 5. In block 12, a safe range in which the robot arm 5 operates safely is set. This is determined by the height of the robot arm 5 and the floor, the surrounding environment, and the like.
【0032】次に図6〜図11の初期設定フローチャー
トに従って関節角度の計算処理を説明する。図6におけ
るブロック13では、コンピュータ2を通して位置セン
サ1より演算部4へ入力した可動体のX座標、Y座標、
Z座標、ロール角、ピッチ角の位置姿勢情報が、図5の
計算フローチャートにおけるブロック10〜12で設定
した値の範囲内にあるかどうかチェックし、範囲外であ
ればその値を各位置姿勢情報の最大値または最小値に設
定する。Next, the calculation processing of the joint angle will be described with reference to the initial setting flowcharts of FIGS. In block 13 in FIG. 6, the X coordinate and Y coordinate of the movable body input from the position sensor 1 to
It is checked whether the position and orientation information of the Z coordinate, the roll angle, and the pitch angle are within the range of the values set in blocks 10 to 12 in the calculation flowchart of FIG. Set to the maximum or minimum value of.
【0033】即ち、検出したセンサXの値が初期設定し
たセンサ最大値より大きいかを判定し、大きければその
時のセンサXの値をセンサ最大値に設定する。しかし、
小さければ、センサXの値が初期設定したセンサ最小値
より小さいかを判定し、小さければその時のセンサXの
値をセンサ最小値に設定する。That is, it is determined whether or not the detected value of the sensor X is larger than the initially set sensor maximum value, and if it is larger, the value of the sensor X at that time is set as the sensor maximum value. But,
If smaller, it is determined whether the value of the sensor X is smaller than the initially set sensor minimum value. If smaller, the value of the sensor X at that time is set as the sensor minimum value.
【0034】以上の処理をセンサXに続いてセンサY,
Zに関しても行う。また、センサロール角、センサピッ
チ角に関しても各角の初期設定された最大値、最小値と
の比較を行い、各角が初期設定された範囲外であれば、
その時のセンサ角度を初期設定された最大値または最小
値に設定する。The above processing is performed after the sensor X and the sensors Y,
This is also performed for Z. The sensor roll angle and the sensor pitch angle are also compared with the initially set maximum value and minimum value of each angle, and if each angle is outside the initially set range,
The sensor angle at that time is set to the initially set maximum value or minimum value.
【0035】次に各関節角の計算方法とエラー処理を図
7の計算フローチャートを用いて説明する。 ロール角の計算、エラー処理。 位置センサのロール角出力は、±180゜の範囲であ
る。したがって、例えば+160゜からプラス方向へ3
0゜変化した場合、+190゜ではなく−170゜と出力
される。この値をそのまま使用するとロボットアーム5
のハンドが急に反転することになる。Next, a method of calculating each joint angle and error processing will be described with reference to a calculation flowchart of FIG. Calculation of roll angle, error handling. The roll angle output of the position sensor is in the range of ± 180 °. Therefore, for example, from + 160 ° to +3
If it changes by 0 °, it is output as -170 ° instead of + 190 °. If this value is used as it is, robot arm 5
Is suddenly reversed.
【0036】−180゜を超えた場合も同様の現象が起
こる。これを防ぐため、ブロック14では位置姿勢情報
は連続して変化することを利用し、ある角度閾値と現在
のロール角、前回のロール角を比較する。そして、+1
80゜を超えてプラス方向へ変化したと判断した場合は
ロール角を最大値(+180゜)に、−180゜を超えて
マイナス方向へ変化したと判断した場合はロール角を最
小値(−180゜)にする。The same phenomenon occurs when the angle exceeds -180 °. In order to prevent this, the block 14 uses a fact that the position and orientation information changes continuously, and compares a certain angle threshold value with the current roll angle and the previous roll angle. And +1
When it is determined that the angle has changed in the plus direction beyond 80 °, the roll angle is set to the maximum value (+ 180 °), and when it is determined that the angle has changed in the minus direction beyond −180 °, the roll angle is set to the minimum value (−180 °).゜).
【0037】ウエスト角の計算、エラー処理。 ブロック15は、ロボットアーム5がY軸上にある場合
の処理である。Y軸上にある場合は+90゜か、−90゜
のいずれかであるが判断できない、そこで前回のウエス
ト角がプラスであればウエスト角を+90゜、マイナス
であれば−90゜にする。ブロック16は、ロボットア
ーム5がX軸上にある場合の処理である。X軸上にある
場合はウエスト角を0゜にする。Calculation of waist angle, error processing. Block 15 is a process when the robot arm 5 is on the Y axis. If it is on the Y axis, it is either + 90 ° or −90 ° but cannot be determined. Therefore, if the previous waist angle is plus, the waist angle is set to + 90 °, and if it is minus, −90 °. Block 16 is processing when the robot arm 5 is on the X axis. When it is on the X axis, the waist angle is set to 0 °.
【0038】ロボットアーム5のウエスト軸は、±10
0゜の範囲で動作するため、ハンド先端がショルダセン
タより前面にある場合、図2に示すようにA、B、C、
Dの4つのエリアが存在する。The waist axis of the robot arm 5 is ± 10
Since the hand operates in the range of 0 °, when the hand tip is in front of the shoulder center, A, B, C,
There are four areas of D.
【0039】また、ハンド先端がショルダセンタより背
面にある場合もA、B、C、Dの4つのエリアが存在す
る。したがって、ハンド先端が+80゜〜+100゜及び
−80゜〜−100゜の範囲にある場合、求めるウエスト
角の解が2つ存在することになる。Also, when the tip of the hand is behind the shoulder center, there are four areas A, B, C and D. Therefore, when the tip of the hand is in the range of + 80 ° to + 100 ° and -80 ° to -100 °, there are two solutions of the waist angle to be obtained.
【0040】図7及び図8の計算フローチャートにおけ
るブロック17〜20では、位置姿勢情報は連続である
ことを利用し、ハンド先端の位置するエリアと、前回ウ
エスト角とある角度閾値の比較によりロボットアーム5
の方向を判断しウエスト角を決定する。In blocks 17 to 20 in the calculation flowcharts of FIGS. 7 and 8, the position and orientation information is continuous, and the robot arm is compared by comparing the area where the tip of the hand is located with the previous waist angle and a certain angle threshold. 5
Judge the direction and determine the waist angle.
【0041】ブロック17においては、ハンド先端54
がエリアAにあり前回ウエスト角が大成りイコール−4
5であればウエスト角をtan−1(センサY/センサ
X)/PIとし、しかしエリアAに無くそして前回ウエ
スト角が大成りイコール−45でなければ、エリアAと
前回ウエスト角が−45より小さいかを判定し、そうで
あればウエスト角をtan−1(センサY/センサX)
/PI−180としてウエスト角エリアAのエラー処理
を行う。In block 17, the hand tip 54
Is in area A, and the previous waist angle was large.
If it is 5, the waist angle is tan -1 (sensor Y / sensor X) / PI, but if it is not in area A and the previous waist angle is not equal to -45, the area A and the previous waist angle are -45. It is determined whether it is small, and if so, the waist angle is tan -1 (sensor Y / sensor X)
Error processing of the waist angle area A is performed as / PI-180.
【0042】ブロック18においては、ハンド先端54
がエリアBにあり前回ウエスト角が小成りイコール45
であればウエスト角をtan−1(センサY/センサ
X)/PIとし、しかしエリアBに無く前回ウエスト角
が小成りイコール−45でなければ、エリアBに有りそ
して前回ウエスト角が45より大きいかを判定し、そう
であればウエスト角をtan−1(センサY/センサ
X)/PI+180としてウエスト角エリアBのエラー
処理を行う。In block 18, the hand tip 54
Is in area B and the last time the waist angle was small 45
If so, the waist angle is set to tan -1 (sensor Y / sensor X) / PI, but if not in area B and the previous waist angle is not equal to -45, it is in area B and the previous waist angle is greater than 45 Is determined, and if so, the waist angle is set to tan −1 (sensor Y / sensor X) / PI + 180, and error processing of the waist angle area B is performed.
【0043】ブロック19においては、ハンド先端がエ
リアCにあり前回ウエスト角が大成りイコール45であ
ればウエスト角をtan−1(センサY/センサX)/
PIとし、しかしエリアCに無く前回ウエスト角が大成
りイコール45でなければ、エリアCにあり前回ウエス
ト角が45より小さいかを判定し、そうであればウエス
ト角をtan−1(センサY/センサX)/PI+18
0としてウエスト角エリアCのエラー処理を行う。In block 19, if the tip of the hand is in area C and the previous waist angle is large and equal 45, the waist angle is set to tan -1 (sensor Y / sensor X) /
PI, however, if it is not in the area C and the previous waist angle is not equal to 45, it is determined whether it is in the area C and the previous waist angle is smaller than 45, and if so, the waist angle is set to tan −1 (sensor Y / Sensor X) / PI + 18
Error processing is performed on the waist angle area C as 0.
【0044】ブロック20においては、ハンド先端がエ
リアDにあり前回ウエスト角が小成りイコール−45で
あればウエスト角をtan−1(センサY/センサX)
/PIとし、しかしエリアDに無く前回ウエスト角が小
成りイコール−45でなければ、エリアDに有り前回ウ
エスト角が−45より大きいかを判定し、そうであれば
ウエスト角をtan−1(センサY/センサX)/PI
+180としてウエスト角エリアCのエラー処理を行
う。In block 20, if the tip of the hand is in the area D and the previous waist angle is equal to -45, the waist angle is tan -1 (sensor Y / sensor X).
/ PI, but if it is not in area D and the previous waist angle is not equal to minus 45, it is determined whether it is in area D and the previous waist angle is larger than -45, and if so, the waist angle is tan -1 ( Sensor Y / Sensor X) / PI
Error processing of the waist angle area C is performed as +180.
【0045】図8の計算フローチャートにおけるブロッ
ク21はウエスト角計算値が、ウエスト軸可動範囲を超
えた場合のエラー処理である。即ち、計算されたウエス
ト角が初期設定された最小値或いは最大値の範囲をはず
れた場合は、ウエスト角を設定された最小値或いは最大
値に設定する。Block 21 in the calculation flowchart of FIG. 8 is an error process when the calculated waist angle exceeds the movable range of the waist axis. That is, when the calculated waist angle is out of the range of the initially set minimum value or maximum value, the waist angle is set to the set minimum value or maximum value.
【0046】図9の計算フローチャートにおけるブロッ
ク22はアークコサイン計算時にエラーが発生しないよ
う、引数を−1〜+1の範囲にするための処理である。Block 22 in the calculation flowchart of FIG. 9 is a process for setting the argument in the range of -1 to +1 so that no error occurs during the arc cosine calculation.
【0047】ショルダ角の計算、エラー処理:ショルダ
角の計算では、ロボットアーム5のリストセンタ53が
ショルダセンタ52より背面に位置する場合がある。図
3にロボットアーム5のリストセンタ53がショルダセ
ンタ52より背面にある場合のショルダ角を示し、図4
にロボットアーム5のリストセンタ53がショルダセン
タ52より前面にある場合のショルダ角を示す。Calculation of shoulder angle, error processing: In calculation of the shoulder angle, the wrist center 53 of the robot arm 5 may be located behind the shoulder center 52 in some cases. FIG. 3 shows a shoulder angle when the wrist center 53 of the robot arm 5 is located behind the shoulder center 52, and FIG.
Shows the shoulder angle when the wrist center 53 of the robot arm 5 is located in front of the shoulder center 52.
【0048】図3の場合と、図4の場合ではリストセン
タ53の位置により同じ計算式ではショルダ角は求めら
れない。図9及び図10の計算フローチャートにおける
ブロック23〜26ではハンド先端のエリアとリストセ
ンタのエリアを判断しショルダ角を決定する。エリアの
定義は図2のウエスト角と同様である。In the case of FIG. 3 and the case of FIG. 4, the shoulder angle cannot be obtained by the same calculation formula depending on the position of the list center 53. In blocks 23 to 26 in the calculation flowcharts of FIGS. 9 and 10, the area at the tip of the hand and the area of the wrist center are determined to determine the shoulder angle. The definition of the area is the same as the waist angle in FIG.
【0049】ブロック23において、図2または3に示
すR1(ショルダセンタ52とリストセンタ53の同一
垂直線の変位)が0であればショルダ角を90+cos
−1(ss)/PIとし、又0でなければハンド先端5
4はエリアAに有り、そしてリストセンタ53がエリア
Aに有るかを判定する。それぞれがエリアAに有ると判
定判断されたならばショルダ角を、tan−1((Z1
−Z2)/R1)+cos−1(ss)/PIより求め
る。In block 23, if R1 (displacement of the same vertical line between the shoulder center 52 and the wrist center 53) shown in FIG. 2 or 3 is 0, the shoulder angle is set to 90 + cos.
-1 (ss) / PI, if not 0, hand tip 5
4 is in the area A, and it is determined whether the list center 53 is in the area A. If each of them is determined to be in the area A, the shoulder angle is set to tan −1 ((Z1
−Z2) / R1) + cos −1 (ss) / PI.
【0050】しかし有ると判定されなかったならば、ハ
ンド先端54がエリアAに有り、そしてリストセンタ5
3がエリアCに有るかを判定する。有ると判定されたな
らばショルダ角を、90+tan−1((Z1−Z2)
/R1)+cos−1(ss)/PIより求める。この
ようにしてハンド先端エリアAのエラー処理を行う。However, if it is not determined that there is, the hand tip 54 is in the area A, and the wrist center 5
It is determined whether 3 is in area C. If it is determined that there is, the shoulder angle is calculated as 90 + tan -1 ((Z1-Z2)
/ R1) + cos -1 (ss) / PI. Thus, the error processing of the hand tip area A is performed.
【0051】ハンド先端54がエリアAそしてリストセ
ンタ53がエリアCに有ると判定されなかったならば、
ハンド先端54がエリアBに、そしてリストセンタ53
がエリアBに有るかを判定する。判定されたならばショ
ルダ角を、tan−1((Z1−Z2)/R1)+co
s−1(ss)/PIより求める。ハンド先端54がエ
リアBに、リストセンタ43がエリアBにと判定されな
かったならば、ハンド先端54がエリアBに、そしてリ
ストセンタ53がエリアDに有るかを判定する。判定さ
れたならばショルダ角を、90+tan−1((Z1−
Z2)/R1)+cos−1(ss)/PIより求め
る。このようにしてハンド先端エリアBのエラー処理を
行う。If it is not determined that the hand tip 54 is in the area A and the wrist center 53 is in the area C,
Hand tip 54 is in area B and wrist center 53
Is located in the area B. If determined, the shoulder angle is calculated as tan −1 ((Z1−Z2) / R1) + co
It is determined from s −1 (ss) / PI. If it is not determined that the hand tip 54 is in the area B and the wrist center 43 is in the area B, it is determined whether the hand tip 54 is in the area B and the wrist center 53 is in the area D. If it is determined, the shoulder angle is set to 90 + tan -1 ((Z1-
Z2) / R1) + cos -1 (ss) / PI. Thus, the error processing of the hand tip area B is performed.
【0052】ハンド先端54がエリアBに、リストセン
タ53がエリアDにと判定されなかったならば、ハンド
先端54がエリアCに、そしてリストセンタ53がエリ
アCに有るかを判定する。判定されたならばショルダ角
を、tan−1((Z1−Z2)/R1)+cos−1
(ss)/PIより求める。If it is not determined that the hand tip 54 is in the area B and the wrist center 53 is in the area D, it is determined whether the hand tip 54 is in the area C and the wrist center 53 is in the area C. If it is determined, the shoulder angle is calculated as tan -1 ((Z1-Z2) / R1) + cos -1
(Ss) / PI.
【0053】ハンド先端54がエリアCに、リストセン
タ53がエリアCにと判定されなかったならば、ハンド
先端54がエリアCに、そしてリストセンタ53がエリ
アAにあるかを判定する。判定されたならばショルダ角
を、90+tan−1((Z1−Z2)/R1)+co
s−1(ss)/PIより求める。このようにしてハン
ド先端エリアCのエラー処理を行う。If it is not determined that the hand tip 54 is in the area C and the wrist center 53 is in the area C, it is determined whether the hand tip 54 is in the area C and the wrist center 53 is in the area A. If it is determined, the shoulder angle is calculated as 90 + tan -1 ((Z1-Z2) / R1) + co
It is determined from s −1 (ss) / PI. Thus, the error processing of the hand tip area C is performed.
【0054】ハンド先端54がエリアCに、リストセン
タ53がエリアAに有ると判定されなかったならば、ハ
ンド先端54がエリアDに、リストセンタ53がエリア
Dに有るかを判定する。判定されたならばショルダ角
を、tan−1((Z1−Z2)/R1)+cos−1
(ss)/PIより求める。If it is not determined that the hand tip 54 is in the area C and the wrist center 53 is in the area A, it is determined whether the hand tip 54 is in the area D and the wrist center 53 is in the area D. If it is determined, the shoulder angle is calculated as tan -1 ((Z1-Z2) / R1) + cos -1
(Ss) / PI.
【0055】ハンド先端54がエリアDに、リストセン
タ53がエリアDにと判定されなかったならば、ハンド
先端54がエリアDとリストセンタ53がエリアBに有
るかを判定する。判定されたならばショルダ角を、90
+tan−1((Z1−Z2)/R1)+cos
−1(ss)/PIより求める。このようにしてハンド
先端エリアDのエラー処理を行う。If it is not determined that the hand tip 54 is in the area D and the wrist center 53 is in the area D, it is determined whether the hand tip 54 is in the area D and the wrist center 53 are in the area B. If determined, the shoulder angle is 90
+ Tan -1 ((Z1-Z2) / R1) + cos
-1 (ss) / PI. Thus, the error processing of the hand tip area D is performed.
【0056】図10の計算フローチャートにおいて、ブ
ロック27はブロック23〜26で求めたショルダ角計
算値がショルダ軸可動範囲を超えた場合のショルダ角リ
ミットエラー処理であり、ショルダ角計算値が初期設定
されたショルダ角最小値或いは最大値を越えたならばシ
ョルダ角を初期設定された最小値或いは最大値に設定す
る。ブロック28はエルボ角計算値が、エルボ軸可動範
囲を超えた場合のエルボ角リミットエラー処理であり、
エルボ角を(cos−1(ee)−PI・180)/P
Iより求めた後にブロック27と同様にエルボ角リミッ
トエラー処理を行う。In the calculation flowchart of FIG. 10, block 27 is a shoulder angle limit error process when the shoulder angle calculated value obtained in blocks 23 to 26 exceeds the movable range of the shoulder axis. The calculated shoulder angle value is initialized. If the shoulder angle exceeds the minimum or maximum value, the shoulder angle is set to the initially set minimum or maximum value. Block 28 is an elbow angle limit error process when the elbow angle calculation value exceeds the elbow axis movable range.
The elbow angle is calculated as (cos -1 (ee) -PI · 180) / P
After obtaining from I, the elbow angle limit error processing is performed as in block 27.
【0057】ブロック29はピッチ角計算値が、ピッチ
軸可動範囲を超えた場合のピッチ角リミットエラー処理
であり、ピッチ角をセンサピッチ角−ショルダ角−エル
ボ角より求めた後にブロック27と同様にピッチ角リミ
ットエラー処理を行う。A block 29 is a pitch angle limit error process when the calculated pitch angle exceeds the movable range of the pitch axis. After the pitch angle is obtained from the sensor pitch angle−shoulder angle−elbow angle, as in block 27, Perform pitch angle limit error processing.
【0058】上記ブロック29までで、ウエスト角、シ
ョルダ角、エルボ角、ピッチ角、ロール角の算出は完了
するが、途中計算値エラー処理、可動範囲エラー処理を
含んでいるためロボットアーム5のハンド先端位置が、
実際の位置センサの位置と異なる場合がある。また、こ
のままでは位置センサの位置により、アームがロボット
本体に衝突したり、床に衝突する危険がある。The calculation of the waist angle, the shoulder angle, the elbow angle, the pitch angle, and the roll angle is completed up to the above block 29. The tip position is
It may be different from the actual position of the position sensor. Further, there is a risk that the arm collides with the robot body or collides with the floor depending on the position of the position sensor.
【0059】そこで、算出した関節角度から順運動の計
算を行ってハンド先端位置とリストセンタ位置を求め、
予め設定した動作範囲内でアームが動作するようにす
る。図11の計算フローチャートにおいて、ブロック3
0では順運動の計算により関節角度から、ハンド先端位
置とリストセンタ位置を求める。Then, the forward motion is calculated from the calculated joint angle to obtain the hand tip position and the wrist center position.
The arm operates within a preset operation range. In the calculation flowchart of FIG.
At 0, the forward end position and the wrist center position are obtained from the joint angles by forward motion calculation.
【0060】先ず、図12で示すショルダセンタ52か
らハンド先端54を以下の式から求める。First, the hand tip 54 is obtained from the shoulder center 52 shown in FIG.
【0061】L=Ls・cos(ショルダ角・PI)+
Le・cos((ショルダ角+エルボ角)・PI)+
(Lw+Lt)・cos((ショルダ角+エルボ角+ピ
ッチ角)・PI)L = Ls · cos (shoulder angle · PI) +
Le · cos ((shoulder angle + elbow angle) · PI) +
(Lw + Lt) · cos ((shoulder angle + elbow angle + pitch angle) · PI)
【0062】次にハンド先端54のX,Y座標値を求め
る。また、ハンド先端54のZ座標値に関しては以下の
式から求める。Next, the X and Y coordinate values of the hand tip 54 are obtained. The Z coordinate value of the hand tip 54 is obtained from the following equation.
【0063】ハンド先端Z=Ls・sin(ショルダ角
・PI)+Le・sin((ショルダ角+エルボ角)・
PI)+(Lw+Lt)・sin((ショルダ角+エル
ボ角+ピッチ角)・PI)Hand tip Z = Ls · sin (shoulder angle · PI) + Le · sin ((shoulder angle + elbow angle) ·
PI) + (Lw + Lt) · sin ((shoulder angle + elbow angle + pitch angle) · PI)
【0064】リストセンタ53はLs・sin(ショル
ダ角・PI)+Le・sin((ショルダ角+エルボ
角)・PI)より計算する。The list center 53 calculates from Ls · sin (shoulder angle · PI) + Le · sin ((shoulder angle + elbow angle) · PI).
【0065】ブロック31ではハンド先端位置が安全な
動作範囲を超えた場合、またはリストセンタ位置が床面
に衝突する場合、ロボットアーム5が動作しないように
するための処理である。Block 31 is a process for preventing the robot arm 5 from operating when the hand tip position exceeds the safe operation range or when the wrist center position collides with the floor.
【0066】即ち、ブロック30で計算されたハンド先
端54のX、Y、Z座標値及びリストセンタ53のZ座
標値がそれぞれの安全範囲の範囲外であると判定された
ならば、ウエスト角、ショルダ角、エルボ角、ロール
角、ピッチ角を前回のウエスト角、ショルダ角、エルボ
角、ロール角、ピッチ角とする。また、安全範囲の範囲
外でなければ計算を終了する。That is, if it is determined that the X, Y, Z coordinate values of the hand tip 54 and the Z coordinate value of the wrist center 53 calculated in the block 30 are out of the respective safe ranges, the waist angle, The shoulder angle, elbow angle, roll angle, and pitch angle are the previous waist angle, shoulder angle, elbow angle, roll angle, and pitch angle. If not outside the safe range, the calculation is terminated.
【0067】(3)他の実施の形態の説明、他の用途へ
の転用例の説明 実施の形態2.上記実施の形態では、位置姿勢情報から
ロボットアームを制御するための関節角度計算を演算装
置に行わせたが、位置センサを制御するコンピュータに
行わせ、コンピュータの画面上でグラフィックスにより
ロボットアームの動きをシミュレーションさせてもよ
い。(3) Description of another embodiment, description of an example of diverting to another use Embodiment 2 In the above embodiment, the calculation device calculates the joint angle for controlling the robot arm from the position and orientation information.However, the calculation is performed by the computer that controls the position sensor. The movement may be simulated.
【0068】実施の形態3.なお、上記実施の形態1は
5軸多関節ロボットアーム5の制御方法であるが、6軸
多関節ロボットアームの制御に応用することも可能であ
る。この場合、計算式は一部異なるがエラー処理の考え
方は同様である。Embodiment 3 Although the first embodiment is a method of controlling the five-axis articulated robot arm 5, it can be applied to the control of a six-axis articulated robot arm. In this case, although the calculation formulas are partially different, the concept of error processing is the same.
【0069】[0069]
【発明の効果】以上のようなこの発明によれば、位置セ
ンサの位置姿勢情報を制御装置の演算部で、さまざまな
エラー処理を施しロボットアームを制御するよう構成し
たので、例えば操縦者の手に位置センサを取りつけ、操
縦者の腕の動作と同様の動きを正確にロボットアーム5
に反映させることができるという効果がある。According to the present invention as described above, the position and orientation information of the position sensor is subjected to various error processings by the arithmetic unit of the control device so as to control the robot arm. A position sensor is attached to the robot arm 5 to accurately perform the same movement as that of the pilot's arm.
This has the effect of being able to be reflected in
【図1】 この発明による多関節ロボットアームの制御
方法の一実施の形態の全体構成図である。FIG. 1 is an overall configuration diagram of an embodiment of a control method of an articulated robot arm according to the present invention.
【図2】 この発明によるウエスト角可動範囲を示す図
である。FIG. 2 is a diagram showing a waist angle movable range according to the present invention.
【図3】 この発明によるショルダ角の計算(パターン
1)を示す図である。FIG. 3 is a diagram showing calculation of a shoulder angle (pattern 1) according to the present invention.
【図4】 この発明によるショルダ角の計算(パターン
2)を示す図である。FIG. 4 is a diagram showing calculation of a shoulder angle (pattern 2) according to the present invention.
【図5】 この発明による計算の初期設定処理を示すフ
ローチャートである。FIG. 5 is a flowchart showing a calculation initial setting process according to the present invention.
【図6】 この発明による計算処理を示すフローチャー
トである。FIG. 6 is a flowchart showing a calculation process according to the present invention.
【図7】 同じくこの発明による計算処理を示すフロー
チャートである。FIG. 7 is a flowchart showing a calculation process according to the present invention.
【図8】 同じくこの発明による計算処理を示すフロー
チャートである。FIG. 8 is a flowchart showing a calculation process according to the present invention.
【図9】 同じくこの発明による計算処理を示すフロー
チャートである。FIG. 9 is a flowchart showing a calculation process according to the present invention.
【図10】 同じくこの発明による計算処理を示すフロ
ーチャートである。FIG. 10 is a flowchart showing a calculation process according to the present invention.
【図11】 同じくこの発明による計算処理を示すフロ
ーチャートである。FIG. 11 is a flowchart showing a calculation process according to the present invention.
【図12】 従来の解析を説明するロボットアームの側
面図である。FIG. 12 is a side view of a robot arm for explaining a conventional analysis.
【図13】 従来の解析を説明するロボットアームにお
けるリスト角部の上面図である。FIG. 13 is a top view of a corner portion of a wrist in a robot arm for explaining a conventional analysis.
【図14】 従来の解析を説明するロボットアームにお
けるリスト角部の側面図である。FIG. 14 is a side view of a wrist corner of a robot arm for explaining a conventional analysis.
【図15】従来の解析を説明するロボットアームにおけ
るショルダ・エルボ角部の側面図である。FIG. 15 is a side view of a shoulder / elbow corner of a robot arm for explaining a conventional analysis.
1 位置センサ、 2 コンピュータ、 3 制御装
置、 4 演算部、5 5軸多関節ロボットアーム、
10 センサ入力リミット値処理、 11アーム制御角
リミット値処理、 12 安全範囲リミット値処理、
13 センサ入力 リミットエラー処理、 14 ロー
ル角エラー処理、 15 Y軸上処理、 16 X軸上
処理、 17 ウエスト角エリアA エラー処理、 1
8ウエスト角エリアB エラー処理、 19 ウエスト
角エリアC エラー処理、20 ウエスト角エリアD エ
ラー処理、 21 ウエスト角リミットエラー処理、
22 cos−1演算のエラー処理、 23 ハンド先
端エリアA エラー処理、 24 ハンド先端エリアB
エラー処理、 25 ハンド先端エリアCエラー処理、
26 ハンド先端エリアD エラー処理、 27 シ
ョルダ角リミットエラー処理、 28 エルボ角リミッ
トエラー処理、 29 ピッチ角リミットエラー処理、
30 ハンド先端位置計算処理、 31 動作範囲の
エラー処理、 46 θw :ウエスト角、 47 θs
:ショルダ角、 48 θe:エルボ角、 49 θp
:ピッチ角、 50 θr :ロール角、52 ショル
ダセンタ、 53 リストセンタ、 54 ハンド先
端、55 水平面、60L1。1 position sensor, 2 computer, 3 control device, 4 arithmetic unit, 5 5-axis articulated robot arm,
10 Sensor input limit value processing, 11 Arm control angle limit value processing, 12 Safe range limit value processing,
13 Sensor input limit error processing, 14 Roll angle error processing, 15 Y axis processing, 16 X axis processing, 17 Waist angle area A error processing, 1
8 Waist angle area B error processing, 19 Waist angle area C error processing, 20 Waist angle area D error processing, 21 Waist angle limit error processing,
22 cos -1 operation error processing, 23 hand tip area A error processing, 24 hand tip area B
Error processing, 25 Hand tip area C error processing,
26 Hand tip area D error processing, 27 Shoulder angle limit error processing, 28 Elbow angle limit error processing, 29 Pitch angle limit error processing,
30 Hand tip position calculation processing, 31 Operation range error processing, 46 θw: Waist angle, 47 θs
: Shoulder angle, 48 θe: Elbow angle, 49 θp
: Pitch angle, 50 θr: Roll angle, 52 Shoulder center, 53 Wrist center, 54 Hand tip, 55 Horizontal plane, 60L1.
───────────────────────────────────────────────────── フロントページの続き Fターム(参考) 3F059 BA04 BC01 CA08 EA01 FC13 3F061 DD00 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 3F059 BA04 BC01 CA08 EA01 FC13 3F061 DD00
Claims (5)
置センサにより検出する検出工程、この検出された位置
姿勢情報に基づいて制御対象である多関節ロボットアー
ムの関節角度を演算手段にて演算する演算工程、上記位
置姿勢情報に沿って前記多関節ロボットを動作させる場
合の動作範囲限定値を前記関節角度に基づいて設定する
設定工程とを有することを特徴とする多関節ロボットア
ームの制御方法。1. A detecting step of detecting position and orientation information of a movable body in a space by a position sensor, and calculating a joint angle of a multi-joint robot arm to be controlled based on the detected position and orientation information by a calculating means. A method for controlling an articulated robot arm, comprising: a calculating step; and a setting step of setting an operation range limit value for operating the articulated robot in accordance with the position and orientation information based on the joint angle.
のロール角よりロール角を計算するロール角演算工程を
有することを特徴とする請求項第1項に記載の多関節ロ
ボットアームの制御方法。2. The control of an articulated robot arm according to claim 1, further comprising a roll angle calculating step of calculating a roll angle from a predetermined angle threshold, a current roll angle, and a previous roll angle. Method.
置、前回ウエスト角よりロボットアームの方向を決定
し、ウエスト角を計算するウエスト角演算工程を有した
ことを特徴とする請求項第1項に記載の多関節ロボット
アームの制御方法。3. A waist angle calculating step of determining a direction of the robot arm from a predetermined angle threshold value, a current hand tip position, and a previous waist angle, and calculating a waist angle. 3. The method for controlling an articulated robot arm according to item 1.
センタ位置によりショルダ角を計算するショルダ角演算
工程を有したことを特徴とする請求項第1項に記載の多
関節ロボットアームの制御方法。4. The control method for an articulated robot arm according to claim 1, further comprising a shoulder angle calculation step of calculating a wrist center position and calculating a shoulder angle from the wrist center position.
置、リスト先端位置を計算し、ロボットアームが設定し
た安全動作範囲を超えないように動作範囲を限定する設
定工程を有することを特徴とする請求項第1項に記載の
多関節ロボットアームの制御方法。5. A setting step of calculating a hand tip position and a wrist tip position from the calculated joint angles and limiting an operation range so as not to exceed a safe operation range set by the robot arm. Item 2. The method for controlling an articulated robot arm according to Item 1.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP10251361A JP2000079582A (en) | 1998-09-04 | 1998-09-04 | Controlling method for articulated robot arm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP10251361A JP2000079582A (en) | 1998-09-04 | 1998-09-04 | Controlling method for articulated robot arm |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2000079582A true JP2000079582A (en) | 2000-03-21 |
Family
ID=17221693
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP10251361A Pending JP2000079582A (en) | 1998-09-04 | 1998-09-04 | Controlling method for articulated robot arm |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2000079582A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2836528A1 (en) | 2002-02-26 | 2003-08-29 | Denso Corp | Electromagnetic clutch for vehicles, has rubber dampers placed at intermediate points of securing portions and placed in gap between leaf spring and hub along axial direction |
CN103279029A (en) * | 2013-05-31 | 2013-09-04 | 北京航空航天大学 | Self-adaptive coordination control method of multi-robot-arm system |
-
1998
- 1998-09-04 JP JP10251361A patent/JP2000079582A/en active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2836528A1 (en) | 2002-02-26 | 2003-08-29 | Denso Corp | Electromagnetic clutch for vehicles, has rubber dampers placed at intermediate points of securing portions and placed in gap between leaf spring and hub along axial direction |
CN103279029A (en) * | 2013-05-31 | 2013-09-04 | 北京航空航天大学 | Self-adaptive coordination control method of multi-robot-arm system |
CN103279029B (en) * | 2013-05-31 | 2015-08-19 | 北京航空航天大学 | The self-adaptive wavelet base method of multirobot hand |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10870199B2 (en) | Robot system and robot teaching method | |
US12011825B2 (en) | Device, method and program for estimating weight and position of gravity center of load by using robot | |
CN108177144B (en) | Robot system | |
KR20190079322A (en) | Robot control system | |
EP1795315A1 (en) | Hand-held control device for an industrial robot | |
JP5228783B2 (en) | Robot origin return device | |
CN110774280A (en) | Robot system | |
JP4211701B2 (en) | Robot hand gripping control device | |
JP2021030364A (en) | Robot control device | |
JP2009226552A (en) | Method for teaching redundant robot | |
WO2020161910A1 (en) | Control device, control method, and recording medium | |
CN112118940B (en) | Direct teaching device and direct teaching method for manipulator | |
JP2000079582A (en) | Controlling method for articulated robot arm | |
JPH06155346A (en) | Teaching device for robot | |
JP2000039911A (en) | Robot controller | |
KR102517598B1 (en) | Robot direct teaching device and direct teaching method | |
JP2000094370A (en) | Inclination measuring method of work surface of robot and measuring device thereof | |
JP2737325B2 (en) | Robot trajectory generation method | |
JPH1058361A (en) | Control device of manipulator | |
JPH05169381A (en) | Force control type robot unit and control method thereof | |
JPH06138930A (en) | Copying control method/device for machining/assembling device | |
JPS62195513A (en) | Shape detecting method for material body | |
JPH0631982U (en) | Robot controller | |
JP3196972B2 (en) | Robot direct teaching device | |
JPH04135189A (en) | Robot-controlling system |