WO1990002028A1 - Vertical articulated robot - Google Patents

Vertical articulated robot Download PDF

Info

Publication number
WO1990002028A1
WO1990002028A1 PCT/JP1989/000897 JP8900897W WO9002028A1 WO 1990002028 A1 WO1990002028 A1 WO 1990002028A1 JP 8900897 W JP8900897 W JP 8900897W WO 9002028 A1 WO9002028 A1 WO 9002028A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
link
robot
end effector
angle
Prior art date
Application number
PCT/JP1989/000897
Other languages
English (en)
French (fr)
Inventor
Ryuichi Hara
Original Assignee
Fanuc Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Fanuc Ltd filed Critical Fanuc Ltd
Publication of WO1990002028A1 publication Critical patent/WO1990002028A1/ja

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/408—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by data handling or data format, e.g. reading, buffering or conversion of data
    • G05B19/4086—Coordinate conversions; Other special calculations

Definitions

  • the present invention relates to a vertical articulated robot having an offset wrist, and more particularly to a mouth robot of this type which can quickly calculate a joint angle and has excellent work accuracy.
  • a 6-axis vertical articulated mouthboat typically has a robot body with an arm consisting of three links and a wrist consisting of three links, which is fixed to the robot working space
  • the base and the link on the base side of the arm, and the adjacent links are connected via rotating joints, so that the end-effector mounted on the wrist can take any position and posture. are doing.
  • the robot uses an end effector given by using a work coordinate system fixedly set in the robot working space.
  • a computer for calculating a target value of each joint angle of the first to sixth joints capable of realizing the target position and posture based on the target position ⁇ and posture, and controlling each joint angle to the target value (Where the numbers of the links and joints are given outward from the base side).
  • the first fulgurus angle is given as a censor only for the position and posture of the endef ekter. It depends on the fourth joint angle. Therefore, the first joint angle is calculated by giving an initial value of ⁇ for the fourth joint angle, and then the second to fourth joint angles and the first joint angle are calculated as the calculated values of the first to fourth joint angles. The procedure for calculating the fourth joint angle is repeated until the calculated value of the fourth joint angle converges.
  • the robot periodically calculates the position and attitude of the end effector so that the end effector moves along a predetermined route while controlling the attitude of the end effector to the target attitude.
  • it is necessary to shorten the calculation cycle (interpolation cycle) of the position and orientation of the end effector.
  • a robot equipped with an offset wrist requires a large amount of exercise to calculate the nodal angle as described above, and therefore requires a long calculation time, so that the interpolation cycle becomes longer and the work accuracy becomes higher. Disclosure of invention that is difficult to improve
  • An object of the present invention is to provide a target position and an attitude of an end effector.
  • An object of the present invention is to provide a vertical multi-joint robot having an off-seven wrist, which can quickly calculate each joint angle based on the wobble, and is excellent in work accuracy.
  • a first link connected to a base fixed to a work space via a first rotary joint and a second link to the first link are provided.
  • the control unit includes: The joint angle of the first rotary joint, which is given as a function of only the target position and posture of the end effector, is calculated, and the joint angle of each of the rotary joints other than the first rotary joint is first calculated. Calculate based on the extracted nodal angle.
  • the present invention arranges the first and second pivot joints of a vertical articulated robot having an offset wrist such that the joint axes of both extend in the axial direction of the base and the first link, respectively. And calculate the nodal angle of the first resection node given as a function of only the end effector ⁇ target position and posture. At the same time, the other nodal angles are calculated based on the joint angles calculated earlier, so that it is not necessary to repeat the calculation of the nodal angles, which has been necessary in this type of robot in the past. Becomes As a result, it is possible to reduce the amount of exercise for calculating the joint angle, and hence the required exercise time, and thus to shorten the calculation cycle (interpolation cycle) of the position and posture of the end effector. The work accuracy of the robot can be improved.
  • FIG. 1 is a conceptual diagram showing a 6-axis vertical multi-articulated robot having an offset wrist according to one embodiment of the present invention.
  • FIG. 2 is a diagram similar to FIG. 1 showing a conventional robot.
  • Fig. 3 is a diagram showing the relationship established between the co-ordinate transformation matrices
  • Fig. 4 is a flowchart showing the joint angle calculation process in the robot in Fig. 1
  • Fig. 5 is a flowchart in Fig. 1.
  • FIG. 3 is a schematic diagram showing the general configuration of a robot.
  • each joint angle is controlled to the target ⁇ nodal angle calculated based on the target value of the position and orientation of the end effector. For this reason, the target joint angle is calculated using a coordinate transformation matrix that associates the work coordinate system, which is set in the robot work space and indicates the position and orientation of the end F ekta, with the coordinate system set for each link (joint). are doing.
  • the mouth bot includes first to third rotating joints (hereinafter referred to as “joints”) 10 to 12 provided on the arm and fourth to sixth joints 13 provided on the wrist.
  • the base fixed to the robot work space and the base side link (first link) are connected by the first joint 10 and are adjacent to the first to sixth links.
  • the sixth link is provided with an end effector.
  • the 1st to 6th links (the 1st to 6th sections and the center of the mounting surface of the end effector X) have the 0th coordinate system (00, X0, Y0, Z 0) and the first to sixth coordinate systems (01, XI, Yl, Z1 :) to (06, X6, ⁇ 6, ⁇ 6) are set, respectively.
  • the joint axis of joint 10 ( ⁇ 0 axis) is the robot workspace
  • the joint axis ( ⁇ 1 axis) of the second joint 11 extends in the direction orthogonal to the axis of the first link, and ⁇ ⁇ 2, ⁇ 3 to ⁇ 5 correspond to the third axis.
  • ⁇ 6th joint Represents the joint axes of 12 to 15.
  • a 1 s 1 c 1 0 0
  • c 1 and s 1 represent cos 0 1 and sin S 2 respectively (C i and 31 similarly represent 003 1 and 3 ⁇ !, respectively).
  • the first belug angle 1 takes a positive value when the first knot 10 is tilled in the direction shown by the arrow in FIG. Takes a positive value when the i-th section rotates in the direction of the arrow).
  • d 2 represents the axial length of the second link, that is, the distance between the origins O 1 and 1 2 of the first and second coordinate systems.
  • di is the length of the i-th link (i-1
  • the distance d represents the offset distance of the wrist with respect to the arm.
  • the component that fluctuates with the angle ⁇ ⁇ 2 indicates that the point that the third coordinate system is rotated by an angle ⁇ / 2 with respect to the ⁇ 2 axis of the second coordinate system is considered ( The same applies hereinafter).
  • End F If the position and orientation of the center of the mounting surface are given, the first to sixth joint angles 0 1 to 6 are calculated from the i-th coordinate system.
  • the transformation matrix 1 T to the i-th coordinate system is shown below. V 68CKJD1 / 8SS
  • ⁇ 1 atan2 (py, ⁇ )
  • Atan2 (y, x) is an extension function of x N Tarr y, variables chi, code Takes a value in the range of ⁇ to ⁇ depending on.
  • a vector ⁇ ( ⁇ •> P y, P 2 and the upper gS vector sole i, a and the length d of the sixth link d) representing the position 1: of the origin of the sixth coordinate system in the zeroth coordinate system
  • p 1 — d 6
  • the robot is controlled by a computer 110 connected to a teaching operation panel 111 and a manual data input device 112 via an input circuit (not shown). It has a mouth body 100 that is driven and controlled by a servo system 120 that operates with the.
  • the robot body 100 includes a base 101 fixedly arranged in the working space, an arm having first to third links 102 to 104, and a fourth to sixth link f.
  • the base 101 and the link 102- With an offset wrist having 105-107, the base 101 and the link 102-:
  • the adjacent ones of 107 are connected to the revolving joints (hereinafter referred to as joints) 1-6. They are connected to each other through the corresponding ones, and the sixth link 107 is equipped with an end effector 108.
  • the # 2 robot main body 100 of this embodiment has a joint axis (Y0 axis) of the first joint 1 orthogonal to the axial direction of the base 101. Is different from the point that extends in the direction (horizontal direction) and the point that the knot joint axis (Z1 axis) of the second joint 2 extends in the axial direction (vertical direction) of the first link 102. I do. Due to this difference, among the transformation matrices A i, 1 ! ⁇ In the robot of this embodiment, the following transformation matrices A 1, A 2, 1 T6 and 8 T s are different from the conventional ones.
  • the operator Prior to the robot operation, the operator operates the manual data input device 112 to input various parameters used in the joint angle detection processing described later, including the link length, and requires the end effector 108.
  • Conventionally known teaching is performed via the teaching operation panel 111 in a state where the position is sequentially determined to the position g and a required posture is taken at each position.
  • a memory (not shown) built in the computer 110 stores a normal vector n, an orientation vector 0, and an approximate vector representing the target position and posture of the end effector 108 at each teaching position.
  • the teaching data consisting of the vector a and the mouth case vector 1 is stored.
  • a teaching program including teaching data is created as needed.
  • a central processing unit built into the computer 110 (hereinafter referred to as a CPU) is connected to a start point of a first block of a control program from a memory or a teaching program.
  • the various vectors n, 0, a. 1 are read out (step S i), and the sixth coordinate system in the zeroth coordinate system according to the equation (7) already described with reference to FIG. Calculate the pX, py, and pz components of the position vector p, which represents the position of the origin of (Schip S2).
  • step S3 the CPU calculates a target value of the first joint angle 1 in accordance with the following equation (8).
  • the expression (8) is the first row and fourth column and the second row and fourth column of both sides of the expression (6) (see the expression (1) for the right side). And the sum of the squares of the elements in the third row and the fourth column, each of which is equal, is obtained by rearranging the following equation, and the position vector components pX, py, pz and the robot obtained in step S2 are obtained. Only the link lengths d2, d3, and offset distance d4, which are determined according to the configuration of the joint, are used as variables, and therefore, the first joint angle 01 can be easily calculated according to the equation (8).
  • ⁇ 1 atan ⁇ px 2 + py ⁇ + pz 2 + d2 3 -d3 2 -d4 £ , ⁇ (3d2 + d3 2 + d4 2 -x 2 -y 2 -pz 2 ) ! ' 2 ⁇ -a tan ( pz, x)
  • step S5 When the computer 110 sends a control output representing the target values of the joint angles 01 to 06 calculated as described above to the servo system 120 via an output circuit (not shown) (step S5), the servo system The drive control of each axis servomotor of the robot main body 100 is performed so that the actual joint angle becomes a target value.
  • the CPU repeatedly executes the above-described joint angle calculation processing at a predetermined cycle. That is, instead of executing the step S1, the CPU executes the end-effector corresponding to the interpolation point on the movement path of the end-effector on the basis of the teaching data indicating the start point and the end point regarding the block read in the step S1. Then, the target position and posture are calculated by a conventionally known interpolation process, and then steps S2 to S5 described above are executed based on the calculated target position and posture.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Description

明 細 害
垂直多関節口ボッ ト
技 術 分 野
本発明は、 オフセッ ト手首を有する垂直多関節ロボッ ト に関し、 特に、 関節角を迅速に算出可能で作業精度に 優れたこの種の口ボッ ト に関する。
背 景 技 術
6軸垂直多関節口ボ、ソ ト は、 典型的には、 3つのリン クよ り なるアームと 3つのリンク よ りなる手首とを有す るロボッ ト本体を備え、 ロボッ ト作業空間に固定したベ ースとアームのベース側リンクとを、 および、 相隣るリ ンク同士を回転閬節を介して夫々連結し、 手首に装着し たエン ドェフエク タが任意の位置， 姿勢をとれるように している。 そして、 エン ドェフエクタの位箧， 姿勢を制 御して各種作業を行うために、 ロボッ ト は、 ロボッ ト作 業空間内に固定的に設定した作業座標系を用いて与えら れたエン ドエフヱクタの目標位箧， 姿勢に基づいて当該 目標位置， 姿勢を実現可能な第 1 〜第 6の関節の夫々の 関節角の目標値を演算するためのコンピュータと、 夫々 の関節角を目標値に制御するためのサーボ系とを備えて いる （ここで、 リ ンクおよび関節の番号はベース側のも のから外側に向かって付してある） 。
エン ドェフエク タの位箧， 姿勢から夫々の関節角を箅 出するにあたり、 手首の 3軸が一点で交わる云わゆるィ ンライン手首においては、 エン ドェフエク タの位 g, 姿 勢のみの閲数と して表される第 1 の関節の閔節角 （以下、 第 i 番目 （ i = 1 〜 6 ) の関節の W節角を第 i 閬節角と 云う） を先ず算出し、 次いで、 第 1 関節角等の先に算出 された閬節角の関数として表される第 i 関節角を順次箅 出している。
これに対して、 手首の 3軸が一点で交わらずアームに 対してオフセッ ト したオフセッ ト手首にあっては、 第 1 閭節角がエン ドェフエク タの位置， 姿勢のみの閲数とし ては与えられず、 第 4関節角にも依存する。 そこで、 第 4関節角について扳の初期値を与えて第 1 関節角を箅出 し、 次いで、 第 2〜第 4関節角および第 1 閭節角を第 1 〜第 4鬨節角の算出値に基づいて箅出する手順を第 4関 節角の算出値が収束するまで繰り返すようにしている。
ロボッ トでは、 エンドェフエクタの姿勢を目標の姿勢 に制御しつつェン ドエフエクタを所定の経路に沿って移 動させるベく、 エンドエフェクタの位置， 姿勢を周期的 に算出している。 そして、 作業精度向上のためにはェン ドエフ Xクタの位置， 姿勢の算出周期 （補間周期） を短 くする必要がある。 しかしながら、 オフセッ ト手首を備 えたロボッ トでは、 上述のように、 閬節角の算出のため に膨大な演箅を要し、 従って所要演算時間が長いので、 補間周期が長く なり、 作業精度を向上することが困難で 発 明 の 開 示
本発明の目的は、 エン ドエフヱクタの目標位箧， 姿勢 に基づいて夫々の関節角を迅速に算出でき、 従って、 作 業精度に優れたオフ七ッ ト手首を備えた垂直多関節ロボ ッ ト を提供することにある。
上記目的を達成するため、 本発明によれば、 作業空間 に固定して配されたベースに第 1 の回転関節を介して連 結された第 1 リ ンクと前記第 1 リ ンクに第 2の回転閔節 を介して連結された.第 2 リンク と前記第 2 リンクに第 3 の回転閬節を介して連結された第 3 リンク とを有するァ ームと、 前記アームに装着されかつ互いの関節軸が一点 で交わらない複数の回転関節を有すると共にエン ドエフ ェクタが装着されるオフセッ ト手首と、 前記エン ドエフ ェクタの目標位置， 姿勢に基づいて前記夫々の回転関節 の関節角を演算するための制御部とを有する垂直多関節 ロボッ ト が提供され、 前記第 1， 第 2の回転閬節は前記 ベースの軸線に直交する方向および前記第 1 リンクの軸 線方向に夫々延びる閬節軸を有し、 前記制御部は、 前記 エン ドェフエク タの目標位置， 姿勢のみの関数として与 えられる前記第 1 の回転関節の関節角を算出すると共に、 前記第 1 の回転閬節以外の前記回転関節の各々の関節角 を、 先に箅出した閬節角に基づいて算出する。
上述のように、 本発明は、 オフセッ ト手首を有する垂 直多関節ロボッ ト の第 1， 第 2の回輊関節を両者の関節 軸がベースおよび第 1 リンクの軸線方向に夫々延びるよ うに配し、 エン ドエフヱクタ φ目標位置， 姿勢のみの関 数として与えられる第 1 の回耘閬節の閬節角を算出する と共にそれ以外の閡節角を先に算出した関節角に基づい て算出するようにしたので、 従来、 この種のロボッ ト に おいて必須であつた閬節角の算出上の繰り返し演算が不 要となる。 結果と して、 関節角算出のための演箅量ひい ては所要演箅時間を低減でき、 従って、 エン ドエフェク タの位置， 姿勢の算出周期 （補間阇期） を短縮でき、 こ れによ り ロボッ ト の作業精度を向上できる。
図 面 の 簡 単 な 説 明
第 1 図は本発明の一実施例によるオフセッ ト手首を有 する 6軸垂直多閱節ロボッ トを示す概念図、 第 2図は従 来のロボッ トを示す第 1 図と同様の図、 第 3図は座檩変 換行列同士の間に成立する関係を示す図、 第 4図は第 1 図のロボッ トにおける関節角算出処理を示すフローチヤ ー ト、 および、 第 5図は第 1 図のロボッ トの全体構成を 示す概喀図である。
発明を実施するための最良の形態
本発明の説明に先立ち、 オフセッ ト手首を有する従来 の 6軸垂直多閬節ロボッ トを説明する。
エン ドェフエク タの位置， 姿勢制御にあたっては、 夫 々の関節角を、 エンドェフエクタの位置， 姿勢の目標値 に基づいて算出した目標閬節角に制御する。 このため、 ロボッ ト作業空間に設定されェン ドエフエクタの位置， 姿勢を表す作業座標系を夫々のリンク （関節） に設定し た座標系に関係づける座標変換行列を利用して目標関節 角を算出している。 第 2図を参照すると、 口ボッ ト は、 アームに設けた第 1 〜第 3 の回転関節 （以下、 関節という〉 1 0〜 1 2と 手首に設けた第 4〜第 6の閡節 1 3〜 1 5を有し、 ロボ ッ ト作業空間に固定したベースとベース側リンク （第 1 リンク） とが第 1 の関節 1 0によ り連結され、 第 1 〜第 6のリ ンクの相隣るもの同士が第 2〜第 6の閬節 1 1〜 1 5によ り連結されている。 また、 第 6のリンクにはェ ン ドエフ エクタが装着されている。 そして、 ベースおよ び第 1 〜第 6のリ ンク （第 1〜第 6の閬節およびエン ド エフ Xクタ取付面中心） には作業座標系と しての第 0座 標系 （ 0 0， X 0， Y 0, Z 0 ) および第 1 〜第 6の座 標系 （ 0 1， X I， Y l， Z 1 ：) 〜 （ 0 6， X 6, Υ 6, Ζ 6 ) が夫々設定ざれている。 第 1 の関節 1 0の関節軸 ( Ζ 0軸） はロボッ ト作業空間に固定したベースの軸線 方向に延び、 第 2の関節 1 1 の関節軸 （ Υ 1軸） は第 1 リンクの軸線に直交する方向に延びている。 Υ 2， Ζ 3 〜 Ζ 5は、 第 3〜第 6の関節 1 2〜 1 5の関節軸を表す。
第 1 〜第 6の関節 1 0〜 1 5の関節角 （以下、 第 1〜 第 6の閬節角と云う） 0 1 〜 6が与えられている場合 に、 これら関節角に基づいて作業座標系におけるェンド エフ：！：クタ取付面中心の位置， 姿勢を算出するための、 第 i 一 1座標系から第 i 座標系への 4 X 4同次座標変換 行列 A i を以下に示す。 c 1 一 s 1 0 0、
A 1 = s 1 c 1 0 0
0 0 1 0
0 0 0 1 ノ
こ こで、 c 1 , s 1 は cos 0 1， s i n S 2を夫々表す （以 下同様に C i ， 3 1 は003 1， 3^ ！を夫々表す） 。 ま た、 第 1 の閭節角 1 は、 第 1 の閬節 1 0が第 2図にお いて矢印で示す方向に回耘したとき正の値をとる （以下 同様に第 閲節角 S i は第 i 閬節が矢印方向に回転した とき正の値をとる） 。
Figure imgf000008_0001
cos (- Θ 2) 0 si n (- ^ 2) 0 、 0 1 0 0
■sin(- Θ 2) 0 cos (- Θ 2) 0
0 0 0 1 ノ 1 0 0 s2d2
0 1 0 0
0 0 1 c2d2
0 0 0 1
こ こで、 d 2は第 2リンクの軸線方向の長さを、 すな わち第 1， 第 2座標系の原点 O 1， 〇 2間距離を表す。 以下同様に d i は第 i リンクの長さ （第 i 一 1， 第 i座 標系の原点〇 i 一 1 , 〇 i 間距離） を表し、 d 4はァー ムに対する手首のオフセッ ト距離を表す。 なお、 第 0， 第 1座標系の原点 O 0, 0 1 同士および第 4， 第 5座標 系の原点 04， 05同士は互いに同一位置に設定されて いる （ d 1 = d 5 = 0 ( d 1 , d 5は図示せず） ） 。
Figure imgf000009_0001
こ こで、 角度 π Ζ 2に鬨瑋する成分は、 第 3の座標系 が第 2の座標系の Υ 2軸に関して角度 π / 2だけ回転し ている点を考慮したことを表している （以下同様） 。
Figure imgf000009_0002
ノ
Figure imgf000010_0001
次に、 エンドエフ：！：ク タ取付面中心の位萤， 姿勢が与 えられている場合に、 これに基づいて第 1 〜第 6の閬節 角 0 1 〜 6を算出するための、 第 i 一 1 座標系から第 i座標系への変換行列 1 T；を以下に示す。 V 68CKJD1/ 8SS
as
ん"
-a o
V 9 CD CD
s 1 V3 M ω CD
Figure imgf000011_0001
CD CD Γ t V? Π
o o CD CD O C CD CD O
a u II IO IO
ω C a
+ + CD o CD w
CD CD C 1 CO + ω IO o
IO 00 CO CD o O 1 00
CO CD
o o a o
1 + +
/ < CD O
CO a
IO IO
a
C CO
CO CO
CO o
IO o o
O o
Figure imgf000012_0001
x = Cl(s2d2+c3d3+s3c4d4) + sls4d4
= Si(s2d2+c3d3+s3c4d4) - cls4d4
P2 = c2d2 - s3d3 + c3c4d4
従来、 以上のよ うにして求めた変換行列 1 T iを利用 して閡節角 3 1 〜 6を求めている。 先ず、 上述の変換 行列 A 1 の逆行列 [A 1 ] と上記変換行列0 T6，
1 Τ 6との間には次式.が成立する。
[ A 1 ] - 1 3 T 6 = 1 T 6 ♦ ♦ ♦ ( 3 ) 左辺 =
10 clnx+slny clox+sloy clax+slay clpx+slpy 、
-slnx+cln -slox+clo -slax+clay slpx+clpy Π2 * 02 az pz
0 0 0 1 / 上記第 （ 3 ) 式 の両辺の 2行 4歹 tj目の行列要素を互
15 いに等しいとおく と （右辺について^第 （ 2 ) 式を参照〉 下式が成立する。 j
- s l p x + c l p y = - s 4 d 4 ♦ ♦ ♦ ( 4 )
θ 1 = atan2(py , ρχ)
- tan2{-s4d4.. ± (px2+p 2 -s42d42)5 -' 2 } ここで、 atan2(y，x)は tarr!yん xの拡張関数で、 変数 χ， の符号に応じて一 τιから πの範囲の値をとる。
そして、 第 0座標系における第 6の座標系の原点の位 1:を表すべク トル ρ (ρ• > P y， P 2ノと上 gSヘク トソレ i , a 及び第 6 リンクの長さ d 6との間に成立する関係 p = 1 — d 6 a p x = 1 x - axd6, py = ty - ayd6 , pz= lz - azd6) を用いて上記式で表される関節角 S 1 を求 める。
さらに、 上述の変換行列 A i の逆行列 [ A i ] —1と上 記変換行列い1 T iとの間には次式が成立する。
[A2] [A1] - 2 T 6 = 2 T 6
[A3] -1 [Α2] -1 [A1] "10 Τ β = 3 Τ 6 一
CA4] [A3] -1 [Α2] [A1] "10 T e = T e
[A53 [A43 -1 [A3] -1 CA2] [A1] " 10 Τ β =
5 Τ 6 - そこで、 上式の両迈の適宜の行， 列の行列要素間に成 立する関係および仮に求めた第 1 の閬節角 1 に基づい て第 2〜第 4の鬨節角 S 2〜 S 4の仮の値を算出する。 そして、 鬨節角 Θ 4の算出値に基づいて上記拡張閬数で 与えられる閬節角 0 1 を再び求める。 この手順を関節角 0 4が収束するまで繰り返し、 関節角 5 1 〜 5 4を決定 する。 次いで、 第 5， 第 6の関節角 0 5， 0 6を求める。
このように、 従来の垂直多閬節ロボッ ト においては閬 節角の箅出に膨大な演算を要し、 不都合であった。 なお、 インライン手首を有する垂直多関節ロボッ ト にあっては 上記第 （ 4〉 式に対応する式一 s 1 p X + c 1 p y = 0 が成立し、 第 1 の関節角 S 1 はエン ドェフエク タの位置, 姿勢のみの閲数で表され、 閲節角算出上の不都合が生じ ない。
以下、 本発明の一実施例によるオフセッ ト手首を有す る 6軸垂直多関節ロボッ ト を説明する。
第 1 図および第 5図を参照すると、 ロボッ ト は、 図示 しない入力回路を介して教示操作盤 1 1 1 および手動デ ータ入力装置 1 1 2を接銃したコンピュータ 1 1 0の制 御下で作動するサーボ系 1 20によ り駆動制御される口 ボッ ト本体 1 0 0を備えている。 ロボッ ト本体 1 0 0は、 作業空 内に固定して配されたベース 1 0 1 と、 第 1〜 第 3リ ク 1 0 2〜： 1 04を有するアームと、 第 4〜第 6リン f 1 05〜 1 07を有するオフセッ ト手首とを備 え、 ベ ÷■ス 1 0 1 およびリンク 1 02〜 ： 1 07の相隣る もの同士は回転関節 （以下、 関節と云う） 1 〜 6の対応 するものを介して互いに連結され、 第 6リンク 1 07に はエン ドェフエク タ 1 08が装着されている。 そして、 夫々の閬節には関節を回転駆動するためのサーボモータ (図示略〉 が連結され、 夫々のサーボモータにはモータ 回転位置を検出するための検出器 （図示略） が装着され、 これらサーボモータおよび検出器はサーボ系 1 20に接 続されている。 第 2図に示す従来のロボッ トに比べて、 本実施例の Π2 ボッ ト本体 1 0 0は、 第 1 の関節 1 の関節軸 （ Y 0軸） がベース 1 0 1 の軸線方向に直交する方向 （水平方向） に延びている点と、 第 2の閬節 2の鬨節軸 （ Z 1軸） が 第 1 リ ンク 1 0 2の軸線方向 （垂直方向） に延びている 点とが相違する。 この相違に起因して、 本実施例のロボ ッ ト における変換行列 A i， 1 !^のうち、 以下に示す 変換行列 A 1， A 2 , 1 T 6および8 T sが従来のものと
¾なる,
Figure imgf000016_0001
T 6 = A 2 2 T 6 =
8
卜
0\
广
II
X
Figure imgf000017_0001
o io o o
t t
01 o o 1
Figure imgf000018_0001
Figure imgf000019_0001
c2(cl nx-slnz)+s2ny c2(clox-sloz)+s2oy -s2(c 1 nx-sl nz)+c2ny -s2(clox-slo2)+c2o s 1 nx+c 1 nz s 1 o x + c 1 o 2
0 0
c2(clax-s
-s2(clax-s
slax
Figure imgf000019_0002
以下、 第 4図を参照して、 上述の構成の 6軸垂直多閬 節ロボッ トの作動を説明する。
ロボッ ト作業前に、 オペレータは、 手動データ入力装 1 1 2を操作してリンク長さ等を含む後述の関節角箅 出処理において用いられる各種パラメータを入力すると 共に、 エン ドエフ ヱクタ 1 08を所要位 gに順次位箧決 めしかつ夫々の位置において所要の姿勢をとらせた状態 で教示操作盤 1 1 1 を介して従来公知の教示を行う。 こ の結果、 コンピュータ 1 1 0に内蔵のメモリ （図示略） に、 夫々の教示位置におけるエン ドェフエクタ 1 0 8の 目標位 ， 姿勢を表す、 ノーマルベク トル n， オリエン チーシヨンべク トル 0， アブローチべク トル aおよび口 ケーシヨンベク トル 1 よ りなる教示デ一タが格納され、 さらに、 必要に応じて教示データを含む教示プログラム が作成される。
ロボッ トの運転開始時、 コンピュータ 1 1 0に内蔵の 図示-しない中央処理装 § (以下、 C P Uという） は、 メ モリ又は教示ブ Πグラムから制御プログラムの最初のブ ロックの始点に閭連する各種ベク トル n, 0 , a. 1 を 読出し （ステップ S i ) 、 第 2図を参照して既に説明し かつ以下に再掲する第 （ 7) 式に従って第 0座標系にお ける第 6座標系の原点の位置を表す位置べク トル pの p X, p y, p z成分を算出する （スチップ S 2) 。
p x = l x— a x d 6
P y = 1 y - a y d 6 · ♦ ♦ ( 7 )
P z = 1 z— a z d 6
次に、 C P Uは下記第 （ 8〉 式に従って第 1の関節角 1の目標値を算出する （ステップ S 3 ) 。
slpx+clpz = {px2 + py2+p22+d22-(d32+d42)} /2d2
♦ ♦ ♦ ( 8 ) ここで、 第 （ 8 ) 式は、 第 （ 6 ) 式の両辺 （右辺につ いては第 （ 1 ) 式を参照） の 1行 4列目， 2行 4列目お よび 3行 4列目の要素を夫々二乗したものの和を等しい とおいて得た次式を整理して求めたもので、 ステップ S 2で箅出した位置ベク トル成分 p X， p y, p zおよび ロボッ トの構成に応じて定まるリンク長 d 2， d 3, ォ フセッ ト距離 d 4のみを変数とし、 従って、 第 1の関節 角 0 1 は第 （ 8 ) 式に従って容易に箅出ざれる。 θ 1 = atan{px2+py^ + pz2+d23 -d32 -d4£ , ± (3d2 +d32 + d42 - x2 - y2 - pz2 ) ! ' 2 } -a tan(pz , x)
そして、 第 2図を参照して既に説明しかつ下記に再掲 する逆行列 [A i ] ''と変換行列 '— 1 T iとの間に成立す る閲係及びステップ S 3で求めた第】 の閔節角 ^ 1 の目 標値に基づいて第 2〜第 6の閔節角 θ 2 6の目標値 を算出する （スチッブ S 4 。
[A3] [Α2] -,1 [A1] - 1 Β Τ 6 = 3 Τ 6
[Α4] -1 [A3] [Α2] -1 [A1] "12 Τ 6 = 4 Τ 6
[A5] -1 [A4] -1 [A3] -1 [A2] -1 [A1] -10丁 s =
5 T 6
上述のようにして算出した関節角 0 1 〜 0 6の目標値 を表す制御出力をコンピュータ 1 1 0が図示しない出力 回路を介してサーボ系 1 2 0に送出すると （ステップ S 5 ) 、 サーボ系 1 2 0は、 実際関節角が目標値になるよ うにロボッ ト本体 1 0 0の各軸サーボモータを駆動制御 する。
図示を省略するが、 C P Uは上述の関節角算出処理を 所定周期で繰り返し実行する。 即ち、 C P Uはステップ S 1 を実行する代わりに、 当該ステップ S 1 で読出した プロックに関する始点及び終点を表す教示データに基づ いて、 エン ドェフエクタの移動経路上の補間点の対応す るものにおけるエンドェフエクタの目標位置， 姿勢を従 来公知の補間処理により算出し、 次いで、 算出目標位置， 姿勢に基づいて上述のステップ S 2〜 S 5を実行する。

Claims

請 求 の 範 囲
1 . 作業空間に固定して配されたベースに第 1 の回転関 節を介して連結された第 1 リンクと前記第 1 リンクに 第 2の回転関節を介して連結された第 2 リンクと前記 δ 第 2 リ ンクに第 3の回転関節を介して連結された第 3 リンク とを有するアームと、 前記アームに装着されか つ互いの関節軸が一点で交わらない複数の回輊関節を 有すると共にェン ドエフエク タが装着されるオフセッ ト手首と、 前記エン ドエフヱク タの目標位置， 姿勢に0 基づいて前記夫々の回転関節の関節角を演箅するため の制御部とを有する垂直多閬節ロボッ ト において、 前 記第 1， 第 2の回耘関節は前記ベ -スの軸線に直交す る方向および前記第 1 リンクの軸線方向に夫々延びる 関節軸を有し、 前記制御部は、 前記エン ドエフ：！：クタ5 の目標位置， 姿勢のみの関数と して与えられる前記第
1 の回輊闋節の関節角を算出すると共に、 前記第 1 の 回転闋節以外の前記回転閬節の各々の関節角を、 先に 箅出した閬節角に基づいて算出することを特徵とする 垂直多閬節ロボッ ト。 '
0 2. 前記制御部は、 前記回転関節の夫々に設定した座標 系の相隣るもの同士間での座標変換を行うための変換 行列間に成立する所定の箅出式に従って当該所定の箅 出式に対応する閬節角を算出する請求の範囲第 1項記 载の垂直多関節ロボッ ト。
5
PCT/JP1989/000897 1988-08-31 1989-08-31 Vertical articulated robot WO1990002028A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP63/214785 1988-08-31
JP63214785A JP2652880B2 (ja) 1988-08-31 1988-08-31 垂直多関節形ロボット

Publications (1)

Publication Number Publication Date
WO1990002028A1 true WO1990002028A1 (en) 1990-03-08

Family

ID=16661497

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP1989/000897 WO1990002028A1 (en) 1988-08-31 1989-08-31 Vertical articulated robot

Country Status (4)

Country Link
US (1) US5065337A (ja)
EP (1) EP0383951A4 (ja)
JP (1) JP2652880B2 (ja)
WO (1) WO1990002028A1 (ja)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2874238B2 (ja) * 1990-01-23 1999-03-24 株式会社安川電機 多関節形ロボットの制御方法
KR950010972B1 (ko) * 1991-12-07 1995-09-26 포항종합제철주식회사 업셋(offset)이 있는 로보트손목의 기구변수 결정법
AT401318B (de) * 1992-11-27 1996-08-26 Khachatouri Yeghiazarians Vahe Fuzzy steuerungssystem für manipulatoren
WO1994017964A1 (en) * 1993-02-09 1994-08-18 Kinetic Rehabilitation Instruments, Inc. Robotic arm for wheelchair
JPH115133A (ja) * 1997-06-12 1999-01-12 Toyotomi Kiko Kk 複合型物品搬送装置
DE10041092A1 (de) * 2000-08-22 2002-03-07 Bosch Gmbh Robert Verfahren zur Korrektur eines Phasenwinkels bei der Abtastung einer Codespur
US8551114B2 (en) * 2006-11-06 2013-10-08 Human Robotics S.A. De C.V. Robotic surgical device
CN102528802B (zh) * 2010-12-31 2014-12-03 北京中科广视科技有限公司 九自由度机器人的运动驱动方法
CN103659789A (zh) * 2013-12-02 2014-03-26 西北农林科技大学 一种用于竞赛的搬运机器人
CN111496783B (zh) * 2020-03-26 2022-04-15 天津大学 一种6r工业机器人逆运动学求解方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS569184A (en) * 1979-06-30 1981-01-30 Fujitsu Fanuc Ltd Industrial robot
JPS61192487A (ja) * 1985-02-19 1986-08-27 株式会社神戸製鋼所 多関節型ロボツト

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4456961A (en) * 1982-03-05 1984-06-26 Texas Instruments Incorporated Apparatus for teaching and transforming noncoincident coordinate systems
US4623971A (en) * 1983-06-30 1986-11-18 Cincinnati Milacron Inc. Machine control with motor velocity constraint
JPS60193016A (ja) * 1984-03-14 1985-10-01 Toyota Motor Corp ロボツト装置
JPS61281305A (ja) * 1985-06-06 1986-12-11 Toyota Motor Corp 多関節ロボツト制御装置
US4680519A (en) * 1985-09-23 1987-07-14 General Electric Co. Recursive methods for world-to-joint transformation for a robot manipulator
US4823279A (en) * 1986-02-27 1989-04-18 Unimation, Inc. Coordinate conversion system and method for controlling an industrial robot
JP2595845B2 (ja) * 1991-08-30 1997-04-02 日本軽金属株式会社 耐食性に優れたブレージングシート

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS569184A (en) * 1979-06-30 1981-01-30 Fujitsu Fanuc Ltd Industrial robot
JPS61192487A (ja) * 1985-02-19 1986-08-27 株式会社神戸製鋼所 多関節型ロボツト

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP0383951A4 *

Also Published As

Publication number Publication date
JPH0265990A (ja) 1990-03-06
EP0383951A1 (en) 1990-08-29
US5065337A (en) 1991-11-12
JP2652880B2 (ja) 1997-09-10
EP0383951A4 (en) 1992-08-19

Similar Documents

Publication Publication Date Title
US4547858A (en) Dynamic control for manipulator
JP3207728B2 (ja) 冗長マニピュレータの制御方法
US9002519B2 (en) Robot control method, robot control device, and robot control system
US8140189B2 (en) Apparatus and method for computing operational-space physical quantity
KR101498836B1 (ko) 7축 다관절 로봇의 제어 장치 및 교시 방법
JPH0282302A (ja) ロボット制御装置
Qassem et al. Modeling and Simulation of 5 DOF educational robot arm
CN104866722B (zh) 一种七轴工业机械臂的逆运动学求解方法
WO1990002028A1 (en) Vertical articulated robot
JP3369351B2 (ja) 多関節マニピュレータの弾性設定方法および制御装置
JPH0720941A (ja) 作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法
JP4230196B2 (ja) 位置決め演算方法および位置決め演算装置
JPH06315880A (ja) ７軸多関節ロボットマニピュレータの肘回転方法
JP5371733B2 (ja) ７軸多関節ロボットの制御方法及び制御プログラム
Tourassis et al. Task decoupling in robot manipulators
JPH11191005A (ja) ロボット制御装置
JPH1190868A (ja) ロボット制御装置
JPH07200018A (ja) ロボットの制御装置
WO2024070568A1 (ja) 多関節ロボットの制御方法、ロボットシステム、プログラム、及び、物品の製造方法
JP3146463B2 (ja) 冗長軸マニピュレータの肘移動制御装置
JP3431026B2 (ja) 冗長軸を有する７軸以上のマニピュレータの制御方法
JPS63118804A (ja) 多関節形ロボツトの軌跡制御方式
CN108107765A (zh) 一种异构主从系统的运动映射方法及系统
JPH05177563A (ja) マスタスレーブマニピュレータの制御方法
JP2676721B2 (ja) 多関節型ロボットの制御装置

Legal Events

Date Code Title Description
AK Designated states

Kind code of ref document: A1

Designated state(s): US

AL Designated countries for regional patents

Kind code of ref document: A1

Designated state(s): AT BE CH DE FR GB IT LU NL SE

WWE Wipo information: entry into national phase

Ref document number: 1989909861

Country of ref document: EP

WWP Wipo information: published in national office

Ref document number: 1989909861

Country of ref document: EP

WWW Wipo information: withdrawn in national office

Ref document number: 1989909861

Country of ref document: EP