JP5144035B2 - Industrial robot tool position / posture control method and control system - Google Patents

Industrial robot tool position / posture control method and control system Download PDF

Info

Publication number
JP5144035B2
JP5144035B2 JP2006152202A JP2006152202A JP5144035B2 JP 5144035 B2 JP5144035 B2 JP 5144035B2 JP 2006152202 A JP2006152202 A JP 2006152202A JP 2006152202 A JP2006152202 A JP 2006152202A JP 5144035 B2 JP5144035 B2 JP 5144035B2
Authority
JP
Japan
Prior art keywords
tool
coordinate system
start point
robot
target
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.)
Active
Application number
JP2006152202A
Other languages
Japanese (ja)
Other versions
JP2007319970A (en
Inventor
泰宏 神品
成弘 澤田
光晴 浜畑
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Daihen Corp
Original Assignee
Daihen Corp
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 Daihen Corp filed Critical Daihen Corp
Priority to JP2006152202A priority Critical patent/JP5144035B2/en
Publication of JP2007319970A publication Critical patent/JP2007319970A/en
Application granted granted Critical
Publication of JP5144035B2 publication Critical patent/JP5144035B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)

Description

本発明は、産業用ロボットのツールの位置・姿勢制御方法及び制御システムに関し、詳しくは、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法、及び、当該方法に適する位置・姿勢制御システムに関する。   The present invention relates to an industrial robot tool position / posture control method and control system, and more particularly, to a method for manually controlling the position and posture of an industrial robot tool using an orthogonal coordinate system, and to the method. The present invention relates to a suitable position / posture control system.

一般に産業用ロボットの手動制御は、多関節を用いた手動制御（以下、「関節手動制御」という。）と直交座標系を用いた手動制御（以下、「直角手動制御」という。）の２種に大別される。関節手動制御では、多関節を有する所謂多関節型ロボットに備えられた各関節を個別に回転動作させる。一方、直角手動制御では、産業用ロボットが把持するツール（ロボットの先端に取り付けられ、ワーク（被溶接物等）に作用させる道具）を、予め決められた、手動制御の際に基準とする座標系（以下、「基準座標系」という。）に従って並進・回転動作させる。これらの手動制御は、通常、産業用ロボットにその姿勢や位置での動作を教え込む所謂教示作業の際に使用される。尚、産業用ロボットでアーク溶接作業を行う場合には、前記ツールは、アーク溶接トーチ（以下、単に「トーチ」という。）となる。   Generally, there are two types of manual control of industrial robots: manual control using multiple joints (hereinafter referred to as “joint manual control”) and manual control using an orthogonal coordinate system (hereinafter referred to as “right angle manual control”). It is divided roughly into. In joint manual control, each joint provided in a so-called multi-joint robot having multiple joints is individually rotated. On the other hand, in the right-angle manual control, the coordinates that the tool that the industrial robot grips (the tool that is attached to the tip of the robot and acts on the workpiece (workpiece to be welded), etc.) that is determined in advance during the manual control. Translation and rotation are performed according to a system (hereinafter referred to as “reference coordinate system”). These manual controls are usually used for so-called teaching work in which an industrial robot is instructed to operate in its posture and position. When arc welding work is performed with an industrial robot, the tool is an arc welding torch (hereinafter simply referred to as “torch”).

通常、関節手動制御は、産業用ロボットの姿勢を大きく変化させるときだけ使用され、その他の大多数の教示作業では、トーチ等が教示により移動される教示点へのツールの位置決めやツールの姿勢変更には、直角手動制御が使用される。このように産業用ロボットの手動制御において、直角手動制御が多用されるのは、関節手動制御では、トーチの位置と姿勢とを分離して手動制御することができないが、直角手動制御ではこれが可能であり、産業用ロボットのツールを所望の位置や姿勢とし易いためである。   Normally, manual joint control is used only when the attitude of an industrial robot is greatly changed. In most other teaching operations, the tool is positioned at a teaching point where the torch or the like is moved by teaching or the tool attitude is changed. For this, right-angle manual control is used. In manual control of industrial robots, right-angle manual control is often used. Joint joint control cannot separate torch position and posture manually, but right-angle manual control allows this. This is because the industrial robot tool can be easily set to a desired position and posture.

また、産業用ロボットの直角手動制御では、ツールの位置や姿勢の変化の自由度を増やすべく、通常、基準座標系を、複数の座標系から選択できるようになっている。これら基準座標系には、ベース座標系、ツール座標系、ワーク座標系、ショップフロア座標系、任意座標系等がある。これら基準座標系が変われば、産業ロボットに手動制御の動作内容を教示する手段（手動運転命令手段）であるティーチングペンダント（以下、「ＴＰ」と略す。）上で同じキー操作を行っても、ツールの並進動作や回転動作が変わるようになる。   In addition, in the right-angle manual control of industrial robots, the reference coordinate system can usually be selected from a plurality of coordinate systems in order to increase the degree of freedom in changing the position and posture of the tool. These reference coordinate systems include a base coordinate system, a tool coordinate system, a work coordinate system, a shop floor coordinate system, and an arbitrary coordinate system. If these reference coordinate systems are changed, even if the same key operation is performed on a teaching pendant (hereinafter abbreviated as “TP”) which is a means (manual operation command means) for teaching the operation contents of manual control to the industrial robot, The translation and rotation of the tool will change.

ところで、前記直角手動制御では、産業用ロボットに行わせる作業や動作の種類に応じて適切な座標系が存在する。例えば、ワークに沿ってツールの位置を並進動作により変更したいときはワーク座標系を基準とし、ロボットの設置位置を基準としてツールを並進動作させたいときはベース座標系を基準とすると、それぞれの場合にロボットの手動制御が容易化される。また、トーチの姿勢を回転動作により変更したいときは、ツールの取り付け部位に基づくツール座標系を基準とすると、ロボットの手動制御が容易化される。これは、通常、ツール座標系では、ツールの形状やツールに接続されるワイヤの送給方向に一致するように座標系が設定されているためである。   By the way, in the right-angle manual control, there is an appropriate coordinate system according to the type of work and operation to be performed by the industrial robot. For example, if you want to change the tool position along the workpiece by translation, use the workpiece coordinate system as a reference, and if you want to translate the tool based on the robot installation position, use the base coordinate system as a reference. In addition, manual control of the robot is facilitated. Further, when the posture of the torch is desired to be changed by a rotation operation, manual control of the robot is facilitated based on the tool coordinate system based on the attachment site of the tool. This is because, in the tool coordinate system, the coordinate system is usually set to match the shape of the tool and the feeding direction of the wire connected to the tool.

ところが、従来は、直角手動制御において産業用ロボットのツールの位置や姿勢を変えるにあたり、並進動作及び回転動作の各動作には、常に同一の基準座標系が予め設定され、用いられている（例えば、特許文献１参照）。
特開平１１−１８８６７６号公報 However, conventionally, in changing the position and posture of an industrial robot tool in right-angle manual control, the same reference coordinate system is always preset and used for each of the translation operation and the rotation operation (for example, , See Patent Document 1).
JP-A-11-188676

このため、作業者は、主としてツールの並進動作を手動制御するときと、その回転動作を手動制御するときとで、その都度、各動作に適した基準座標系に切り替えることが必要となり、手動制御が著しく煩雑化することで産業用ロボットの教示作業に長時間を要し、作業効率を大きく低下させていた。   For this reason, it is necessary for the operator to switch to a reference coordinate system suitable for each operation each time when manually controlling the translation operation of the tool and when manually controlling the rotation operation. However, since the work is extremely complicated, it takes a long time to teach the industrial robot, which greatly reduces the work efficiency.

本発明は、上記問題点を解決するためになされたものであって、その目的は、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行うことなく、簡易に産業用ロボットのツールを手動制御することができる産業用ロボットのツールの位置・姿勢制御方法及び当該方法を実現する位置・姿勢制御システムを提供することにある。   The present invention has been made to solve the above-described problems, and its object is to manually control the position and posture of an industrial robot tool using an orthogonal coordinate system. Industry that can easily manually control industrial robot tools without performing complicated operations such as switching to a reference coordinate system suitable for each of the translation and rotation operations of industrial robot tools. It is an object to provide a position / posture control method for a robot tool and a position / posture control system for realizing the method.

上記問題点を解決するために、請求項１に記載の発明は、多関節を有する産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、（１）ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成ステップと、（２）前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算ステップと、（３）前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算ステップと、（４）前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成ステップと、（５）前記指令を前記産業用ロボットの各関節を動かすサーボ機構に与えることで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御ステップと、を備え、前記開始点座標計算ステップ及び累積移動量計算ステップの各ステップにおいて、前記ロボットのツールの並進動作及び回転動作の各動作に対してそれぞれ個別に１つ、前記手動制御前に予め設定した基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたこと、を要旨とする。 In order to solve the above problems, the invention described in claim 1 is a method of manually controlling the position and orientation of a tool of an industrial robot having multiple joints using an orthogonal coordinate system, (1) A manual operation command generation step of detecting that manual control is performed on the robot work command teaching means and generating a manual operation command to the industrial robot in response to the detection; (2) the manual operation A start point coordinate calculating step for calculating a start point coordinate of manual control of the tool based on the command; and (3) a position of the tool at a predetermined interpolation cycle based on the manual operation command and the start point coordinate; In addition to calculating the data related to the posture, the position and posture data are cumulatively calculated in the entire interpolation period from the start point represented by the start point coordinates to the target point of the tool, respectively, and the translation operation and rotation are performed. A cumulative movement amount calculating step for obtaining a cumulative movement amount of the operation; and (4) converting the cumulative movement amount into a joint angle of each joint of the industrial robot, so that the tool is changed from the start point to the target point. A tool position command generation step for generating a position command to be reached; and (5) controlling the servo mechanism by giving the command to a servo mechanism that moves each joint of the industrial robot, thereby causing the tool to move to the start point. A servomechanism control step for causing the robot to reach the target point, and in each step of the start point coordinate calculation step and the cumulative movement amount calculation step, for each of the translation operation and the rotation operation of the tool of the robot , one, individually, based on the reference coordinate system set in advance before the manual control, the start point coordinates, the position and orientation data, or for each operation It has to calculate the product movement amount, and the gist.

上記請求項１の構成によれば、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する前に、該ツールの並進動作及び回転動作の各動作に対してそれぞれ適した基準座標系を設定することができる。これにより、前記手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行う必要がなくなり、作業者が簡易に産業用ロボットのツールを手動制御することができるようになる。その結果、産業用ロボットへ作業プログラムを教示するために要する教示時間が低減され、産業用ロボットの教示作業が簡略化されるようになる。   According to the configuration of the first aspect, before manually controlling the position and orientation of the tool of the industrial robot using the orthogonal coordinate system, the tool is suitable for each of the translation operation and the rotation operation of the tool. A reference coordinate system can be set. This eliminates the need for a complicated operation of switching to a reference coordinate system suitable for each of the translation operation and the rotation operation of the tool of the industrial robot during the manual control. The robot tool can be controlled manually. As a result, the teaching time required for teaching the work program to the industrial robot is reduced, and the teaching work of the industrial robot is simplified.

また、上記請求項１の構成によれば、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、簡略的且つ系統だったプロセスで実現することができる。 In addition, according to the configuration of the first aspect , a method for manually controlling the position and orientation of an industrial robot using an orthogonal coordinate system, the translation operation and the rotation operation of a tool provided in the industrial robot. A method for performing manual control of translation and rotation based on each reference coordinate system after setting the reference coordinate system for each operation individually for each operation can be realized by a simple and systematic process. it can.

請求項２に記載の発明は、請求項１に記載のツールの位置・姿勢制御方法において、前記ツール位置指令生成ステップには、（ｉ）前記位置及び姿勢の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列を計算する等価並進・回転行列計算ステップと、（ii）前記開始点座標と、前記等価並進行列及び等価回転行列とに基づいて、前記ツールを前記開始点から目標点に到達させる目標並進行列及び目標回転行列を計算する目標並進・回転行列計算ステップと、（iii）前記目標並進行列及び目標回転行列に基づいて前記産業用ロボットの各関節角度を計算するロボット関節角度計算ステップと、が含まれること、を要旨とする。 The invention according to claim 2, in the position and posture control method tool of claim 1, the said tool position command generation step, the corresponding equivalent translation respectively the cumulative amount of movement of (i) the position and orientation An equivalent translation / rotation matrix calculation step for calculating a matrix and an equivalent rotation matrix; and (ii) the tool reaches the target point from the start point based on the start point coordinates, the equivalent parallel progression sequence and the equivalent rotation matrix. A target translation / rotation matrix calculation step for calculating a target translation sequence and a target rotation matrix to be performed; and (iii) a robot joint angle calculation step for calculating each joint angle of the industrial robot based on the target translation sequence and the target rotation matrix. And is included in the gist.

上記請求項２の構成によれば、産業用ロボットのツールの並進動作及び回転動作の累積移動量を各関節の関節角度に逆変換することで、ツールを開始点から目標点に到達させる位置指令を生成する方法を、簡略的且つ系統だったプロセスで実現することができる。 According to the configuration of claim 2 , the position command for causing the tool to reach the target point from the starting point by inversely converting the cumulative movement amount of the translation operation and rotation operation of the tool of the industrial robot into the joint angle of each joint. Can be realized by a simple and systematic process.

請求項３に記載の発明は、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成手段と、前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算手段と、前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算手段と、前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成手段と、前記位置指令を前記産業用ロボットの各関節を動かすサーボ機構に送信することで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御手段と、を備え、前記開始点座標計算手段及び累積移動量計算手段の各手段において、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別に１つ、前記手動制御前に予め設定した基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたこと、を要旨とする。 The invention described in claim 3 is a control system used for manually controlling the position and orientation of a tool of an industrial robot using an orthogonal coordinate system, and manually controlling the teaching means of the robot work instruction. And a manual operation command generating means for generating a manual operation command to the industrial robot in response to the detection, and a start point coordinate of the manual control of the tool based on the manual operation command Based on the start point coordinate calculation means for calculating the manual operation command and the start point coordinates, the data on the position and orientation of the tool is calculated for each predetermined interpolation period, and the position and orientation data are Cumulative movement amount calculation unit that obtains cumulative movement amount of translational operation and rotational operation by accumulative calculation in all interpolation cycles from the starting point represented by the starting point coordinates to the target point of the tool And a tool position command generating means for generating a position command for causing the tool to reach a target point from the start point by inversely converting the cumulative movement amount into a joint angle of each joint of the industrial robot, Servo mechanism control means for controlling the servo mechanism by transmitting a position command to a servo mechanism for moving each joint of the industrial robot, thereby causing the tool to reach the target point from the start point; Each of the starting point coordinate calculating means and the cumulative movement amount calculating means is individually set in advance for each of the translation operation and the rotation operation of the tool of the robot, and set in advance before the manual control. Based on a reference coordinate system, the starting point coordinates, position and orientation data, or the cumulative movement amount of each operation is calculated.

上記請求項３の構成によれば、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、簡略的且つ系統だった制御システムで実現することができる。 According to the configuration of claim 3 , a method for manually controlling the position and orientation of an industrial robot using an orthogonal coordinate system, wherein the tool includes a translation operation and a rotation operation of the industrial robot. After the reference coordinate system to be used is individually set for each operation, a method for performing manual control of the translation operation and the rotation operation based on each reference coordinate system can be realized with a simple and systematic control system. .

本発明によれば、産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該手動制御の最中に、産業用ロボットのツールの並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行うことなく、簡易に産業用ロボットのツールを手動制御することができる産業用ロボットのツールの位置・姿勢制御方法及び当該方法を実現する位置・姿勢制御システムを提供することができる。   According to the present invention, when manually controlling the position and orientation of the tool of the industrial robot using the orthogonal coordinate system, each of the translation operation and the rotation operation of the tool of the industrial robot is performed during the manual control. Industrial robot tool position / posture control method that can easily control the industrial robot tool manually without performing a complicated operation of switching to a reference coordinate system suitable for each, and a position for realizing the method -An attitude control system can be provided.

以下、本発明の実施形態について図面を参照しながら説明する。
図１に本実施形態で用いる多関節型ロボット装置の制御システムを示す。該制御システムには、多関節型ロボット１、ポジショナ２、前記多関節型ロボット１とポジショナ２とを制御するロボット制御装置３、及び前記多関節型ロボット１とポジショナ２とに駆動電力を供給する溶接電源装置４が設けられている。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 shows a control system for an articulated robot apparatus used in this embodiment. The control system supplies driving power to the articulated robot 1, the positioner 2, the robot control device 3 that controls the articulated robot 1 and the positioner 2, and the articulated robot 1 and the positioner 2. A welding power source device 4 is provided.

前記多関節型ロボット１は、トーチ（ツール）１１と、５個（複数個）のアーム１２ａ，１２ｂ，１２ｃ，１２ｄ，１２ｅと、各アーム間に存在する各関節の関節角度を変えるための６個（複数個）のサーボモータ（駆動モータ）１３ａ，１３ｂ，１３ｃ，１３ｄ，１３ｅ、１３ｆ（但し、サーボモータ１３ｂとサーボモータ１３ｅとは図示省略）とを備えている（トーチ１１側よりロボット１が設置される大地面に向けてアルファベット順で示す。）。これらサーボモータ１３ａ〜１３ｆには、ロボット制御装置３のサーボドライバより回転駆動電力が供給されるともに、ロボット制御装置３から送信される制御信号によって制御される。   The articulated robot 1 includes a torch (tool) 11, five (plural) arms 12 a, 12 b, 12 c, 12 d, 12 e, and 6 for changing the joint angle of each joint existing between the arms. (A plurality of) servo motors (drive motors) 13a, 13b, 13c, 13d, 13e, 13f (however, the servo motor 13b and the servo motor 13e are not shown) (the robot 1 from the torch 11 side). Shown in alphabetical order toward the ground where is installed.) These servomotors 13 a to 13 f are supplied with rotational driving power from a servo driver of the robot control device 3 and are controlled by a control signal transmitted from the robot control device 3.

また、前記多関節型ロボット１は、トーチ１１を把持するためのフランジ部（面）（符号省略）を有するエンドエフェクタ１４を更に備えており、このトーチ１１は、アーム１２ａを介して前記フランジ部に取り付けられている。この多関節型ロボット１では、トーチ１１を用いてワーク（被溶接物）に対してアーク溶接（溶接）が行われる。尚、図１に示すトーチ１１の直棒状の先端部１１ａは、その基準位置において大地面に対して略直交するようにされている。   Further, the articulated robot 1 further includes an end effector 14 having a flange portion (surface) (reference numeral omitted) for gripping the torch 11, and the torch 11 is connected to the flange portion via an arm 12a. Is attached. In the articulated robot 1, arc welding (welding) is performed on a work (workpiece) using a torch 11. Note that the straight rod-shaped tip portion 11a of the torch 11 shown in FIG. 1 is substantially orthogonal to the ground plane at the reference position.

このように、多関節型ロボット１は、複数個のサーボモータ１３ａ〜１３ｆがロボット制御装置３により制御され、回転駆動されることによって、多関節型ロボット１の各関節が回転し、且つアーム１２ａ〜１２ｅが移動する。これにより、エンドエフェクタ１４のフランジ部に取り付けられたトーチ１１が上下前後左右に移動可能とされ、所望の位置とされるとともに、その姿勢も所望の状態とされる。   As described above, in the multi-joint robot 1, the plurality of servo motors 13a to 13f are controlled by the robot controller 3 and driven to rotate, whereby each joint of the multi-joint robot 1 rotates and the arm 12a. ~ 12e move. As a result, the torch 11 attached to the flange portion of the end effector 14 can be moved up and down, front and rear, left and right, and is brought to a desired position and its posture is also made a desired state.

前記多関節型ロボット１のアーム１２ｃ近傍には、トーチ１１に対して溶接ワイヤ（符号省略）を送給するワイヤ送給装置１５が設けられている。該ワイヤ送給装置１５は、溶接ワイヤが巻回されたリールと、該リールを回転させる送給モータ（いずれも図示省略）とを備え、この送給モータは、溶接電源装置４からの電力によって所定のタイミングで回転駆動される。前記溶接ワイヤを介して溶接電源装置４からトーチ１１に高電圧の溶接電圧が供給されることで溶接ワイヤの先端とワークとの間にアークが発生し、その熱で溶接ワイヤが溶融することで、ワークが溶接される。   Near the arm 12 c of the articulated robot 1, a wire feeding device 15 that feeds a welding wire (not shown) to the torch 11 is provided. The wire feeding device 15 includes a reel around which a welding wire is wound, and a feeding motor (both not shown) that rotates the reel. The feeding motor is powered by electric power from the welding power source device 4. It is rotationally driven at a predetermined timing. When a high welding voltage is supplied from the welding power source device 4 to the torch 11 via the welding wire, an arc is generated between the tip of the welding wire and the workpiece, and the welding wire is melted by the heat. The workpiece is welded.

本実施形態の制御システムにおいて、前記ポジショナ２は、多関節型ロボット１のトーチ１１と対向し、ワークを回転又は傾斜させて当該ワークをトーチ１１による溶接に適する状態とする。詳述すると、このポジショナ２は、溶接時にワークが載置される略円形のワーク載置面２１ａを有するワーク載置台２１と、該ワーク載置台２１を所定方向に回転又は傾斜自在に支持する支持台２１ｂとを備えている。そして、前記トーチ１１は、その先端部１１ａがワーク載置面２１ａに対向するように多関節型ロボット１に設けられている。このようにして、ポジショナ２がワークを自在に回転又は傾斜させるとともに、前述したように複数個のサーボモータ１３ａ〜１３ｆによってアーム１２ａ〜１２ｅを変位させることで、ワークがトーチ１１に対して溶接に適する所要の位置及び姿勢関係に設定される。尚、ポジショナ２のワーク載置台２１は、支持台２１ｂの側部に設けられたサーボモータ２２によって大地面に対して傾斜可能とされている。このサーボモータ２２には、その下方位置にて支持台２１ｂに設けられた電源ボックス２３を介して溶接電源装置４から回転駆動電力が供給される。   In the control system of the present embodiment, the positioner 2 faces the torch 11 of the articulated robot 1 and rotates or tilts the workpiece so that the workpiece is suitable for welding by the torch 11. More specifically, the positioner 2 includes a workpiece mounting table 21 having a substantially circular workpiece mounting surface 21a on which a workpiece is mounted during welding, and a support that supports the workpiece mounting table 21 so that the workpiece mounting table 21 can rotate or tilt in a predetermined direction. And a table 21b. The torch 11 is provided in the articulated robot 1 so that the tip 11a faces the workpiece placement surface 21a. In this way, the positioner 2 freely rotates or tilts the workpiece, and the workpieces are welded to the torch 11 by displacing the arms 12a to 12e by the plurality of servo motors 13a to 13f as described above. It is set to a suitable required position and posture relationship. The workpiece mounting table 21 of the positioner 2 can be tilted with respect to the ground by a servo motor 22 provided on the side of the support table 21b. Rotational driving power is supplied to the servo motor 22 from the welding power source device 4 via a power box 23 provided on the support base 21b at a lower position.

また、図１に示すように、本実施形態の制御システムにおいては、多関節型ロボット１の手動制御にあたり、複数の基準座標系が用いられる。これら基準座標系としては、多関節型ロボット１側には、大地を基準に設定され、本制御システムで絶対的な基準となるワールド座標系（Ｏ-Ｘ０，Ｙ０，Ｚ０）、多関節型ロボット１のアーム１２ｅを基準に設定されたベース座標系（Ｏ１-Ｘ１，Ｙ１，Ｚ１）、トーチ１１が取り付けられるフランジ面（取り付け面）を基準に設定されたエンドエフェクタ座標系（Ｏ２-Ｘ２，Ｙ２，Ｚ２）、トーチ１１を基準に設定されたツール座標系（Ｏ３-Ｘ３，Ｙ３，Ｚ３）が定義される。また、ポジショナ２側には、ポジショナ２の設置面を基準に設定されたポジショナ座標系（Ｏ４-Ｘ４，Ｙ４，Ｚ４）、及び、ポジショナ２のワーク載置面２１ａを基準に設定されたワーク座標系（Ｏ５-Ｘ５，Ｙ５，Ｚ５）が定義される。本実施形態における多関節型ロボット１の直角手動制御では、このように定義された複数の基準座標系を用いて、トーチ１１の位置及び姿勢が表現される。   Further, as shown in FIG. 1, in the control system of the present embodiment, a plurality of reference coordinate systems are used for manual control of the articulated robot 1. As these reference coordinate systems, the world coordinate system (O-X0, Y0, Z0), which is set on the ground on the articulated robot 1 side and is an absolute reference in this control system, is an articulated robot. Base coordinate system (O1-X1, Y1, Z1) set with reference to one arm 12e, and end effector coordinate system (O2-X2, Y2) set with reference to the flange surface (attachment surface) to which the torch 11 is attached , Z2), and a tool coordinate system (O3-X3, Y3, Z3) set with reference to the torch 11 is defined. Further, on the positioner 2 side, a positioner coordinate system (O4-X4, Y4, Z4) set with reference to the installation surface of the positioner 2 and a work coordinate set with reference to the work placement surface 21a of the positioner 2 The system (O5-X5, Y5, Z5) is defined. In the right-angle manual control of the articulated robot 1 in this embodiment, the position and orientation of the torch 11 are expressed using a plurality of reference coordinate systems defined in this way.

前記ロボット制御装置３は、図２に示すように、本ロボット制御装置３の制御中枢となるＣＰＵ３１、多関節型ロボット１の作業や動作を規定する作業プログラムの各バッファとして機能するＲＡＭ３２、作業プログラムが格納されるＲＯＭ３３、及び所定の定期時刻毎に同期信号をＣＰＵ３１へ発生するタイマー（ＴＩＭＥＲ）３４を備えている。さらに、該ロボット制御装置３は、多関節型ロボット１に対して手動制御によって教示された教示データが格納されるハードディスク３５、及び、手動制御時に使用し、ＴＰ３６ａが接続されるティーチングペンダントＩ／Ｆ（インターフェース）３６を備えている。このＴＰ３６ａは、多関節型ロボット１に前記作業プログラムを教示する教示手段として用いる。さらに、該ロボット制御装置３は、操作ボックスＩ／Ｆ３７及びサーボドライバＩ／Ｆ３８を備えている。この操作ボックスＩ／Ｆ３７には、教示した作業プログラムのタイミングをＣＰＵ３１に与える操作ボックス３７ａが接続されている。   As shown in FIG. 2, the robot control device 3 includes a CPU 31 serving as a control center of the robot control device 3, a RAM 32 functioning as a buffer for a work program for defining work and operation of the articulated robot 1, and a work program. Is stored, and a timer (TIMER) 34 that generates a synchronization signal to the CPU 31 at predetermined time intervals. Further, the robot control device 3 includes a hard disk 35 that stores teaching data taught by manual control for the articulated robot 1, and a teaching pendant I / F that is used during manual control and connected to the TP 36a. (Interface) 36 is provided. The TP 36a is used as teaching means for teaching the articulated robot 1 the work program. Further, the robot control device 3 includes an operation box I / F 37 and a servo driver I / F 38. Connected to the operation box I / F 37 is an operation box 37a that gives the CPU 31 the timing of the taught work program.

また、サーボドライバＩ／Ｆ３８には、多関節型ロボット１の各関節を駆動する前記した複数のサーボドライバ（サーボ機構）３８ａ，…が接続されている。そして、サーボドライバ３８ａ，…には、各サーボドライバ３８ａによって制御可能な状態で、前記したサーボモータ１３ａ〜１３ｆ（図１参照）が接続されている。詳述すると、各サーボドライバ３８ａの制御は、ＣＰＵ３１からの動作指令信号に基づいて行われ、図２に示す多関節型ロボット１を、６個のサーボモータ１３ａ〜１３ｆによってそれぞれ各アーム１２ａ〜１２ｅ間の関節角度θ１，θ２，θ３，θ４，θ５，θ６で回転するようにされている。   The servo driver I / F 38 is connected to the plurality of servo drivers (servo mechanisms) 38a,... For driving each joint of the articulated robot 1. The servo drivers 38a,... Are connected to the servo motors 13a to 13f (see FIG. 1) in a state that can be controlled by each servo driver 38a. More specifically, each servo driver 38a is controlled on the basis of an operation command signal from the CPU 31, and the articulated robot 1 shown in FIG. 2 is controlled by the six servo motors 13a to 13f. It is designed to rotate at joint angles θ1, θ2, θ3, θ4, θ5, and θ6.

以上説明したＣＰＵ３１、ＲＡＭ３２、ＲＯＭ３３、タイマー３４、ハードディスク３５、ＴＰ３６ａ、操作ボックスＩ／Ｆ３７、及びサーボドライバＩ／Ｆ３８は、バス（ＢＵＳ）３９を介して相互に接続され、上述した各機能を実現すべく必要な信号をやりとりするように構成されている。   The CPU 31, RAM 32, ROM 33, timer 34, hard disk 35, TP 36 a, operation box I / F 37, and servo driver I / F 38 described above are connected to each other via a bus (BUS) 39 to realize each function described above. It is configured to exchange necessary signals as much as possible.

本実施形態の制御システムを構成する主要部について詳述すると、前記ＣＰＵ３１は、前記作業プログラム、ＴＰ３６ａや操作ボックス３７ａから送信される操作信号、又は、各サーボモータ１３ａ〜１３ｆに設けられた図示しないエンコーダからの現在位置情報等に基づいて、所定のデータ演算処理を行い、バス３９及びサーボドライバＩ／Ｆ３８を介して各サーボドライバ３８ａに動作指令を与える。これにより、サーボモータ１３ａ〜１３ｆが制御され回転駆動され、前述したように、トーチ１１がワークに対して溶接に適した所要の位置及び姿勢関係とされる。また、前記ＲＯＭ３３には、前記作業プログラムの他、この作業プログラムの実行条件を示すデータ、及び制御定数を示すデータ等が格納される。   Describing in detail the main parts constituting the control system of the present embodiment, the CPU 31 is not shown in the figure provided in the work program, the operation signal transmitted from the TP 36a or the operation box 37a, or the servo motors 13a to 13f. Based on current position information from the encoder and the like, predetermined data calculation processing is performed, and an operation command is given to each servo driver 38a via the bus 39 and the servo driver I / F 38. As a result, the servomotors 13a to 13f are controlled and rotated, and as described above, the torch 11 has a required position and posture relationship suitable for welding with respect to the workpiece. In addition to the work program, the ROM 33 stores data indicating execution conditions of the work program, data indicating control constants, and the like.

また、前記ＴＰ３６ａは、多関節型ロボット１を手動制御するときに作業者によって操作され、所定の操作情報が表示される表示装置３６ｂと所定の情報の入力に用いるキーボード３６ｃとを備えている。該キーボード３６ｃには、手動制御操作用のスイッチ（以下、「軸キー」と呼ぶ。）が１２個設けられている（図示省略）。このようにキーボード３６ｃに１２個の軸キーが設けられているのは、本実施形態のように多関節型ロボット１を直角手動制御で操作する場合では、位置（３自由度）及び姿勢（３自由度）からなる計６自由度に対して、それぞれ正（＋）方向及び負（−）方向の２つの方向に対応する数の軸キーが使用される（６自由度×２つ＝１２個）ためである。   The TP 36a includes a display device 36b that is operated by an operator when manual control of the articulated robot 1 and displays predetermined operation information, and a keyboard 36c used for inputting predetermined information. The keyboard 36c is provided with twelve switches for manual control operation (hereinafter referred to as “axis keys”) (not shown). Thus, the twelve axis keys are provided on the keyboard 36c when the articulated robot 1 is operated by right-angled manual control as in this embodiment, in position (3 degrees of freedom) and posture (3 The number of axis keys corresponding to two directions of the positive (+) direction and the negative (−) direction is used (6 degrees of freedom × 2 = 12) for a total of 6 degrees of freedom. For).

前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の基準座標系を設定するための不図示のスイッチ（以下、「座標系スイッチ」という。）が備えられている。本実施形態において、並進動作及び回転動作の各動作の基準座標系としては、前述したワールド座標系、ベース座標系、ワーク座標系、及びツール座標系の４個の座標系が使用でき、それぞれの座標系について、１から４の間の整数で表される座標系番号が識別のために割り当てられる。ここで、並進動作の座標系番号が入力されるパラメータ変数には、CoorParaを使用し、回転動作の座標系番号が入力されるパラメータ変数には、CoorRotを使用する。作業者は、前記した座標系番号から所望の数字を選択し、前記座標系スイッチを使用して、CoorPara及びCoorRotにそれぞれ個別に座標系番号を入力する。   On the keyboard 36c, when operating the articulated robot 1 by right-angled manual control, the operator sets an individual reference coordinate system for each of the translation operation and the rotation operation of the torch 11 (not shown). Switch (hereinafter referred to as “coordinate system switch”). In the present embodiment, as the reference coordinate system for each of the translation operation and the rotation operation, the above-described four coordinate systems of the world coordinate system, the base coordinate system, the work coordinate system, and the tool coordinate system can be used. For the coordinate system, a coordinate system number represented by an integer between 1 and 4 is assigned for identification. Here, CoorPara is used for the parameter variable to which the coordinate system number for the translation operation is input, and CoorRot is used for the parameter variable to which the coordinate system number for the rotation operation is input. The operator selects a desired number from the coordinate system numbers described above, and inputs the coordinate system numbers individually to CoorPara and CoorRot using the coordinate system switch.

また、前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の動作速度を設定するための不図示のスイッチ（以下、「速度スイッチ」という。）も備えられている。本実施形態において、並進動作の速度（単位：［mm/sec］）が入力されるパラメータ変数には、ManuParaVelを使用し、回転動作の速度（単位：［rad/sec］）が入力されるパラメータ変数には、ManuRotVelを使用する。作業者は、前記速度スイッチを使用して、ManuParaVel及びManuRotVelにそれぞれ個別に速度を入力する。   In addition, the keyboard 36c is not used for the operator to set individual operation speeds for each of the translation operation and the rotation operation of the torch 11 when operating the articulated robot 1 by right-angle manual control. The illustrated switch (hereinafter referred to as “speed switch”) is also provided. In the present embodiment, ManuParaVel is used as a parameter variable to which the speed of translation operation (unit: [mm / sec]) is input, and the parameter to which the speed of rotation operation (unit: [rad / sec]) is input. For the variable, use ManuRotVel. The operator inputs the speed individually into ManuParaVel and ManuRotVel using the speed switch.

さらに、前記キーボード３６ｃには、多関節型ロボット１を直角手動制御で操作するにあたり、作業者がトーチ１１の並進動作及び回転動作の各動作に対してそれぞれ個別の動作方向を指示するための不図示のスイッチ（以下、「動作方向指示スイッチ」という。）も備えられている。本実施形態において、例えば、Ｘ軸方向の並進動作については、正（＋）方向への並進動作の指示の有無（０：なし、１：有り）が入力されるパラメータ変数には、AxesKey_Xpulsを使用し、負（−）方向への並進動作の指示の有無（０：なし、１：有り）が入力されるパラメータ変数には、AxesKey_XMinusを使用する。作業者は、前記動作方向指示スイッチを使用して、AxesKey_Xpuls及びAxesKey_XMinus等にそれぞれ個別に動作指示の有無を入力する。   Further, the keyboard 36c is not used for the operator to instruct the individual movement directions for the translational operation and the rotational operation of the torch 11 when operating the articulated robot 1 by right-angled manual control. The illustrated switch (hereinafter referred to as “operation direction instruction switch”) is also provided. In this embodiment, for example, for the translation operation in the X-axis direction, AxesKey_Xpuls is used as a parameter variable to which the presence / absence of the translation operation in the positive (+) direction is input (0: no, 1: yes) AxesKey_XMinus is used as a parameter variable to which the presence / absence of a translation operation instruction in the negative (−) direction (0: no, 1: yes) is input. The worker inputs the presence / absence of an operation instruction individually to AxesKey_Xpuls, AxesKey_XMinus, and the like using the operation direction instruction switch.

下表１に、トーチ１１の並進動作及び回転動作に用いる基準座標系のパラメータ変数（CoorPar及びCoorRot）、当該並進動作及び回転動作の各動作の速度のパラメータ変数（ManuParaVel及びManuRotVel）、及び当該並進動作及び回転動作の各動作における２つの動作方向、即ち、正（＋）方向及び負（−）方向のパラメータ変数（AxesKey_Xpuls及びAxesKey_XMinus等）、から構成される動作指示の一覧を示す。

Figure 0005144035
Table 1 below shows the parameter variables (CoorPar and CoorRot) of the reference coordinate system used for the translation operation and the rotation operation of the torch 11, the parameter parameters of the speed of each operation of the translation operation and the rotation operation (ManuParaVel and ManuRotVel), and the translation A list of operation instructions composed of parameter variables (AxesKey_Xpuls, AxesKey_XMinus, etc.) in two operation directions, that is, a positive (+) direction and a negative (−) direction in each operation of the operation and the rotation operation is shown.
Figure 0005144035

前記ＣＰＵ３１は、これらの各種のパラメータ変数で表される操作信号を受け取ることによって所定のデータ処理を行うとともに、ＴＰ３６ａに表示データを送信して前記表示装置３６ｂに操作情報を表示させる。さらに、ＣＰＵ３１は、これら操作信号を、後述する手動運転命令生成部５１（図４参照）によって教示データの形で読み出し可能な状態でハードディスク３５に格納する。   The CPU 31 performs predetermined data processing by receiving operation signals represented by these various parameter variables, and transmits display data to the TP 36a to display operation information on the display device 36b. Further, the CPU 31 stores these operation signals in the hard disk 35 in a state where the operation signals can be read out in the form of teaching data by a manual operation command generation unit 51 (see FIG. 4) described later.

以下、図３を参照しつつ、前記したサーボドライバ３８ａにおけるサーボモータ１３ａ〜１３ｆのサーボ制御系について簡単に説明する。このサーボ制御系には、位置制御ブロック３００、速度制御ブロック３０１、及び電流制御ブロック３０２が設けられており、各ブロックによって、後述する多関節型ロボット１の各関節に対する位置指令にサーボモータ１３ａ〜１３ｆの動作を追従させることが可能となるとともに、教示データの再生が実行される。詳述すると、ＣＰＵ３１から送信される前記位置指令は、先ず、位置制御ブロック３００に入力され、該位置制御ブロック３００からの出力は、速度制御ブロック３０１に入力される。そして、この速度制御ブロック３０１からの出力は、電流制御ブロック３０２に入力され、電流制御ブロック３０２の出力は、サーボモータ１３ａ〜１３ｆに入力される。サーボモータ１３ａ〜１３ｆからの出力は、ばね等の減速機構３０４を介して、負荷となるアーム１２ａ〜１２ｅに伝えられる。   Hereinafter, the servo control system of the servo motors 13a to 13f in the servo driver 38a will be briefly described with reference to FIG. This servo control system is provided with a position control block 300, a speed control block 301, and a current control block 302, and servo motors 13a to 13b are used to send position commands to the joints of the multi-joint robot 1 to be described later. It is possible to follow the operation of 13f, and the teaching data is reproduced. Specifically, the position command transmitted from the CPU 31 is first input to the position control block 300, and the output from the position control block 300 is input to the speed control block 301. The output from the speed control block 301 is input to the current control block 302, and the output of the current control block 302 is input to the servo motors 13a to 13f. Outputs from the servo motors 13a to 13f are transmitted to the arms 12a to 12e serving as loads via a speed reduction mechanism 304 such as a spring.

各サーボモータ１３ａ〜１３ｆには、それぞれ電流検出ブロック３０３が接続され、各電流検出ブロック３０３において各サーボモータ１３ａ〜１３ｆに流れる電流が検出され、該電流値は電流制御ブロック３０２にフィードバックされる。また、各サーボモータ１３ａ〜１３ｆには、それぞれエンコーダ３０５が接続され、各エンコーダ３０５によって現在のアームの移動速度に対応する各サーボモータ１３ａ〜１３ｆの回転速度に関するデータが取得され、該取得された回転速度データは、位置制御ブロック３００と速度制御ブロック３０１とにフィードバックされる。   A current detection block 303 is connected to each of the servo motors 13a to 13f. In each current detection block 303, a current flowing through each of the servo motors 13a to 13f is detected, and the current value is fed back to the current control block 302. Each of the servo motors 13a to 13f is connected to an encoder 305, and the encoder 305 acquires data related to the rotational speed of each servo motor 13a to 13f corresponding to the current arm moving speed. The rotational speed data is fed back to the position control block 300 and the speed control block 301.

前記位置制御ブロック３００では、電流制御ブロック３０２にフィードバックされた前記回転速度データが積分ブロック３０６によって積分されることにより位置データに変換されて減算器３０７に入力され、この減算器３０７により多関節型ロボット１の各関節に対する位置指令に対する現在の各関節の位置（関節角度）の誤差データが演算される。この誤差データは増幅器３０８により所定のゲインＫｐｐ（以下、「位置ＦＢゲイン」という。）でレベル補正が行なわれた後、加算器３０９に入力される。   In the position control block 300, the rotational speed data fed back to the current control block 302 is integrated by the integration block 306 to be converted into position data and input to the subtractor 307, and this subtractor 307 is an articulated type. Error data of the current position (joint angle) of each joint with respect to the position command for each joint of the robot 1 is calculated. The error data is level-corrected by the amplifier 308 with a predetermined gain Kpp (hereinafter referred to as “position FB gain”) and then input to the adder 309.

また、位置制御ブロック３００では、多関節型ロボット１の各関節に対する位置指令のデータは、微分ブロック３１０によって微分されることにより速度データに変換され、さらに増幅器３１１により所定のゲインＫｆｆ（以下、「速度ＦＦゲイン」という。）でレベル補正が行なわれた後、加算器３０９に入力される。該加算器３０９では速度ＦＦゲインＫｆｆで出力される速度データと、位置ＦＢゲインＫｐｐで出力される誤差データとが加算され、前記した速度制御ブロック３０１に出力される。   Further, in the position control block 300, position command data for each joint of the articulated robot 1 is converted into velocity data by being differentiated by the differentiation block 310, and further, a predetermined gain Kff (hereinafter, “ After level correction is performed by “speed FF gain”, the signal is input to the adder 309. The adder 309 adds the speed data output with the speed FF gain Kff and the error data output with the position FB gain Kpp, and outputs the result to the speed control block 301 described above.

以上のとおり、本実施形態の制御システムでは、多関節型ロボット１の各関節に対する位置指令に対して各サーボモータ１３ａ〜１３ｆの動作をフィードバック（ＦＢ）制御することにより、予め教示された教示データの再生が正確に行われるようになっている。   As described above, in the control system of the present embodiment, teaching data previously taught by performing feedback (FB) control of the operation of each servo motor 13a to 13f in response to a position command for each joint of the multi-joint robot 1 is provided. Is played correctly.

以下、図４を参照しつつ、このプログラムの構成について説明する。このプログラムは、多関節型ロボット１のトーチ１１（ツール）の位置及び姿勢を直角手動制御によって制御するためのものである。   The configuration of this program will be described below with reference to FIG. This program is for controlling the position and posture of the torch 11 (tool) of the articulated robot 1 by right-angle manual control.

前述したＲＯＭ３３（図２参照）には、多関節型ロボット１の動作制御を実行するプログラム、そのプログラムの実行条件、及び制御定数等が格納される。このプログラムは、図４に示すように、手動運転命令生成部５１、開始点座標生成部５２、補間点生成部５３、関節補間点生成部５４、及びサーボ出力部５５を備えており、本プログラムに従ってＣＰＵ３１にて演算処理が行われる。また、ＲＡＭ３２は、ＣＰＵ３１のワーキングエリアとして用いられ、図４に示す動作命令バッファ５１ａ、軌道バッファ５２ａ、補間点バッファ５３ａ、及び関節補間点バッファ５４ａの各機能が実現される。これら各バッファは、ＦＩＦＯ（first-in first-out）バッファとして構成されており、先入れ及び先出しでデータが処理され、上記プログラムの各部５１〜５５で生成するデータが一時的に格納されるようになっている。   The ROM 33 (see FIG. 2) described above stores a program for executing operation control of the articulated robot 1, execution conditions for the program, control constants, and the like. As shown in FIG. 4, this program includes a manual operation command generation unit 51, a start point coordinate generation unit 52, an interpolation point generation unit 53, a joint interpolation point generation unit 54, and a servo output unit 55. Accordingly, the CPU 31 performs arithmetic processing. The RAM 32 is used as a working area of the CPU 31, and realizes the functions of the operation command buffer 51a, the trajectory buffer 52a, the interpolation point buffer 53a, and the joint interpolation point buffer 54a shown in FIG. Each of these buffers is configured as a first-in first-out (FIFO) buffer so that data is processed by first-in and first-out, and data generated by the units 51 to 55 of the program is temporarily stored. It has become.

以下、図５〜図９を参照しつつ、本実施形態の多関節型ロボット装置の制御システムを使用し、産業用ロボットの位置及び姿勢を直角手動制御によって制御する方法について説明する。   Hereinafter, a method of controlling the position and posture of an industrial robot by right-angle manual control using the control system of the articulated robot apparatus of the present embodiment will be described with reference to FIGS.

図５を参照して、先ず、多関節型ロボット１の動作制御を実行するプログラムにおける手動運転命令生成部５１（図４参照）が、ＴＰ３６ａからの手動制御のための操作（以下、「手動制御操作」という。）の有無を待機する（ステップＳ０）。この手動制御操作が実行されない場合は、この待機の状態を継続する。一方、手動制御操作を実行したことが手動運転命令生成部５１によって検知されると、当該手動運転命令生成部５１において、前記検知を契機として多関節型ロボット１の手動運転命令が生成されるとともに、該手動運転命令が動作命令バッファ５１ａに格納される（ステップＳ１）。詳述すると、ステップＳ０にて、手動運転命令生成部５１において、ＴＰ３６ａの１２個の軸キーの内の少なくとも１個が操作されたこと、または、操作中の前記軸キーの数や種類が変更されたことが検知されると、ステップＳ１にて、前記手動運転命令生成部５１において、新たな手動運転命令が生成される。この手動運転命令には、表１に示したトーチ１１の並進動作及び回転動作の基準座標系のパラメータ変数（CoorPar及びCoorRot）、当該並進動作及び回転動作の各動作の速度［mm/sec，rad/sec］のパラメータ変数（ManuParaVel及びManuRotVel）、及び当該並進動作及び回転動作の各動作における２つの動作方向、即ち、正（＋）方向及び負（−）方向のパラメータ変数（AxesKey_Xpuls及びAxesKey_XMinus等）、から構成される動作指示データが含まれる。   Referring to FIG. 5, first, a manual operation command generation unit 51 (see FIG. 4) in a program for executing the operation control of the articulated robot 1 performs an operation for manual control from the TP 36 a (hereinafter referred to as “manual control”). It waits for the presence or absence of "operation" (step S0). When this manual control operation is not executed, this standby state is continued. On the other hand, when the manual operation command generation unit 51 detects that a manual control operation has been performed, the manual operation command generation unit 51 generates a manual operation command for the articulated robot 1 triggered by the detection. The manual operation command is stored in the operation command buffer 51a (step S1). More specifically, in step S0, at least one of the twelve axis keys of TP36a has been operated in the manual operation command generation unit 51, or the number and type of the axis keys being operated are changed. When it is detected that this has been done, a new manual operation command is generated in the manual operation command generation unit 51 in step S1. The manual operation command includes parameter variables (CoorPar and CoorRot) in the reference coordinate system of the translation operation and rotation operation of the torch 11 shown in Table 1, and the speed [mm / sec, rad] of each operation of the translation operation and rotation operation. / sec] parameter variables (ManuParaVel and ManuRotVel) and parameter variables (AxesKey_Xpuls, AxesKey_XMinus, etc.) in two movement directions in the translation operation and the rotation operation, that is, positive (+) direction and negative (−) direction Are included.

本実施形態では、ＴＰ３６ａを用い、パラメータ変数CoorParaに２を入力し（即ち、並進動作の基準座標系をベース座標系とする。）、パラメータ変数CoorRotに４を入力する（即ち、回転動作の基準座標系をツール座標系とする。）。また、パラメータ変数ManuParaVel及びManuRotVelには、ぞれぞれ所定値［mm/sec，rad/sec］を入力するとともに、軸キーを操作し、AxesKey_Xpuls及びAxesKey_XMinus等にも、所要の動作方向に応じた数値が入力されるようにする。   In the present embodiment, TP36a is used, 2 is input to the parameter variable CoorPara (that is, the reference coordinate system of the translation operation is used as the base coordinate system), and 4 is input to the parameter variable CoorRot (that is, the reference of the rotation operation). The coordinate system is the tool coordinate system.) In addition, input predetermined values [mm / sec, rad / sec] to the parameter variables ManuParaVel and ManuRotVel, respectively, operate the axis keys, and AxesKey_Xpuls, AxesKey_XMinus, etc. according to the required operation direction Allow numeric values to be entered.

ここで、並進動作の座標系は、図６（ａ）に示すように、ベース座標系を基準座標系に用いて表現され（同図では並進動作の移動距離＝Ｄ［mm］とする。）、回転動作の座標系は、図６（ｂ）に示すように、ツール座標系を基準座標系に用いて表現される（同図では回転動作の回転角度＝α［°］とする。）。このように、本実施形態では、並進動作及び回転動作の座標系はそれぞれ個別の基準座標系を用いて表現されるが、後述する同次変換行列を使用すれば、図７に示すように、並進動作及び回転動作の座標系は、まるで同一の基準座標系を用いる場合のように、統一して表現されるようになる。   Here, as shown in FIG. 6A, the coordinate system of the translation operation is expressed using the base coordinate system as the reference coordinate system (in the same figure, the translational movement distance = D [mm]). As shown in FIG. 6B, the coordinate system of the rotation operation is expressed using the tool coordinate system as the reference coordinate system (in the figure, the rotation angle of the rotation operation is α [°]). As described above, in this embodiment, the coordinate systems of the translation operation and the rotation operation are expressed using individual reference coordinate systems, respectively, but if a homogeneous transformation matrix described later is used, as shown in FIG. The coordinate system for the translation operation and the rotation operation is expressed in a unified manner as if the same reference coordinate system is used.

図５に戻り、次に、開始点座標生成部５２（図４参照）において、動作命令バッファ５１ａから手動運転命令が読み出され、この手動運転命令に基づいて、トーチ１１の手動制御の開始点座標（開始点の位置及び姿勢に対応する。）が三次元空間の直交座標上で計算（生成）されるとともに該開始点座標及び手動運転命令に関するデータ（以下、「軌道データ」という。）が軌道バッファ５２ａに格納される（ステップＳ２）。このステップＳ２では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作に対して、それぞれ個別に基準座標系を用いて開始点座標が計算（生成）される。詳述すると、本実施形態では、前述したように、並進動作（位置に対応）、回転動作（姿勢に対応）の各動作に対して、それぞれベース座標系、ツール座標系を基準座標系に用いるので、トーチ１１の並進動作及び回転動作の各動作用にそれぞれ個別に計２個の開始点座標が計算（生成）される。尚、本実施形態では、前記開始点座標の目標座標系（基準座標系によって表現する目標（目的）となる座標系）については、並進動作及び回転動作共に、トーチ１１が実際（最終的）に動作する座標系であるツール座標系を用いて表現する。   Returning to FIG. 5, next, in the start point coordinate generation unit 52 (see FIG. 4), the manual operation command is read from the operation command buffer 51 a, and the start point of the manual control of the torch 11 based on the manual operation command. Coordinates (corresponding to the position and orientation of the start point) are calculated (generated) on the orthogonal coordinates in the three-dimensional space, and data about the start point coordinates and the manual operation command (hereinafter referred to as “trajectory data”). It is stored in the trajectory buffer 52a (step S2). In this step S2, start point coordinates are calculated (generated) using the reference coordinate system individually for each of the translation operation and the rotation operation of the torch 11 of the articulated robot 1. More specifically, in this embodiment, as described above, the base coordinate system and the tool coordinate system are used as the reference coordinate system for the translation operation (corresponding to the position) and the rotation operation (corresponding to the posture), respectively. Therefore, a total of two start point coordinates are calculated (generated) for each of the translation operation and the rotation operation of the torch 11. In the present embodiment, for the target coordinate system of the start point coordinates (the coordinate system serving as the target (object) expressed by the reference coordinate system), the torch 11 is actually (finally) used for both the translation operation and the rotation operation. This is expressed using the tool coordinate system, which is the coordinate system that operates.

このように、本実施形態では、並進動作、回転動作の各動作に対して、それぞれベース座標系、ツール座標系を基準座標系に用いる一方、両動作共にツール座標系を目標座標系に用いてその位置及び姿勢を表現する。このため、前記開始点座標は、トーチ１１の並進動作については、ベース座標系から見たツール座標系の位置が表現され、トーチ１１の回転動作については、ツール座標系から見たツール座標系の姿勢が表現される。後者のように回転動作の目標座標系及び基準座標系が共にツール座標系の場合は、ツール座標系から見たツール座標系の姿勢を求めることとなって意味をなさないので、ベース座標系を基準座標系とする。以上のようにして、本実施形態では、開始点座標の並進動作及び回転動作は、ベース座標系から見たツール座標系の位置及び姿勢を表す４行４列の同次変換行列として計算される。   As described above, in this embodiment, for each of the translation operation and the rotation operation, the base coordinate system and the tool coordinate system are used as the reference coordinate system, respectively, while the tool coordinate system is used as the target coordinate system for both operations. The position and orientation are expressed. For this reason, the start point coordinates represent the position of the tool coordinate system viewed from the base coordinate system for the translation operation of the torch 11, and the rotation of the torch 11 is represented by the tool coordinate system viewed from the tool coordinate system. Posture is expressed. If the target coordinate system and the reference coordinate system of the rotation operation are both tool coordinate systems as in the latter case, it is meaningless because the posture of the tool coordinate system viewed from the tool coordinate system is not meaningful. The reference coordinate system is used. As described above, in this embodiment, the translation operation and rotation operation of the start point coordinates are calculated as a 4-by-4 homogeneous transformation matrix representing the position and orientation of the tool coordinate system as viewed from the base coordinate system. .

ここで、前記同次変換行列の計算方法について説明する。下表２に、本計算で用いる同次変換行列を一覧して示す。表２に示す同次変換行列は、一の基準座標系から見た隣接する他の座標系（目標座標系）の位置及び姿勢を表現するものであり、多関節型ロボット１及びポジショナ２のアームの寸法、各関節の関節角度、設置（据付）位置姿勢等のリンクパラメータを用いて従来周知の方法で計算される。   Here, a method for calculating the homogeneous transformation matrix will be described. Table 2 below shows a list of homogeneous transformation matrices used in this calculation. The homogeneous transformation matrix shown in Table 2 expresses the position and orientation of another adjacent coordinate system (target coordinate system) viewed from one reference coordinate system, and the arms of the articulated robot 1 and the positioner 2 Are calculated by a conventionally well-known method using link parameters such as the dimensions of each joint, the joint angle of each joint, and the installation (installation) position and orientation.

Figure 0005144035
注）上表２において、「時不変」とは、同次変換行列の特性が時間に依存せずに不変であることをいう。
Figure 0005144035
 Note) In Table 2 above, “time invariant” means that the characteristic of the homogeneous transformation matrix is invariant without depending on time.

以下、用いる基準座標系別に、該基準座標系から見た、トーチ１１の並進動作及び回転動作の各開始点座標を表す同次変換行列の計算式を示す。ここでは、トーチ１１の並進動作の開始点座標を表す同次変換行列をMStartPara、トーチ１１の回転動作の開始点座標を表す同次変換行列をMStartRotでそれぞれ表記する。尚、各同次変換行列において、右上の添え字「−１」がある行列は、同表記の添え字のない行列に対する逆行列を示す。   Hereinafter, for each reference coordinate system to be used, a calculation formula of a homogeneous transformation matrix representing each start point coordinate of the translation operation and the rotation operation of the torch 11 viewed from the reference coordinate system is shown. Here, a homogeneous transformation matrix representing the start point coordinates of the translation operation of the torch 11 is represented as MStartPara, and a homogeneous transformation matrix representing the start point coordinates of the rotation operation of the torch 11 is represented as MStartRot. In each homogeneous transformation matrix, a matrix with the subscript “−1” in the upper right indicates an inverse matrix for a matrix without the subscript of the same notation.

１）並進動作のパラメータ変数CoorPara＝１（ワールド座標系基準）のとき、
MStartPara＝XWorld2Base×XBase2End×XEnd2Tool…式(１−１)
２）回転動作のパラメータ変数CoorRot＝１（ワールド座標系基準）のとき、
MStartRot＝XWorld2Base×XBase2End×XEnd2Tool…式(１−２)
３）並進動作のパラメータ変数CoorPara＝２（ベース座標系基準）のとき、
MStartPara＝XBase2End×XEnd2Tool…式(１−３)
４）回転動作のパラメータ変数CoorRot＝２（ベース座標系基準）のとき、
MStartRot＝XBase2End×XEnd2Tool…式(１−４)
５）並進動作のパラメータ変数CoorPara＝３（ワーク座標系基準）のとき、
MStartPara＝XPo2Work-1×XWorld2Po-1×XWorld2Base×XBase2End×XEnd2Tool…式(１−５)
６）回転動作のパラメータ変数CoorRot＝３（ワーク座標系基準）のとき、
MStartRot＝XPo2Work-1×XWorld2Po-1×XWorld2Base×XBase2End×XEnd2Tool…式(１−６)
７）並進動作のパラメータ変数CoorPara＝４（ツール座標系基準）のとき、
MStartPara ＝XBase2End×XEnd2Tool…式(１−７)
８）回転動作のパラメータ変数CoorRot＝４（ツール座標系基準）のとき、
MStartRot＝XBase2End×XEnd2Tool…式(１−８) 1) When the parameter variable CoorPara for translation is = 1 (world coordinate system reference)
MStartPara = XWorld2Base × XBase2End × XEnd2Tool Formula (1-1)
2) When the rotation operation parameter variable CoorRot = 1 (world coordinate system reference),
MStartRot = XWorld2Base x XBase2End x XEnd2Tool ... Formula (1-2)
3) When parameter variable CoorPara = 2 (base coordinate system reference) for translation,
MStartPara = XBase2End × XEnd2Tool Expression (1-3)
4) When the rotation operation parameter variable CoorRot = 2 (base coordinate system reference),
MStartRot = XBase2End × XEnd2Tool (Formula 1-4)
5) When parameter variable CoorPara = 3 (work coordinate system reference) for translation
MStartPara = XPo2Work -1 x XWorld2Po -1 x XWorld2Base x XBase2End x XEnd2Tool ... Formula (1-5)
6) When parameter variable CoorRot = 3 (work coordinate system reference)
MStartRot = XPo2Work -1 x XWorld2Po -1 x XWorld2Base x XBase2End x XEnd2Tool ... Formula (1-6)
7) When parameter variable CoorPara = 4 (tool coordinate system reference) for translation,
MStartPara = XBase2End × XEnd2Tool (formula 1-7)
8) When parameter variable CoorRot = 4 (tool coordinate system reference)
MStartRot = XBase2End × XEnd2Tool (Formula 1-8)

前述したように、本実施形態では、パラメータ変数CoorPara＝２、パラメータ変数CoorRot＝４であるので、以下の計算プロセスでは、上式(１−３)及び式(１−８)が使用される。   As described above, in the present embodiment, the parameter variable CoorPara = 2 and the parameter variable CoorRot = 4. Therefore, in the following calculation process, the above equations (1-3) and (1-8) are used.

次いで、図５を参照して、補間点生成部５３（図４参照）において、軌道バッファ５２ａから並進動作及び回転動作についての軌道データが各動作について個別に読み出される。そして、この軌道データに基づいて、トーチ１１の位置及び姿勢のそれぞれについて、補間周期（以降の計算式で「Sampling」と表記する。単位：[sec]）と呼ぶ所定の時間毎にトーチ１１が到達すべき位置及び姿勢に関するデータが補間点データとして計算されるとともに、該補間点データは、トーチ１１が前記開始点座標で表される開始点から目標点（手動制御の終了点）に至る全補間周期の間で累積計算される。こうして得られた各補間点データ及び累積移動量は、補間点バッファ５３ａに一時的に格納される（ステップＳ３）。これら累積計算の内、位置（並進動作）の累積計算について図８を参照して説明すると、開始点Ｐ０から目標点Ｐｎ間をｎ等分する各点Ｐ１〜Ｐｎ−１についてそれぞれサイクリックに補間点データが計算され、各補間点データが開始点Ｐ０から目標点Ｐｎまで累積計算される。即ち、本ステップＳ３では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作に対して、それぞれベース座標系及びツール座標系を基準座標系として補間点データ及び累積移動量が計算されることになる。具体的には、これら補間点データ及び累積移動量は、前述した直角手動制御のパラメータ変数を用いて計算される。つまり、軸キーの操作状態（AxesKey_XPuls―AxesKey_Xminus等）に対して、直角手動制御の並進動作又は回転動作の速度（ManuParaVel［mm/sec］又はManuRotVel［rad/sec］）に補間周期（Sampling[sec]）を乗じた値が、一の補間周期の間で計算される位置データ（SampParaX、並進移動量）又は姿勢データ（SampRotA、回転移動量）となる。具体的には、下に示す計算式で表される。   Next, referring to FIG. 5, in the interpolation point generation unit 53 (see FIG. 4), the trajectory data regarding the translation operation and the rotation operation is individually read out from the trajectory buffer 52 a for each operation. Then, based on this trajectory data, the torch 11 is detected at each predetermined time called an interpolation cycle (hereinafter referred to as “Sampling” in the calculation formula. Unit: [sec]) for each position and orientation of the torch 11. Data relating to the position and orientation to be reached are calculated as interpolation point data. The interpolation point data includes all the points from the start point represented by the start point coordinates to the target point (end point of manual control) by the torch 11. Cumulatively calculated during the interpolation period. Each interpolation point data and accumulated movement amount obtained in this way are temporarily stored in the interpolation point buffer 53a (step S3). Of these cumulative calculations, the cumulative calculation of the position (translational operation) will be described with reference to FIG. 8. Each point P1 to Pn-1 that equally divides the starting point P0 to the target point Pn by n is cyclically interpolated. Point data is calculated, and each interpolation point data is cumulatively calculated from the start point P0 to the target point Pn. That is, in this step S3, the interpolation point data and the accumulated movement amount are calculated using the base coordinate system and the tool coordinate system as the reference coordinate system for each of the translation operation and the rotation operation of the torch 11 of the articulated robot 1. Will be. Specifically, the interpolation point data and the accumulated movement amount are calculated by using the parameter variable of the above-described right-angle manual control. In other words, for the operation state of the axis key (AxesKey_XPuls-AxesKey_Xminus, etc.), the interpolation cycle (Sampling [sec. [Sampling [sec / ManuParaVel [mm / sec]) ]) Is position data (SampParaX, translational movement amount) or attitude data (SampRotA, rotational movement amount) calculated during one interpolation cycle. Specifically, it is represented by the following calculation formula.

SampParaX＝(AxesKey_XPuls―AxesKey_XMinus)×ManuParaVel×Sampling…式(２−１)
SampParaY＝(AxesKey_Ypuls―AxesKey_YMinus)×ManuParaVel×Sampling…式(２−２)
SampParaZ＝(AxesKey_Zpuls―AxesKey_ZMinus)×ManuParaVel×Sampling…式(２−３)
SampRotA＝(AxesKey_Apuls―AxesKey_AMinus)×ManuRotVel×Sampling…式(２−４)
SampRotB＝(AxesKey_Bpuls―AxesKey_BMinus)×ManuRotVel×Sampling…式(２−５)
SampRotC＝(AxesKey_Cpuls―AxesKey_CMinus)×ManuRotVel×Sampling…式(２−６)
ここで、
SampParaX：一の補間周期の間におけるトーチ１１のX軸方向の並進移動量[mm]
SampParaY：一の補間周期の間におけるトーチ１１のY軸方向の並進移動量[mm]
SampParaZ：一の補間周期の間におけるトーチ１１のZ軸方向の並進移動量[mm]
SampRotA：一の補間周期の間におけるトーチ１１のX軸周りの回転移動量[rad]
SampRotB：一の補間周期の間におけるトーチ１１のY軸周りの回転移動量[rad]
SampRotC：一の補間周期の間におけるトーチ１１のZ軸周りの回転移動量[rad] SampParaX = (AxesKey_XPuls−AxesKey_XMinus) × ManuParaVel × Sampling (2)
SampParaY = (AxesKey_Ypuls−AxesKey_YMinus) × ManuParaVel × Sampling… Formula (2-2)
SampParaZ = (AxesKey_Zpuls−AxesKey_ZMinus) × ManuParaVel × Sampling… Formula (2-3)
SampRotA = (AxesKey_Apuls−AxesKey_AMinus) × ManuRotVel × Sampling ... Formula (2-4)
SampRotB = (AxesKey_Bpuls−AxesKey_BMinus) × ManuRotVel × Sampling (2-5)
SampRotC = (AxesKey_Cpuls−AxesKey_CMinus) × ManuRotVel × Sampling (Formula 2-6)
here,
SampParaX: Translational amount of torch 11 in the X-axis direction during one interpolation period [mm]
SampParaY: Translational movement amount of the torch 11 in the Y-axis direction during one interpolation period [mm]
SampParaZ: Translational amount of the torch 11 in the Z-axis direction during one interpolation period [mm]
SampRotA: Amount of rotational movement about the X axis of the torch 11 during one interpolation period [rad]
SampRotB: The amount of rotational movement about the Y axis of the torch 11 during one interpolation period [rad]
SampRotC: The amount of rotational movement of the torch 11 around the Z axis during one interpolation period [rad]

さらに、トーチ１１の並進動作の累積移動量（累積並進移動量：TotalParaX等）及びトーチ１１の回転動作の累積移動量（累積回転移動量：TotalRotA等）は、下に示す計算式で表される。
TotalParaX＝TotalParaX +SampParaX…式(３−１)
TotalParaY＝TotalParaY +SampParaY…式(３−２)
TotalParaZ＝TotalParaZ +SampParaZ…式(３−３)
TotalRotA＝TotalRotA +SampRotA…式(３−４)
TotalRotB＝TotalRotB +SampRotB…式(３−５)
TotalRotC＝TotalRotC +SampRotC…式(３−６)
ここで、
TotalParaX: 手動制御スタート後の基準座標系のＸ軸方向の累積並進移動量[mm]
TotalParaY: 手動制御スタート後の基準座標系のＹ軸方向の累積並進移動量[mm]
TotalParaZ: 手動制御スタート後の基準座標系のＺ軸方向の累積並進移動量[mm]
TotalRotA: 手動制御スタート後の基準座標系のＸ軸周りの累積回転移動量[rad]
TotalRotB: 手動制御スタート後の基準座標系のＹ軸周りの累積回転移動量[rad]
TotalRotC: 手動制御スタート後の基準座標系のＺ軸周りの累積回転移動量[rad] Further, the cumulative movement amount of the translation operation of the torch 11 (cumulative translation movement amount: TotalParaX, etc.) and the cumulative movement amount of the rotation operation of the torch 11 (cumulative rotation movement amount: TotalRotA, etc.) are expressed by the following calculation formulas. .
TotalParaX = TotalParaX + SampParaX ... Formula (3-1)
TotalParaY = TotalParaY + SampParaY ... Formula (3-2)
TotalParaZ = TotalParaZ + SampParaZ ... Formula (3-3)
TotalRotA = TotalRotA + SampRotA ... Formula (3-4)
TotalRotB = TotalRotB + SampRotB ... Formula (3-5)
TotalRotC = TotalRotC + SampRotC ... Formula (3-6)
here,
TotalParaX: Cumulative translation amount in the X-axis direction of the reference coordinate system after starting manual control [mm]
TotalParaY: Cumulative translation amount in the Y-axis direction of the reference coordinate system after starting manual control [mm]
TotalParaZ: Cumulative translation amount in the Z-axis direction of the reference coordinate system after starting manual control [mm]
TotalRotA: Cumulative rotational movement around the X axis of the reference coordinate system after starting manual control [rad]
TotalRotB: Cumulative rotational movement around the Y axis of the reference coordinate system after manual control starts [rad]
TotalRotC: Cumulative rotational movement around the Z axis of the reference coordinate system after manual control start [rad]

尚、各累積移動量TotalParaX，TotalParaY，TotalParaZ，TotalRotA，TotalRotB，及びTotalRotCは、先ず、手動運転命令生成部５１において、作業者がＴＰ３６ａに対して手動制御操作を実行したことが検知され、さらに開始点座標生成部５２において、動作命令バッファ５１ａから手動運転命令が読み出された後、最初に補間点生成部５３において、補間点データが計算される直前にそれぞれ初期の累積移動量０［mm，rad］にリセットされる。   The cumulative movement amounts TotalParaX, TotalParaY, TotalParaZ, TotalRotA, TotalRotB, and TotalRotC are first detected by the manual operation command generation unit 51 that the operator has performed a manual control operation on the TP 36a, and further started. After the manual operation command is read from the operation command buffer 51a in the point coordinate generation unit 52, the initial cumulative movement amount 0 [mm, respectively] immediately before the interpolation point data is calculated in the interpolation point generation unit 53 first. rad].

続いて、図５を参照して、関節補間点生成部５４（図４参照）において、上述したように計算され、直交座標系で表現された位置及び姿勢にそれぞれ対応する累積移動量が、多関節型ロボット１の各関節の関節角度に逆変換される。これにより、前記関節補間点生成部５４において、トーチ１１を開始点座標で表される開始点から目標点に到達させるためのサーボドライバ３８ａ，…への位置指令（多関節型ロボット１の各関節に対する位置指令）が計算される。この位置指令は、関節補間点バッファ５４ａに一時的に格納され、その後、サーボ出力部５５によって関節補間点バッファ５４ａから読み出され、サーボドライバ３８ａ（図２参照）に送信される（ステップＳ４）。   Subsequently, referring to FIG. 5, the joint interpolation point generation unit 54 (see FIG. 4) calculates the accumulated movement amount corresponding to each of the position and posture calculated as described above and expressed in the orthogonal coordinate system. The joint angle of each joint of the articulated robot 1 is inversely converted. Thus, in the joint interpolation point generation unit 54, a position command (each joint of the multi-joint robot 1) to the servo drivers 38a,... For causing the torch 11 to reach the target point from the start point represented by the start point coordinates. Position command) is calculated. This position command is temporarily stored in the joint interpolation point buffer 54a, and then read out from the joint interpolation point buffer 54a by the servo output unit 55 and transmitted to the servo driver 38a (see FIG. 2) (step S4). .

前記ステップＳ４について詳述すると、該ステップＳ４は、３つ（複数）の小ステップＳ４１，Ｓ４２，及びＳ４３によって構成される。各小ステップＳ４１，Ｓ４２，及びＳ４３について、図５を参照して説明すると、先ず、関節補間点生成部５４において、トーチ１１の位置（並進動作）及び姿勢（回転動作）の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列が計算される（小ステップＳ４１）。   The step S4 will be described in detail. The step S4 includes three (plural) small steps S41, S42, and S43. Each small step S41, S42, and S43 will be described with reference to FIG. 5. First, in the joint interpolation point generation unit 54, the accumulated movement amount of the position (translation operation) and posture (rotation operation) of the torch 11 is respectively determined. Corresponding equivalent parallel progression and equivalent rotation matrix are calculated (small step S41).

具体的には、上述したように計算した累積並進移動量及び累積回転移動量に基づいて、トーチ１１の並進動作と回転動作の各動作について個別に等価並進行列及び等価回転行列を計算する。即ち、先ず、等価並進行列MatXYZについては、累積並進移動量を用いて以下の４次元ベクトルの形で定義する。

Figure 0005144035
Specifically, an equivalent translation sequence and an equivalent rotation matrix are calculated individually for each of the translation operation and the rotation operation of the torch 11 based on the accumulated translation amount and the accumulated rotation amount calculated as described above. That is, first, the equivalent translation sequence MatXYZ is defined in the form of the following four-dimensional vector using the accumulated translational movement amount.
Figure 0005144035

また、等価回転行列MatRPYについては、累積回転移動量を用いて以下の４次元ベクトルの形で定義する。尚、剛体の回転動作（姿勢）には多種の表現が提案されているが、ここでは、多関節型ロボットの制御分野において一般的に使用されるロール・ピッチ・ヨー角φ，θ，ψによる表現を使用する。   The equivalent rotation matrix MatRPY is defined in the form of the following four-dimensional vector using the accumulated rotational movement amount. Various expressions have been proposed for the rotational motion (posture) of the rigid body, but here, it is based on roll pitch pitch yaw angles φ, θ, and ψ that are generally used in the art of articulated robot control. Use expressions.

Figure 0005144035
Figure 0005144035

次に、前述したように並進動作及び回転動作について個別に計算された各開始点座標と、上式（４−１）及び（４−４）によってそれぞれ計算された等価並進行列及び等価回転行列とに基づいて、関節補間点生成部５４において、各開始点座標で表される開始点からトーチ１１の目標点に至る目標並進行列及び目標回転行列がそれぞれ計算される（小ステップＳ４２）。   Next, as described above, the starting point coordinates calculated separately for the translation operation and the rotation operation, and the equivalent translation sequence and the equivalent rotation matrix calculated by the above equations (4-1) and (4-4), respectively, Based on the above, the joint interpolation point generation unit 54 calculates the target parallel progression and the target rotation matrix from the start point represented by the respective start point coordinates to the target point of the torch 11 (small step S42).

具体的には、目標並進行列MTargetParaについては、ＴＰ３６ａに入力されたパラメータ変数（CoorParａ）が１（ワールド座標系）、２（ベース座標系）、又は３（ワーク座標系）のいずれかの場合には、式（４−１）で示されるMatXYZを、並進動作の開始点座標を表す同次変換行列MStartParaに対して左からかけることで計算する。このように等価並進行列MatXYZを同次変換行列MStartParaに対して左からかけることは、並進動作をワールド、ベース、ワーク座標系のいずれか一の基準座標系に関して行うことに相当する。本実施形態では、前述したように、パラメータ変数CoorParａ＝２であって、並進動作の基準座標系はベース座標系であるので、MatXYZはベース座標系の並進動作として計算される。このように計算すると、作業者が直接的に並進動作を認知できるベース座標系において所望の並進動作が実現され、トーチ１１の操作が容易化されるため好ましい。この場合、目標並進行列MTargetParaは、関節補間点生成部５４において、下式(４−２)によって計算される。
MTargetPara＝MatXYZ×MStartPara…式(４−２) Specifically, for the target parallel progression MTargetPara, when the parameter variable (CoorPara) input to the TP 36a is 1 (world coordinate system), 2 (base coordinate system), or 3 (work coordinate system). Is calculated by multiplying MatXYZ represented by Expression (4-1) from the left with respect to the homogeneous transformation matrix MStartPara representing the coordinate of the start point of the translation operation. Applying the equivalent translation sequence MatXYZ to the homogeneous transformation matrix MStartPara from the left in this way corresponds to performing the translation operation with respect to any one of the reference coordinate system of the world, base, and workpiece coordinate systems. In the present embodiment, as described above, since the parameter variable CoorPara = 2 and the reference coordinate system of the translation operation is the base coordinate system, MatXYZ is calculated as the translation operation of the base coordinate system. This calculation is preferable because a desired translation operation is realized in the base coordinate system in which the operator can directly recognize the translation operation, and the operation of the torch 11 is facilitated. In this case, the target parallel progression sequence MTargetPara is calculated by the following equation (4-2) in the joint interpolation point generation unit 54.
MTargetPara = MatXYZ × MStartPara (Formula 4-2)

これに対して、ＴＰ３６ａに入力されたパラメータ変数（CoorPar）が４（ツール座標系）の場合には、式（４−１）で示されるMatXYZを、並進動作の開始点座標を表す同次変換行列MStartParaに対して右からかけることで目標並進行列MTargetParaを計算する。このように等価並進行列MatXYZを同次変換行列MStartParaに対して右からかけることは、並進動作を目標座標系に関して行うことに相当する。即ち、この場合は、目標並進行列MTargetParaは、関節補間点生成部５４において、下式(４−３)によって計算される。
MTargetPara＝MStartPara×MatXYZ…式(４−３) On the other hand, when the parameter variable (CoorPar) input to the TP 36a is 4 (tool coordinate system), the MatXYZ expressed by the equation (4-1) is converted to the homogeneous transformation representing the start point coordinates of the translation operation. The target parallel progression MTargetPara is calculated by multiplying the matrix MStartPara from the right. Applying the equivalent translation sequence MatXYZ to the homogeneous transformation matrix MStartPara from the right in this way corresponds to performing the translation operation with respect to the target coordinate system. That is, in this case, the target parallel progression sequence MTargetPara is calculated by the joint interpolation point generation unit 54 according to the following equation (4-3).
MTargetPara = MStartPara × MatXYZ Formula (4-3)

一方、目標回転行列MTargetRotについては、ＴＰ３６ａに入力されたパラメータ変数（CoorRot）が１（ワールド座標系）、２（ベース座標系）、又は３（ワーク座標系）のいずれかの場合には、式（４−４）で示されるMatRPYを、回転動作の開始点座標を表す同次変換行列MStartRotに対して左からかけることで計算する。このように等価回転行列MatRPYを同次変換行列MStartRotに対して左からかけることは、回転動作をワールド、ベース、ワーク座標系のいずれか一の基準座標系に関して行うことに相当する。即ち、この場合、目標回転行列MTargetRotは、関節補間点生成部５４において、下式(４−５)によって計算される。
MTargetRot＝MatRPY×MStartRot…式(４−５) On the other hand, for the target rotation matrix MTargetRot, if the parameter variable (CoorRot) input to the TP 36a is 1 (world coordinate system), 2 (base coordinate system), or 3 (work coordinate system), the equation The MatRPY shown by (4-4) is calculated by multiplying the homogeneous transformation matrix MStartRot representing the starting point coordinates of the rotation operation from the left. Applying the equivalent rotation matrix MatRPY to the homogeneous transformation matrix MStartRot from the left in this way corresponds to performing the rotation operation on any one of the reference coordinate system of the world, base, and work coordinate systems. That is, in this case, the target rotation matrix MTargetRot is calculated in the joint interpolation point generation unit 54 by the following equation (4-5).
MTargetRot = MatRPY × MStartRot ... (4-5)

これに対して、ＴＰ３６ａに入力されたパラメータ変数（CoorRot）が４（ツール座標系）の場合には、式（４−４）で示されるMatRPYを、回転動作の開始点座標を表す同次変換行列MStartRotに対して右からかけることで目標回転行列MTargetRotを計算する。このように等価回転行列MatRPYを同次変換行列MStartRotに対して右からかけることは、回転動作を目標座標系に関して行うことに相当する。本実施形態では、前述したように、回転動作の目標座標系はツール座標系であるので、MatRPYはツール座標系の回転動作として計算される。このように計算すると、作業者が直接的に回転動作を認知できるツール座標系において所望の回転動作が実現され、トーチ１１の操作が容易化されるため好ましい。即ち、この場合は、目標回転行列MTargetRotは、関節補間点生成部５４において、下式(４−６)によって計算される。
MTargetRot＝MStartRot×MatRPY…式(４−６) On the other hand, when the parameter variable (CoorRot) input to the TP 36a is 4 (tool coordinate system), the MatRPY expressed by the equation (4-4) is converted to the homogeneous conversion representing the start point coordinates of the rotation operation. The target rotation matrix MTargetRot is calculated by multiplying the matrix MStartRot from the right. Applying the equivalent rotation matrix MatRPY to the homogeneous transformation matrix MStartRot from the right in this way corresponds to performing the rotation operation with respect to the target coordinate system. In the present embodiment, as described above, since the target coordinate system of the rotation operation is the tool coordinate system, MatRPY is calculated as the rotation operation of the tool coordinate system. This calculation is preferable because a desired rotation operation is realized in the tool coordinate system in which the operator can directly recognize the rotation operation, and the operation of the torch 11 is facilitated. That is, in this case, the target rotation matrix MTargetRot is calculated by the joint interpolation point generation unit 54 by the following equation (4-6).
MTargetRot = MStartRot × MatRPY ... Formula (4-6)

次いで、以上のようにして計算された目標並進行列及び目標回転行列に基づいて、関節補間点生成部５４において、多関節型ロボット１の各関節の関節角度が計算され、関節補間点バッファ５４ａに一時的に格納される（小ステップＳ４３）。   Next, based on the target parallel progression and target rotation matrix calculated as described above, the joint interpolation point generation unit 54 calculates the joint angle of each joint of the articulated robot 1 and stores it in the joint interpolation point buffer 54a. Temporarily stored (small step S43).

一般に、トーチ１１の位置及び姿勢を前記した目標並進行列及び目標回転行列で示される座標に一致させるには、多関節型ロボットの各関節を操作することが必要になる。本実施形態では、多関節型ロボット１の各関節の関節角度を決定するために、多関節型ロボット１のベース座標系から見たエンドエフェクタ座標系の位置及び姿勢をそれぞれ表す同次変換行列TargetParaXBase2End及びTargetRotXBase2Endを、それぞれ表２に示す同次変換行列を用いて目標並進行列MTargetPara及び目標回転行列MTargetRotから抽出する方法を用いる。この方法を実行する計算式について、以下、並進動作及び回転動作の各動作について基準座標系別に示す。尚、各同次変換行列において、右上の添え字「−１」がある行列は、同表記の添え字のない行列に対する逆行列を示す。   Generally, in order to make the position and orientation of the torch 11 coincide with the coordinates indicated by the target parallel progression row and the target rotation matrix, it is necessary to operate each joint of the articulated robot. In the present embodiment, in order to determine the joint angle of each joint of the articulated robot 1, a homogeneous transformation matrix TargetParaXBase2End representing the position and orientation of the end effector coordinate system viewed from the base coordinate system of the articulated robot 1 respectively. And TargetRotXBase2End are extracted from the target parallel progression sequence MTargetPara and the target rotation matrix MTargetRot using the homogeneous transformation matrix shown in Table 2, respectively. The calculation formulas for executing this method will be described below for each reference coordinate system for each of the translation operation and the rotation operation. In each homogeneous transformation matrix, a matrix with the subscript “−1” in the upper right indicates an inverse matrix for a matrix without the subscript of the same notation.

１）並進動作のパラメータ変数CoorPar＝１（ワールド座標系基準）のとき、
TargetPaｒaXBase2End＝XWorld2Base-1×MTargetPara×XEnd2Tool-1…式(５−１)
２）回転動作のパラメータ変数CoorRot＝１（ワールド座標系基準）のとき、
TargetRotXBase2End＝XWorld2Base-1×MTargetRot×XEnd2Tool-1…式(５−２)
３）並進動作のパラメータ変数CoorPar＝２（ベース座標系基準）のとき、
TargetPaｒaXBase2End＝MTargetPara×XEnd2Tool―1…式(５−３)
４）回転動作のパラメータ変数CoorRot＝２（ベース座標系基準）のとき、
TargetRotXBase2End＝MTargetRot×XEnd2Tool-1…式(５−４)
５）並進動作のパラメータ変数CoorPar＝３（ワーク座標系基準）のとき、
TargetPaｒaXBase2End＝XWorld2Base-1×XWorld2Po×XPo2Work×MTargetPara×XEnd2Tool-1・・・式(５−５)
６）回転動作のパラメータ変数CoorRot＝３（ワーク座標系基準）のとき、
TargetRotXBase2End＝XWorld2Base-1×XWorld2Po×XPo2Work×MTargetRot×XEnd2Tool-1・・・式(５−６)
７）並進動作のパラメータ変数CoorPar＝４（ツール座標系基準）のとき、
TargetPaｒaXBase2End＝MTargetPara×XEnd2Tool-1…式(５−７)
８）回転動作のパラメータ変数CoorRot＝４（ツール座標系基準）のとき、
TargetRotXBase2End＝MTargetRot×XEnd2Tool-1…式(５−８) 1) When the translation operation parameter variable CoorPar = 1 (world coordinate system reference),
TargetParaXBase2End = XWorld2Base −1 × MTargetPara × XEnd2Tool −1 Equation (5-1)
2) When the rotation operation parameter variable CoorRot = 1 (world coordinate system reference),
TargetRotXBase2End = XWorld2Base -1 x MTargetRot x XEnd2Tool -1 ... Formula (5-2)
3) When parameter variable CoorPar = 2 (base coordinate system reference) for translation operation,
TargetParaXBase2End = MTargetPara x XEnd2Tool- 1 ... Formula (5-3)
4) When the rotation operation parameter variable CoorRot = 2 (base coordinate system reference),
TargetRotXBase2End = MTargetRot x XEnd2Tool -1 ... Formula (5-4)
5) When parameter variable CoorPar = 3 (work coordinate system reference) for translation operation,
TargetParaXBase2End = XWorld2Base -1 x XWorld2Po x XPo2Work x MTargetPara x XEnd2Tool -1 Formula (5-5)
6) When parameter variable CoorRot = 3 (work coordinate system reference)
TargetRotXBase2End = XWorld2Base -1 x XWorld2Po x XPo2Work x MTargetRot x XEnd2Tool -1 Formula (5-6)
7) When parameter variable CoorPar = 4 (tool coordinate system reference) for translation,
TargetParaXBase2End = MTargetPara × XEnd2Tool −1 ... Formula (5-7)
8) When parameter variable CoorRot = 4 (tool coordinate system reference)
TargetRotXBase2End = MTargetRot x XEnd2Tool -1 ... Formula (5-8)

並進動作及び回転動作の各動作について、それぞれ個別の座標系を用いて上記のように計算されたTargetParaXBase2End及びTargetRotXBase2Endは、それぞれ、多関節型ロボットのベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列である。したがって、同次変換行列TargetParaXBase2Endから位置成分を、同次変換行列TargetRotXBase2Endから姿勢成分を取り出して合成して得られる同次変換行列TargetMixXBase2Endが、トーチ１１の位置及び姿勢を共に所望の状態（目標点の状態）とする同次変換行列となる。ここで、上述したように計算されたTargetParaXBase2End及びTargetRotXBase2Endの各同次変換行列は、それぞれ下式(７−１)及び式（７−２）で表される。   For each of the translation operation and the rotation operation, TargetParaXBase2End and TargetRotXBase2End calculated as described above using the respective coordinate systems are respectively the position of the end effector coordinate system viewed from the base coordinate system of the articulated robot, and It is a homogeneous transformation matrix representing an attitude. Therefore, the homogeneous transformation matrix TargetMixXBase2End obtained by synthesizing the position component from the homogeneous transformation matrix TargetParaXBase2End and the posture component from the homogeneous transformation matrix TargetRotXBase2End combines the position and orientation of the torch 11 in a desired state (target point State)). Here, the homogeneous transformation matrices of TargetParaXBase2End and TargetRotXBase2End calculated as described above are expressed by the following equations (7-1) and (7-2), respectively.

Figure 0005144035
Figure 0005144035

この場合、同次変換行列TargetMixXBase2Endは、並進成分と回転成分とをそれぞれ上式(７−１)及び式（７−２）から分離し、再結合する従来周知の方法によって、下式(７−３)で表される。

Figure 0005144035
In this case, the homogeneous transformation matrix TargetMixXBase2End separates the translation component and the rotation component from the above equations (7-1) and (7-2), respectively, and recombines them using the following equation (7− It is expressed by 3).
Figure 0005144035

上式（７−３）によって、多関節型ロボット１のベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列が得られる。この変換プロセスについて、図９を参照して説明すると、前述したように、本実施形態では、開始点座標は、並進動作については、ベース座標系を基準とし、回転動作については、ツール座標系を基準とする。また、開始点Ｐ０から目標点Ｐｎまでの並進移動（距離Ｄ[mm]の移動）及び回転移動（角度α［°］の移動）は、ベース座標系を基準としたエンドエフェクタ座標系の同次変換行列で表現する。即ち、並進動作については、TargetParaXBase2Endと表現し、回転動作については、TargetRotXBase2Endと表現する。さらに、目標点Ｐｎは、両同次変換行列を合成して得られる同次変換行列TargetMixXBase2Endによって、ベース座標系を基準として統一的に扱われる。   By the above equation (7-3), a homogeneous transformation matrix representing the position and orientation of the end effector coordinate system viewed from the base coordinate system of the articulated robot 1 is obtained. This conversion process will be described with reference to FIG. 9. As described above, in this embodiment, the start point coordinates are based on the base coordinate system for translation operations, and the tool coordinate system is used for rotation operations. The standard. Further, translational movement (movement of distance D [mm]) and rotational movement (movement of angle α [°]) from start point P0 to target point Pn are homogeneous in the end effector coordinate system with reference to the base coordinate system. Expressed as a transformation matrix. That is, the translation operation is expressed as TargetParaXBase2End, and the rotation operation is expressed as TargetRotXBase2End. Furthermore, the target point Pn is uniformly handled with reference to the base coordinate system by a homogeneous transformation matrix TargetMixXBase2End obtained by synthesizing both homogeneous transformation matrices.

以上のように計算された同次変換行列を、関節補間点生成部５４において、従来周知の方法で逆変換して多関節型ロボット１の各関節角度を算出することにより、当該多関節型ロボット１のトーチ１１が開始点から目標点（手動制御の終了点）に至る全補間周期で到達する関節角度が求められる。   The joint interpolation point generation unit 54 inversely transforms the homogeneous transformation matrix calculated as described above by a conventionally known method to calculate each joint angle of the articulated robot 1, so that the articulated robot 1 The joint angle that one torch 11 reaches in all interpolation cycles from the start point to the target point (end point of manual control) is obtained.

その後、図５を参照して、サーボ出力部５５（図４参照）によって、関節補間点バッファ５４ａから多関節型ロボット１の各関節角度が読み出され、該各関節角度に基づいて、サーボドライバ３８ａ（図２参照）に多関節型ロボット１の各関節及びトーチ１１の位置及び姿勢を目標の状態とさせる指令（多関節型ロボット１の各関節に対する位置指令）が生成される。その後、サーボ出力部５５から、前記位置指令をサーボドライバＩ／Ｆ３８を介して各サーボドライバ３８ａに送信することでサーボモータ１３ａ〜１３ｆを制御し、これによりトーチ１１を開始点から目標点に到達させるようにする。尚、前記位置指令は、タイマー３４（図２参照）で発生する同期信号に同期して各サーボドライバ３８ａへ送信される（ステップＳ５）。   Thereafter, referring to FIG. 5, the servo output unit 55 (see FIG. 4) reads out the joint angles of the articulated robot 1 from the joint interpolation point buffer 54a, and based on the joint angles, the servo driver is read out. A command (position command for each joint of the multi-joint robot 1) is generated in 38a (see FIG. 2) to set the positions and postures of the joints of the multi-joint robot 1 and the torch 11 to a target state. After that, the servo output unit 55 controls the servo motors 13a to 13f by transmitting the position command to each servo driver 38a via the servo driver I / F 38, whereby the torch 11 reaches the target point from the start point. I will let you. The position command is transmitted to each servo driver 38a in synchronization with a synchronization signal generated by the timer 34 (see FIG. 2) (step S5).

以上説明したように、本実施形態によれば、下記する作用・効果が奏される。
（１）本実施形態では、多関節型ロボット１の位置及び姿勢を直交座標系を用いて手動で制御するにあたり、当該多関節型ロボット１に備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行うようにしている。このため、多関節型ロボット１のトーチ１１の位置及び姿勢を直交座標系を用いて手動で制御する前に、該トーチ１１の並進動作及び回転動作の各動作に対してそれぞれ適した基準座標系を設定しておくことができる。これにより、前記手動制御の最中に、多関節型ロボット１のトーチ１１の並進動作及び回転動作の各動作にそれぞれ適した基準座標系に切り替えるという煩雑な操作を行う必要がなくなり、作業者が簡易に多関節型ロボット１のトーチ１１を手動制御することができるようになる。その結果、多関節型ロボット１へ作業プログラムを教示するために要する教示時間が低減され、多関節型ロボット１の教示作業が簡略化されるようになる。 As described above, according to the present embodiment, the following operations and effects are achieved.
(1) In this embodiment, when manually controlling the position and orientation of the articulated robot 1 using an orthogonal coordinate system, it is used for the translation operation and the rotation operation of the tool provided in the articulated robot 1. After the reference coordinate system is individually set for each operation, the translation operation and the rotation operation are manually controlled based on each reference coordinate system. Therefore, before manually controlling the position and orientation of the torch 11 of the articulated robot 1 using the orthogonal coordinate system, the reference coordinate system suitable for each of the translational operation and the rotational operation of the torch 11 is used. Can be set. Thereby, during the manual control, there is no need to perform a complicated operation of switching to a reference coordinate system suitable for each of the translation operation and the rotation operation of the torch 11 of the articulated robot 1. The torch 11 of the articulated robot 1 can be manually controlled easily. As a result, the teaching time required for teaching the work program to the articulated robot 1 is reduced, and the teaching work of the articulated robot 1 is simplified.

（２）本実施形態では、トーチ１１の並進動作の基準座標系としてベース座標系を用いるとともに、トーチ１１の回転動作の基準座標系としてツール座標系を用いている。このため、トーチ１１の並進動作については、作業者が直接的にその動作を認知できるベース座標系において所望の並進動作が実現される一方、トーチ１１の回転動作についても、作業者が直接的にその動作を認知できるツール座標系において所望の回転動作が実現されるようになり、トーチ１１の手動制御が容易化されるようになる。   (2) In this embodiment, the base coordinate system is used as the reference coordinate system for the translation operation of the torch 11, and the tool coordinate system is used as the reference coordinate system for the rotation operation of the torch 11. For this reason, with regard to the translation operation of the torch 11, a desired translation operation is realized in the base coordinate system in which the operator can directly recognize the operation. On the other hand, the operator also directly performs the rotation operation of the torch 11. A desired rotational motion is realized in the tool coordinate system that can recognize the motion, and manual control of the torch 11 is facilitated.

（３）本実施形態では、トーチ１１の並進動作の基準座標系を手動制御の状態に応じて変更したときでも、トーチ１１の回転動作の基準座標系は、トーチ１１の姿勢が変更された方向が作業者によって容易に認知できるツール座標系に固定されている。したがって、作業者に多関節型ロボット１の手動制御にあたっての混乱を生じさせることがなく、多関節型ロボット１の手動制御による教示時間を低減させることができる。   (3) In this embodiment, even when the reference coordinate system of the translation operation of the torch 11 is changed according to the state of manual control, the reference coordinate system of the rotation operation of the torch 11 is the direction in which the attitude of the torch 11 is changed. Is fixed in a tool coordinate system that can be easily recognized by the operator. Therefore, it is possible to reduce teaching time by manual control of the articulated robot 1 without causing confusion in manual control of the articulated robot 1 for the operator.

尚、上記実施形態は以下のように変形してもよい。
・上記実施形態では、多関節型ロボット１のトーチ１１の並進動作及び回転動作の基準座標系として、それぞれワールド座標系、ベース座標系、ワーク座標系、又はツール座標系のいずれか一の座標系を選択可能なようにしたが、トーチ１１の並進動作及び回転動作に対して個別に基準座標系を設定し得るようにする限り、これらの座標系に限るものではなく、多関節型ロボット１で一般に用いられるその他の座標系、例えば、エンドエフェクタ座標系やポジショナ座標系、ショップフロア座標系、又は、その他の任意座標系を選択可能としてもよい。 The above embodiment may be modified as follows.
In the above embodiment, the coordinate system of any one of the world coordinate system, the base coordinate system, the work coordinate system, and the tool coordinate system is used as the reference coordinate system for the translation operation and the rotation operation of the torch 11 of the articulated robot 1. However, as long as the reference coordinate system can be individually set for the translation operation and the rotation operation of the torch 11, the present invention is not limited to these coordinate systems. Other commonly used coordinate systems such as an end effector coordinate system, a positioner coordinate system, a shop floor coordinate system, or other arbitrary coordinate systems may be selectable.

・また、上記実施形態では、多関節型ロボット１のトーチ１１の並進動作の基準座標系にベース座標系を用い、回転動作の基準座標系にツール座標系を用いるようにした。しかし、これに限らず、例えば、並進動作の基準座標系にワールド座標系を用い、回転動作の基準座標系にツール座標系を用いるようにしてもよい。   In the above embodiment, the base coordinate system is used as the reference coordinate system for the translation operation of the torch 11 of the articulated robot 1, and the tool coordinate system is used as the reference coordinate system for the rotation operation. However, the present invention is not limited to this. For example, the world coordinate system may be used as the reference coordinate system for translation, and the tool coordinate system may be used as the reference coordinate system for rotation.

・また、上記実施形態では、直交座標系で表現された姿勢に対応する累積回転移動量を多関節型ロボット１の各関節の関節角度に逆変換するにあたり、該累積回転移動量を用いて等価回転行列を定義するためにロール・ピッチ・ヨー角による表現を用いたが、オイラー角による表現を用いることも可能である。   In the above embodiment, when the cumulative rotational movement amount corresponding to the posture expressed in the Cartesian coordinate system is inversely converted to the joint angle of each joint of the articulated robot 1, the cumulative rotational movement amount is equivalently used. In order to define the rotation matrix, the expression by roll, pitch, and yaw angle is used, but the expression by Euler angle can also be used.

・また、上記実施形態の多関節型ロボット装置の制御システムにおいては、６軸の多関節型ロボット１及び２軸のポジショナ２を備えた構成としたが、当該制御システムにおいては、ロボットによる溶接作業が容易且つ確実に行える限り、ポジショナ２は必ずしも必要ではない。また、ポジショナ２の代わりに６軸の第二の多関節型ロボットを用いることもできる。このようにポジショナ２に代えて６軸の第二の多関節型ロボットを用いる場合には、ポジショナ２に設定したワーク座標系とポジショナ座標系の代わりに、該第二の多関節型ロボットに設定したロボットワールド座標系、ベース座標系、又はツール座標系のいずれかの座標系を基準座標系又は目標座標系に用いることができる。   In the control system for the articulated robot apparatus of the above embodiment, the 6-axis articulated robot 1 and the 2-axis positioner 2 are provided. However, in the control system, welding work by the robot is performed. The positioner 2 is not necessarily required as long as the positioner 2 can be easily and reliably operated. Further, instead of the positioner 2, a six-axis second articulated robot can be used. In this way, when a 6-axis second articulated robot is used in place of the positioner 2, the work coordinate system set in the positioner 2 and the positioner coordinate system are set in the second articulated robot. Any one of the robot world coordinate system, the base coordinate system, and the tool coordinate system can be used as the reference coordinate system or the target coordinate system.

・また、上記実施形態では、多関節型ロボット１（産業用ロボット）を自動車等の溶接に用いる溶接ロボット、即ち、ツールとしてトーチ１１（溶接トーチ）を備える溶接ロボットとしたが、本発明の技術的思想は、その他の産業用ロボット、例えば、組立ロボット、塗装ロボット、検査ロボット、研摩ロボット、洗浄ロボット等にも適用することができる。   In the above embodiment, the articulated robot 1 (industrial robot) is a welding robot that is used for welding of an automobile or the like, that is, a welding robot including a torch 11 (welding torch) as a tool. The technical idea can also be applied to other industrial robots such as assembly robots, painting robots, inspection robots, polishing robots, and cleaning robots.

さらに、前記した実施形態および変形例より把握できる技術的思想について以下に記載する。   Further, technical ideas that can be grasped from the above-described embodiments and modifications will be described below.

・産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、請求項４又は請求項５に記載の各ステップを実行することを特徴とする産業用ロボットのツールの位置・姿勢制御システム。このように構成した場合、産業用ロボットの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、前記産業用ロボットに備えられたツールの並進動作と回転動作とに用いる基準座標系を各動作ごとにそれぞれ個別に設定した後、各基準座標系に基づいて並進動作及び回転動作の手動制御を行う方法を、系統だった制御システムで実現することができる。   A control system used for manually controlling the position and orientation of a tool of an industrial robot using an orthogonal coordinate system, wherein each step according to claim 4 or 5 is executed. Position / attitude control system for industrial robot tools. When configured in this way, it is a method for manually controlling the position and orientation of an industrial robot using an orthogonal coordinate system, and the reference coordinates used for the translation operation and the rotation operation of the tool provided in the industrial robot. After the system is individually set for each operation, a method for performing manual control of the translation operation and the rotation operation based on each reference coordinate system can be realized by a systematic control system.

本発明に係る多関節型ロボットの制御システムの全体図。1 is an overall view of a control system for an articulated robot according to the present invention. 本発明に係るロボット制御装置の内部構成及びその周辺装置を示すブロック図。The block diagram which shows the internal structure of the robot control apparatus which concerns on this invention, and its peripheral device. 本発明に係るサーボドライバによるサーボ制御系を示すブロック図。The block diagram which shows the servo control system by the servo driver which concerns on this invention. 本発明に係るロボット制御装置に用いる制御プログラムの構成を示す概念図。The conceptual diagram which shows the structure of the control program used for the robot control apparatus which concerns on this invention. 本発明に係るロボット制御装置に用いる制御プログラムのフローチャート図。The flowchart figure of the control program used for the robot control apparatus which concerns on this invention. 本発明に係る多関節型ロボットに用いる基準座標系及び同基準座標系を基準とした目標座標系の動作を示す説明図であり、（ａ）は、ベース座標系を基準座標系とした目標座標系の並進動作を示す図、（ｂ）は、ツール座標系を基準座標系とした目標座標系の回転動作を示す図。It is explanatory drawing which shows operation | movement of the reference coordinate system used for the articulated robot which concerns on this invention, and the target coordinate system on the basis of the same reference coordinate system, (a) is the target coordinate which used the base coordinate system as the reference coordinate system The figure which shows the translation operation | movement of a system, (b) is a figure which shows the rotation operation | movement of the target coordinate system which made the tool coordinate system the reference | standard coordinate system. 本発明に係る多関節型ロボットにおけるベース座標系、ツール座標系をそれぞれ基準座標系とした目標座標系の並進動作、回転動作を示す説明図。Explanatory drawing which shows the translation operation | movement and rotation operation | movement of the target coordinate system which made the base coordinate system and the tool coordinate system the reference coordinate system in the articulated robot which concerns on this invention, respectively. 本発明に係る多関節型ロボットにおける基準座標系を用いて目標座標系の並進動作の補間点データ及び累積移動量を計算するプロセスを示す説明図。Explanatory drawing which shows the process which calculates the interpolation point data and cumulative movement amount of translation operation | movement of a target coordinate system using the reference | standard coordinate system in the articulated robot which concerns on this invention. ベース座標系から見たエンドエフェクタ座標系の位置及び姿勢を表す同次変換行列と目標座標系（ツール座標系）との対応関係を示す説明図。Explanatory drawing which shows the correspondence of the homogeneous transformation matrix showing the position and attitude | position of an end effector coordinate system seen from the base coordinate system, and a target coordinate system (tool coordinate system).

符号の説明Explanation of symbols

１…多関節型ロボット，２…ポジショナ，３…ロボット制御装置，４…溶接制御装置，１１…トーチ。   DESCRIPTION OF SYMBOLS 1 ... Articulated robot, 2 ... Positioner, 3 ... Robot control apparatus, 4 ... Welding control apparatus, 11 ... Torch.

Claims (3)

多関節を有する産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御する方法であって、
（１）ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成ステップと、
（２）前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算ステップと、
（３）前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算ステップと、
（４）前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成ステップと、
（５）前記指令を前記産業用ロボットの各関節を動かすサーボ機構に与えることで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御ステップと、を備え、
前記開始点座標計算ステップ及び累積移動量計算ステップの各ステップにおいて、前記ロボットのツールの並進動作及び回転動作の各動作に対してそれぞれ個別に１つ、前記手動制御前に予め設定した基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたことを特徴とする産業用ロボットのツールの位置・姿勢制御方法。 A method of manually controlling the position and orientation of a tool of an industrial robot having multiple joints using an orthogonal coordinate system,
(1) a manual operation command generation step of detecting that manual control is performed on the robot work command teaching means, and generating a manual operation command to the industrial robot in response to the detection;
(2) a start point coordinate calculating step of calculating a start point coordinate of manual control of the tool based on the manual operation command;
(3) Based on the manual operation command and start point coordinates, data on the position and orientation of the tool is calculated for each predetermined interpolation period, and the position and orientation data are represented by the start point coordinates, respectively. A cumulative movement amount calculating step for accumulating and calculating a cumulative movement amount of translational motion and rotational motion by accumulating all interpolation cycles from the start point to the target point of the tool;
(4) A tool position command generation step for generating a position command for causing the tool to reach a target point from the start point by inversely converting the cumulative movement amount into a joint angle of each joint of the industrial robot;
(5) a servomechanism control step for controlling the servomechanism by giving the command to a servomechanism that moves each joint of the industrial robot, thereby causing the tool to reach the target point from the start point; With
In each of the start point coordinate calculation step and the cumulative movement amount calculation step, one individually for each of the translation operation and the rotation operation of the tool of the robot, a reference coordinate system set in advance before the manual control The position / posture control method of the industrial robot tool, wherein the start point coordinates, position and posture data, or cumulative movement amount of each operation is calculated based on the above. 請求項１に記載のツールの位置・姿勢制御方法において、前記ツール位置指令生成ステップには、
（ｉ）前記位置及び姿勢の累積移動量にそれぞれ対応する等価並進行列及び等価回転行列を計算する等価並進・回転行列計算ステップと、
（ii）前記開始点座標と、前記等価並進行列及び等価回転行列とに基づいて、前記ツールを前記開始点から目標点に到達させる目標並進行列及び目標回転行列を計算する目標並進・回転行列計算ステップと、
（iii）前記目標並進行列及び目標回転行列に基づいて前記産業用ロボットの各関節角
度を計算するロボット関節角度計算ステップと、
が含まれる産業用ロボットのツールの位置・姿勢制御方法。 The tool position / posture control method according to claim 1, wherein the tool position command generation step includes:
(I) an equivalent translation / rotation matrix calculation step for calculating an equivalent translation sequence and an equivalent rotation matrix respectively corresponding to the accumulated movement amounts of the position and orientation;
(Ii) Target translation / rotation matrix calculation for calculating a target translation sequence and target rotation matrix for causing the tool to reach a target point from the start point based on the start point coordinates and the equivalent translation sequence and equivalent rotation matrix Steps,
(Iii) a robot joint angle calculation step of calculating each joint angle of the industrial robot based on the target parallel progression and target rotation matrix;
Tool position / posture control method for industrial robots. 産業用ロボットのツールの位置及び姿勢を直交座標系を用いて手動で制御するにあたって使用される制御システムであって、
ロボット作業命令の教示手段に対して手動制御が実行されたことを検知し、当該検知を契機として前記産業用ロボットへの手動運転命令を生成する手動運転命令生成手段と、
前記手動運転命令に基づいて、前記ツールの手動制御の開始点座標を計算する開始点座標計算手段と、
前記手動運転命令及び開始点座標に基づいて、所定の補間周期毎に前記ツールの位置及び姿勢に関するデータを計算するとともに、該位置及び姿勢データを、それぞれ前記開始点座標で表される開始点からツールの目標点に至る全補間周期で累積計算して並進動作及び回転動作の累積移動量を得る累積移動量計算手段と、
前記累積移動量を、前記産業用ロボットの各関節の関節角度に逆変換することで、前記ツールを前記開始点から目標点に到達させる位置指令を生成するツール位置指令生成手段と、
前記位置指令を前記産業用ロボットの各関節を動かすサーボ機構に送信することで当該サーボ機構を制御し、これにより前記ツールを前記開始点から目標点に到達させるようにするサーボ機構制御手段と、を備え、
前記開始点座標計算手段及び累積移動量計算手段の各手段において、前記ロボットのツールの並進動作及び回転動作の各動作に対して、それぞれ個別に１つ、前記手動制御前に予め設定した基準座標系に基づき、前記開始点座標、位置及び姿勢データ、又は各動作の累積移動量を計算するようにしたことを特徴とする産業用ロボットのツールの位置・姿勢制御システム。 A control system used to manually control the position and posture of an industrial robot tool using an orthogonal coordinate system,
Manual operation instruction generating means for detecting that manual control has been executed on the robot work instruction teaching means, and generating a manual operation instruction to the industrial robot in response to the detection;
Starting point coordinate calculating means for calculating a starting point coordinate of manual control of the tool based on the manual operation command;
Based on the manual operation command and the start point coordinates, data on the position and orientation of the tool is calculated for each predetermined interpolation cycle, and the position and orientation data are respectively calculated from the start point represented by the start point coordinates. Cumulative movement amount calculation means for obtaining cumulative movement amounts of translational motion and rotational motion by cumulatively calculating in all interpolation cycles to the target point of the tool;
A tool position command generating means for generating a position command for causing the tool to reach a target point from the start point by inversely converting the cumulative movement amount into a joint angle of each joint of the industrial robot;
Servo mechanism control means for controlling the servo mechanism by transmitting the position command to a servo mechanism for moving each joint of the industrial robot, thereby causing the tool to reach the target point from the start point; With
In each of the starting point coordinate calculating means and the cumulative movement amount calculating means, one individually for each of the translational operation and the rotational operation of the tool of the robot, and the reference coordinates set in advance before the manual control An industrial robot tool position / posture control system, wherein the start point coordinates, position and posture data, or cumulative movement amount of each operation is calculated based on a system.
JP2006152202A 2006-05-31 2006-05-31 Industrial robot tool position / posture control method and control system Active JP5144035B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2006152202A JP5144035B2 (en) 2006-05-31 2006-05-31 Industrial robot tool position / posture control method and control system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2006152202A JP5144035B2 (en) 2006-05-31 2006-05-31 Industrial robot tool position / posture control method and control system

Publications (2)

Publication Number Publication Date
JP2007319970A JP2007319970A (en) 2007-12-13
JP5144035B2 true JP5144035B2 (en) 2013-02-13

Family

ID=38853186

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2006152202A Active JP5144035B2 (en) 2006-05-31 2006-05-31 Industrial robot tool position / posture control method and control system

Country Status (1)

Country Link
JP (1) JP5144035B2 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110497391A (en) * 2019-09-04 2019-11-26 湖南三一快而居住宅工业有限公司 Mechanical arm teaching method and mechanical arm teaching apparatus

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012106321A (en) * 2010-11-19 2012-06-07 Daihen Corp Method and device for controlling robot
JP5848535B2 (en) * 2011-07-05 2016-01-27 川崎重工業株式会社 Stretch forming device
JP5971842B2 (en) * 2012-03-26 2016-08-17 株式会社ダイヘン Robot controller
JP5972027B2 (en) * 2012-04-27 2016-08-17 株式会社ダイヘン Robot controller
JP2016101650A (en) * 2014-11-29 2016-06-02 ライフロボティクス株式会社 Robot device and robot control device
EP3348360A4 (en) * 2015-09-11 2019-09-25 Life Robotics Inc. Robot apparatus
DE102019211270B3 (en) * 2019-07-30 2020-11-26 Kuka Deutschland Gmbh Controlling a Robot
CN110977991A (en) * 2019-12-31 2020-04-10 芜湖哈特机器人产业技术研究院有限公司 Motion control method for airplane cleaning mechanical arm
CN112475897B (en) * 2020-11-23 2022-03-29 陕西固德电子技术有限公司 Welding robot
CN115008468B (en) * 2022-07-04 2024-09-03 中国科学院沈阳自动化研究所 Mechanical arm attitude speed planning control method

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06155236A (en) * 1992-11-11 1994-06-03 Yokogawa Electric Corp Robot control device
JPH08161025A (en) * 1994-12-08 1996-06-21 Kobe Steel Ltd Teaching device
JP4221464B2 (en) * 2002-04-19 2009-02-12 川崎重工業株式会社 Robot system teaching method and teaching apparatus

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110497391A (en) * 2019-09-04 2019-11-26 湖南三一快而居住宅工业有限公司 Mechanical arm teaching method and mechanical arm teaching apparatus
CN110497391B (en) * 2019-09-04 2020-11-03 湖南三一快而居住宅工业有限公司 Mechanical arm demonstration method and mechanical arm demonstration device

Also Published As

Publication number Publication date
JP2007319970A (en) 2007-12-13

Similar Documents

Publication Publication Date Title
JP5144035B2 (en) Industrial robot tool position / posture control method and control system
KR100765671B1 (en) Robot locus control method and apparatus, and computer-readable recording medium storing program of robot locus control method
JP6167770B2 (en) Multi-axis robot trajectory generation method and multi-axis robot controller
JP6268819B2 (en) Trajectory generation method for multi-axis robot
JP6379874B2 (en) Teaching system, robot system, and teaching method
JP6238628B2 (en) Robot device, robot control method, robot control program, and part manufacturing method using robot device
JPH079606B2 (en) Robot controller
JP2017094440A (en) Direct teaching method of robot
WO2018212239A1 (en) Remote operation robot system
US10946514B2 (en) Controller, work control unit, multi-axis motion control unit, and drive control unit
US20210260750A1 (en) Method to Modify a Program for Robotic Welding
JP4396553B2 (en) Robot controller, computer program
JP2017196722A (en) Robot control apparatus for displaying operation program including state of additional axis
JP4498061B2 (en) Welding robot controller
JP2013223895A (en) Robot control method and robot control device
JP4528577B2 (en) Industrial robot
JP2013220501A (en) Robot control method and robot control device
JP6429977B2 (en) Robot apparatus and robot control method
JP4221464B2 (en) Robot system teaching method and teaching apparatus
JP5513207B2 (en) Welding robot controller
JP2009148894A (en) Multi-articulated robot
JP7502439B2 (en) Robot Control Device
JP2013223896A (en) Robot control method and robot control device
JP2014159066A (en) Robot control device and robot control method
JPH08323665A (en) Instruction controller of robot

Legal Events

Date Code Title Description
A711 Notification of change in applicant

Free format text: JAPANESE INTERMEDIATE CODE: A711

Effective date: 20090304

A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20090309

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A821

Effective date: 20090304

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20101109

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20110913

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20111111

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20120403

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20120530

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20121113

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20121122

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20151130

Year of fee payment: 3

R150 Certificate of patent or registration of utility model

Ref document number: 5144035

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250