JP2020062730A

JP2020062730A - Robot control method, robot device, program, recording medium, and article manufacturing method

Info

Publication number: JP2020062730A
Application number: JP2018196847A
Authority: JP
Inventors: 横井　昭佳; Akiyoshi Yokoi; 昭佳横井; 春彦堀口; Haruhiko Horiguchi
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2018-10-18
Filing date: 2018-10-18
Publication date: 2020-04-23

Abstract

To improve working efficiency of a robot, while securing operation accuracy of the articulated robot.SOLUTION: Each servomotor 1 is controlled so that detection angles detected by each input side encoder 10 are target angles, in order to make a robot tip reach a second position from a first position. Then, a position of the robot tip is calculated on the basis of detection angles of a reduction gear 11 detected by each output side encoder 16 and an inclination of an output shaft. A time duration, from a point in time when the detection angles of each input side encoder 10 attain the target angles to a point in time when a swing amplitude at the robot tip position in the calculated second position converges into a prescribed range, is obtained.SELECTED DRAWING: Figure 5

Description

本発明は、多関節ロボットを制御するロボット制御方法、ロボット装置、プログラム、記録媒体及び物品の製造方法に関する。 The present invention relates to a robot control method for controlling an articulated robot, a robot device, a program, a recording medium, and an article manufacturing method.

従来、工場等において、様々なロボット装置が使用されており、昨今においては、より複雑な動作を行わせるために多軸多関節のロボットを備えたロボット装置が広く普及している。 Conventionally, various robot devices have been used in factories and the like, and in recent years, robot devices equipped with multi-axis and multi-joint robots for performing more complicated motions have become widespread.

生産装置として多関節ロボットを用いる場合、作業効率を高めるためには、ロボットを高速に動作させる必要がある。そのためには、一つの動作が確実に完了したことの検出、すなわち位置決め完了の検出を可能な限り速やかに行う必要がある。 When an articulated robot is used as a production device, it is necessary to operate the robot at high speed in order to improve work efficiency. For that purpose, it is necessary to detect that one operation is surely completed, that is, to detect the completion of positioning as quickly as possible.

ロボットの位置決め完了の検出としては、例えば、ロボットの先端位置を各関節の出力側に搭載した出力側エンコーダにより各関節の回転角度を検出し、その回転角度に基づきロボットの先端の位置を求める方法が知られている。 The detection of the completion of the positioning of the robot is, for example, a method of detecting the rotation angle of each joint by the output side encoder mounted on the output side of each joint and determining the position of the robot tip based on the rotation angle. It has been known.

特開２０１６−１３１６１３号公報JP, 2016-131613, A

しかしながら、特許文献１に記載された方法は各関節の回転角度を検出することは可能だが、各関節軸の傾き（減速機の出力軸の傾き）を検出することができなかった。そのため、旋回動作のような各関節の回転軸と直交する方向に力が加わる動作においては、多関節ロボットの先端位置の振動の収束を正確に検知できず、ロボットの動作を停止する時間に多くのマージンを設ける必要があった。 However, the method described in Patent Document 1 can detect the rotation angle of each joint, but cannot detect the inclination of each joint axis (the inclination of the output shaft of the speed reducer). Therefore, in a motion such as a turning motion in which a force is applied in a direction orthogonal to the rotation axis of each joint, it is not possible to accurately detect the convergence of the vibration at the tip position of the multi-joint robot, and it is often necessary to stop the motion of the robot. It was necessary to provide a margin.

上記課題を解決するためのロボット制御方法は、多関節ロボットの各関節を駆動する各関節に設けられた関節駆動装置が、減速機、及び前記減速機の回転角度を検出するエンコーダを各々有し、制御手段により、前記多関節ロボットが第１位置から第２位置に到達する第１動作を実行した後に、前記多関節ロボットを停止時間、停止させてから、第２動作を実行するロボット制御方法であって、前記停止時間は、前記多関節ロボットが前記第１動作で前記第２位置に到達した時点から、前記各エンコーダにより検出された前記減速機の回転角度と前記減速機の出力軸の傾きに基づき求められた前記多関節ロボットの先端の位置の振れ幅が所定範囲内に収束する時点までの時間であることを特徴とする。 In a robot control method for solving the above-mentioned problems, a joint drive device provided for each joint that drives each joint of an articulated robot has a reduction gear and an encoder that detects a rotation angle of the reduction gear. A robot control method in which the control means executes the first operation of reaching the second position from the first position and then stops the multi-joint robot for a stop time and then executes the second operation. The stop time is the rotation angle of the speed reducer detected by the encoders and the output shaft of the speed reducer from the time when the articulated robot reaches the second position in the first motion. It is characterized in that it is a time until the swing width of the position of the tip of the articulated robot obtained based on the inclination converges within a predetermined range.

本発明によれば、回転軸と直交する方向に力が加わるような動作の後でも、多関節ロボットの先端の位置の収束を正確に検知することが可能となるため、位置決め検出の時間に多くのマージンを設ける必要が無くなる。そのため、従来よりもロボットの停止時間を短縮できるため、多関節ロボットの作業効率を向上させることができる。 According to the present invention, it is possible to accurately detect the convergence of the position of the tip of the articulated robot even after the operation in which a force is applied in the direction orthogonal to the rotation axis, and thus it is possible to increase the positioning detection time. It is no longer necessary to provide a margin. Therefore, the robot stop time can be shortened as compared with the related art, and the work efficiency of the articulated robot can be improved.

第１実施形態に係るロボット装置を示す斜視図である。FIG. 3 is a perspective view showing the robot apparatus according to the first embodiment. 第１実施形態に係るロボット装置のロボットアームの１つの関節を示す部分断面図である。FIG. 3 is a partial cross-sectional view showing one joint of the robot arm of the robot apparatus according to the first embodiment. 第１実施形態に係るロボット装置の制御装置の構成を示すブロック図である。It is a block diagram which shows the structure of the control apparatus of the robot apparatus which concerns on 1st Embodiment. 第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。It is a functional block diagram which shows the control system of the robot apparatus which concerns on 1st Embodiment. 第１実施形態に係るロボット装置におけるロボット制御方法の各工程を示すフローチャートである。6 is a flowchart showing each step of the robot control method in the robot apparatus according to the first embodiment. 第１実施形態に係るロボットの先端の振動状態を示すグラフである。6 is a graph showing a vibration state of the tip of the robot according to the first embodiment. 第１実施形態に係るロボットの関節Ｊ１を示す分解斜視図である。FIG. 3 is an exploded perspective view showing a joint J1 of the robot according to the first embodiment. 第１実施形態に係る出力側エンコーダの構成を示す模式図である。It is a schematic diagram which shows the structure of the output side encoder which concerns on 1st Embodiment. 第１実施形態に係る出力側エンコーダのＡＰＣの動作説明図である。FIG. 3 is an operation explanatory diagram of APC of the output side encoder according to the first embodiment. 第１実施形態に係る出力側エンコーダの模式図である。It is a schematic diagram of the output side encoder which concerns on 1st Embodiment. 第２実施形態に係るロボットの関節Ｊ２を示す分解斜視図である。It is an exploded perspective view showing joint J2 of the robot concerning a 2nd embodiment. 第３実施形態に係るロボット装置の制御系を示す機能ブロック図である。It is a functional block diagram which shows the control system of the robot apparatus which concerns on 3rd Embodiment. 第３実施形態に係るロボット装置におけるロボット制御方法の各工程を示すフローチャートである。9 is a flowchart showing each step of the robot control method in the robot apparatus according to the third embodiment. 第３実施形態に係るロボット装置における推定工程を示すフローチャートである。It is a flowchart which shows the estimation process in the robot apparatus which concerns on 3rd Embodiment. 第３実施形態に係るロボット装置における推定工程を説明するための概念図である。It is a conceptual diagram for demonstrating the estimation process in the robot apparatus which concerns on 3rd Embodiment.

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。 Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings.

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置を示す斜視図である。 [First Embodiment]
FIG. 1 is a perspective view showing a robot apparatus according to the first embodiment of the present invention.

ロボット装置５００は、把持した第１部品Ｗ１を第２部品Ｗ２に組付けた組付け物品を製造するロボット１００と、ロボット１００を制御する制御手段としての制御装置２００と、制御装置２００に接続されたティーチングペンダント３００と、を備えている。 The robot device 500 is connected to the control device 200, and a robot 100 that manufactures an assembled article in which the held first component W1 is assembled to the second component W2, a control device 200 as a control unit that controls the robot 100. And a teaching pendant 300.

ロボット１００は、多関節のロボットであり、垂直多関節型のロボットアーム１０１と、ロボットアーム１０１の先端に接続されたエンドエフェクタであるロボットハンド１０２と、を備えている。 The robot 100 is an articulated robot, and includes a vertical articulated robot arm 101 and a robot hand 102 that is an end effector connected to the tip of the robot arm 101.

ロボット１００の先端（ロボットハンド１０２又はロボットアーム１０１の先端）には、ツールセンターポイント（ＴＣＰ）が設定されており、教示点がタスク空間で指定される場合、その教示点は、ＴＣＰの位置及び姿勢を示すパラメータで表現される。教示点がコンフィグレーション空間（関節空間）で指定される場合、その教示点は、ロボット１００の各関節の関節角度を示すパラメータで表現される。なお、コンフィグレーション空間で指定された教示点は、順運動学計算により、タスク空間に変換することができ、タスク空間で指定された教示点は、逆運動学計算により、コンフィグレーション空間に変換することができる。 A tool center point (TCP) is set at the tip of the robot 100 (the tip of the robot hand 102 or the robot arm 101), and when the teaching point is designated in the task space, the teaching point is the position of the TCP and It is expressed by a parameter indicating the posture. When the teaching point is specified in the configuration space (joint space), the teaching point is represented by a parameter indicating the joint angle of each joint of the robot 100. The teaching point specified in the configuration space can be converted into the task space by the forward kinematics calculation, and the teaching point specified in the task space can be converted into the configuration space by the inverse kinematics calculation. be able to.

ロボットアーム１０１は、作業台に固定されるベース部（基端リンク）１０３と、変位や力を伝達する複数のリンク１２１〜１２６と、を有している。ベース部１０３及び複数のリンク１２１〜１２６は、複数の関節Ｊ１〜Ｊ６で旋回又は回転可能に互いに連結されている。また、ロボットアーム１０１は、各関節Ｊ１〜Ｊ６に設けられた、関節を駆動する関節駆動装置１１０を備えている。各関節Ｊ１〜Ｊ６に配置された関節駆動装置１１０は、必要なトルクの大きさに合わせて適切な出力のものが用いられる。 The robot arm 101 has a base portion (base end link) 103 fixed to a workbench and a plurality of links 121 to 126 for transmitting displacement and force. The base portion 103 and the plurality of links 121 to 126 are connected to each other via a plurality of joints J1 to J6 so as to be pivotable or rotatable. The robot arm 101 also includes a joint drive device 110 that is provided in each of the joints J1 to J6 and that drives the joint. As the joint drive device 110 arranged in each of the joints J1 to J6, one having an appropriate output is used according to the magnitude of the required torque.

ロボットハンド１０２は、部品Ｗ１を把持する複数の把持爪（フィンガ）１０４と、複数の把持爪１０４を駆動する不図示の駆動部と、駆動部の回転角度を検出する不図示のエンコーダと、回転を把持動作に変換する不図示の機構とを有している。この不図示の機構は、カム機構やリンク機構などで必要な把持動作に合わせて設計される。なお、ロボットハンド１０２に用いる駆動部に必要なトルクは、ロボットアーム１０１の関節用と異なるが、基本構成は同じである。また、ロボットハンド１０２は、把持爪１０４等に作用する応力（反力）を検出可能な不図示の力覚センサを有している。 The robot hand 102 includes a plurality of grip claws (fingers) 104 that grip the component W1, a drive unit (not shown) that drives the plurality of grip claws 104, an encoder (not shown) that detects a rotation angle of the drive unit, and a rotation. Has a mechanism (not shown) for converting the gripping motion into a gripping motion. This mechanism (not shown) is designed according to the gripping operation required by the cam mechanism, the link mechanism, or the like. The torque required for the drive unit used for the robot hand 102 is different from that for the joint of the robot arm 101, but the basic configuration is the same. Further, the robot hand 102 has a force sensor (not shown) capable of detecting a stress (reaction force) acting on the grip claw 104 and the like.

ティーチングペンダント３００は、制御装置２００に接続可能に構成され、制御装置２００に接続された際に、ロボットアーム１０１やロボットハンド１０２を駆動制御する指令を制御装置２００に送信可能に構成されている。 The teaching pendant 300 is configured to be connectable to the control device 200, and when connected to the control device 200, a command for driving and controlling the robot arm 101 and the robot hand 102 can be transmitted to the control device 200.

以下、関節Ｊ２における関節駆動装置１１０を例に説明し、他の関節Ｊ１，Ｊ３〜Ｊ６の関節駆動装置１１０については、サイズや性能が異なる場合もあるが、同様の構成であるため、説明を省略する。 Hereinafter, the joint drive device 110 of the joint J2 will be described as an example, and the joint drive devices 110 of the other joints J1, J3 to J6 may have different sizes and performances, but since they have the same configuration, the description thereof will be omitted. Omit it.

図２は、ロボットアーム１０１の関節Ｊ２を示す部分断面図である。関節駆動装置１１０は、電動モータであるサーボモータ（以下、「モータ」という）１と、モータ１の回転軸２の回転を減速して出力する減速機１１と、を有している。 FIG. 2 is a partial cross-sectional view showing the joint J2 of the robot arm 101. The joint drive device 110 includes a servo motor (hereinafter referred to as “motor”) 1 that is an electric motor, and a speed reducer 11 that decelerates and outputs the rotation of a rotation shaft 2 of the motor 1.

また、関節駆動装置１１０は、モータ１の回転軸２の回転角度（出力角度）を検出する入力側エンコーダ１０を有している。また、関節駆動装置１１０は、減速機１１の出力軸の回転角度（出力角度）を検出する出力側エンコーダ１６を有している。 Further, the joint drive device 110 has an input side encoder 10 that detects a rotation angle (output angle) of the rotation shaft 2 of the motor 1. Further, the joint driving device 110 has an output side encoder 16 that detects a rotation angle (output angle) of the output shaft of the speed reducer 11.

モータ１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ１は、回転軸２とロータマグネット３とで構成された回転部４と、モータハウジング５と、回転軸２を回転自在に支持する軸受６，７と、回転部４を回転させるステータコイル８と、を備えている。軸受６，７はモータハウジング５に設けられ、ステータコイル８はモータハウジング５に取り付けられている。また、サーボモータ１はモータカバー９で囲われている。 The motor 1 is, for example, a brushless DC servo motor or an AC servo motor. The motor 1 includes a rotating portion 4 including a rotating shaft 2 and a rotor magnet 3, a motor housing 5, bearings 6 and 7 that rotatably support the rotating shaft 2, and a stator coil 8 that rotates the rotating portion 4. And are equipped with. The bearings 6 and 7 are provided in the motor housing 5, and the stator coil 8 is attached to the motor housing 5. The servo motor 1 is surrounded by a motor cover 9.

入力側エンコーダ１０は、光学式のロータリエンコーダであり、モータ１の回転軸２の一端側に設けられ、モータ１の回転軸２の回転に伴ってパルス信号を生成し、制御装置２００に生成したパルス信号を出力する。なお、入力側エンコーダ１０は、回転軸２に取り付けられているが、減速機１１の入力軸に取り付けてもよい。 The input side encoder 10 is an optical rotary encoder, is provided on one end side of the rotary shaft 2 of the motor 1, generates a pulse signal in accordance with the rotation of the rotary shaft 2 of the motor 1, and generates the pulse signal in the control device 200. Output a pulse signal. Although the input encoder 10 is attached to the rotary shaft 2, it may be attached to the input shaft of the speed reducer 11.

出力側エンコーダ１６は、光学式のロータリエンコーダでスケール５２０と２つのセンサユニット５０７で構成されており、スケール５２０とセンサユニット５０７の相対回転を検出が可能である。第１実施形態としては減速機１１の出力角度としてベース部１０３とリンク１２１、或いは隣り合う２つのリンク間の相対角度を検出する。関節Ｊ２においては、出力側エンコーダ１６は、リンク１２１とリンク１２２との間の相対角度を検出する。具体的には、出力側エンコーダ１６は、関節Ｊ２の駆動（リンク１２１とリンク１２２との相対移動）に伴ってパルス信号を生成し、制御装置２００に生成したパルス信号を出力する。 The output side encoder 16 is an optical rotary encoder, which includes a scale 520 and two sensor units 507, and can detect relative rotation between the scale 520 and the sensor unit 507. As the first embodiment, the output angle of the speed reducer 11 is detected as the relative angle between the base portion 103 and the link 121 or between two adjacent links. At the joint J2, the output encoder 16 detects the relative angle between the link 121 and the link 122. Specifically, the output side encoder 16 generates a pulse signal in accordance with the driving of the joint J2 (relative movement of the link 121 and the link 122), and outputs the generated pulse signal to the control device 200.

また、出力側エンコーダ１６は、スケール５２０とセンサユニット５０７の距離であるギャップを測定することが可能となっている。この測定原理については後述する。 Further, the output side encoder 16 can measure the gap which is the distance between the scale 520 and the sensor unit 507. This measurement principle will be described later.

リンク１２１とリンク１２２とは、クロスローラベアリング１５を介して回転自在に結合されている。モータ１と入力側エンコーダ１０との間には、必要に応じて、電源オフ時にロボットアーム１０１の姿勢を保持するためのブレーキユニットを設けてもよい。 The link 121 and the link 122 are rotatably coupled via a cross roller bearing 15. A brake unit for holding the posture of the robot arm 101 when the power is off may be provided between the motor 1 and the input side encoder 10 as required.

減速機１１は、小型軽量で減速比の大きい波動歯車減速機である。減速機１１は、モータ１の回転軸２に結合された、入力軸を有するウェブジェネレータ１２と、リンク１２２に固定された、出力軸を有するサーキュラスプライン１３と、を備えている。なお、サーキュラスプライン１３は、リンク１２２に直結されているが、リンク１２２に一体に形成されていてもよい。 The speed reducer 11 is a wave gear speed reducer that is small and lightweight and has a large reduction ratio. The speed reducer 11 includes a web generator 12 having an input shaft, which is coupled to the rotary shaft 2 of the motor 1, and a circular spline 13 having an output shaft, which is fixed to a link 122. Although the circular spline 13 is directly connected to the link 122, it may be integrally formed with the link 122.

また、減速機１１は、ウェブジェネレータ１２とサーキュラスプライン１３との間に配置され、リンク１２１に固定されたフレクスプライン１４を備えている。フレクスプライン１４は、ウェブジェネレータ１２の回転に対して減速比Ｎで減速され、サーキュラスプライン１３に対して相対的に回転する。従って、モータ１の回転軸２の回転数が減速機１１で１／Ｎに減速され、フレクスプライン１４が固定されたリンク１２１に対してサーキュラスプライン１３が固定されたリンク１２２を相対的に回転運動させ、関節Ｊ２を屈曲（回転）させる。このときの減速機１１の出力側の回転角度が、実出力角度、即ち関節Ｊ２の角度（関節角度）となる。 Further, the speed reducer 11 is provided between the web generator 12 and the circular spline 13 and includes a flex spline 14 fixed to the link 121. The flex spline 14 is decelerated with a speed reduction ratio N with respect to the rotation of the web generator 12, and rotates relative to the circular spline 13. Therefore, the rotation speed of the rotary shaft 2 of the motor 1 is reduced to 1 / N by the speed reducer 11, and the link 122 to which the circular spline 13 is fixed is relatively rotated with respect to the link 121 to which the flexspline 14 is fixed. Then, the joint J2 is bent (rotated). The rotation angle on the output side of the speed reducer 11 at this time becomes the actual output angle, that is, the angle of the joint J2 (joint angle).

図３は、ロボット装置５００の制御装置２００の構成を示すブロック図である。制御装置２００は、制御部（演算部）としてのＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）２０１を備えている。また、制御装置２００は、記憶手段として、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）２０２、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）２０３、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）２０４を備えている。また、制御装置２００は、記録ディスクドライブ２０５、計時手段である計時部（タイマ）２０６及び各種のインタフェース２１１〜２１６を備えている。また、制御装置２００は、インタフェース２１６に接続されたサーボ制御装置２３０を備えている。 FIG. 3 is a block diagram showing the configuration of the control device 200 of the robot device 500. The control device 200 includes a CPU (Central Processing Unit) 201 as a control unit (arithmetic unit). Further, the control device 200 includes a ROM (Read Only Memory) 202, a RAM (Random Access Memory) 203, and a HDD (Hard Disk Drive) 204 as storage means. Further, the control device 200 includes a recording disk drive 205, a time measuring unit (timer) 206 which is a time measuring means, and various interfaces 211 to 216. The controller 200 also includes a servo controller 230 connected to the interface 216.

ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５、タイマ２０６及び各種のインタフェース２１１〜２１６が、バス２２０を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。 A ROM 202, a RAM 203, an HDD 204, a recording disk drive 205, a timer 206, and various interfaces 211 to 216 are connected to the CPU 201 via a bus 220. The ROM 202 stores basic programs such as BIOS. The RAM 203 is a storage device that temporarily stores various data such as the calculation processing result of the CPU 201.

ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ２０１に、後述する各種演算処理を実行させるためのプログラム２４０を記録するものである。ＣＰＵ２０１は、ＨＤＤ２０４に記録（格納）されたプログラム２４０に基づいてロボット制御方法の各工程を実行する。 The HDD 204 is a storage device that stores the calculation processing result of the CPU 201, various data acquired from the outside, and the like, and also records a program 240 for causing the CPU 201 to execute various calculation processing described below. The CPU 201 executes each step of the robot control method based on the program 240 recorded (stored) in the HDD 204.

記録ディスクドライブ２０５は、記録ディスク２４１に記録された各種データやプログラム等を読み出すことができる。タイマ２０６は、ＣＰＵ２０１からの計時開始の指令と計時終了の指令に基づき時間をカウントし、その時間のデータをＣＰＵ２０１に出力する。 The recording disk drive 205 can read various data, programs, etc. recorded on the recording disk 241. The timer 206 counts time based on a command to start timing and a command to terminate timing from the CPU 201, and outputs data of the time to the CPU 201.

インタフェース２１１には、ユーザが操作可能なティーチングペンダント３００が接続されている。ティーチングペンダント３００は、入力された教示点のデータ（コンフィグレーション空間で指定された場合は各関節Ｊ１〜Ｊ６の目標角度）をインタフェース２１１及びバス２２０を介してＣＰＵ２０１に出力する。ＣＰＵ２０１は、ティーチングペンダント３００から入力を受けた教示点のデータをＨＤＤ２０４等の記憶部に設定する。 A teaching pendant 300 that can be operated by a user is connected to the interface 211. The teaching pendant 300 outputs the input teaching point data (target angles of the joints J1 to J6 when specified in the configuration space) to the CPU 201 via the interface 211 and the bus 220. The CPU 201 sets the teaching point data received from the teaching pendant 300 in the storage unit such as the HDD 204.

インタフェース２１２，２１３には、各種画像が表示されるモニタ３１１や書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３１２が接続されている。 A monitor 311 for displaying various images and an external storage device 312 such as a rewritable nonvolatile memory or an external HDD are connected to the interfaces 212 and 213.

インタフェース２１４，２１５には、入力側エンコーダ１０及び出力側エンコーダ１６がそれぞれ接続されている。入力側エンコーダ１０及び出力側エンコーダ１６は、前述したパルス信号をインタフェース２１４，２１５及びバス２２０を介してＣＰＵ２０１に出力する。 The input side encoder 10 and the output side encoder 16 are connected to the interfaces 214 and 215, respectively. The input side encoder 10 and the output side encoder 16 output the above-mentioned pulse signals to the CPU 201 via the interfaces 214 and 215 and the bus 220.

インタフェース２１６には、サーボ制御装置２３０が接続されている。ＣＰＵ２０１は、教示点間を繋ぐ軌道を生成して、サーボモータ１の回転軸２の回転角度の制御量を示す駆動指令のデータを所定の時間間隔でバス２２０及びインタフェース２１６を介してサーボ制御装置２３０に出力する。 The servo control device 230 is connected to the interface 216. The CPU 201 generates a trajectory connecting the teaching points, and outputs drive command data indicating the control amount of the rotation angle of the rotation shaft 2 of the servo motor 1 at predetermined time intervals via the bus 220 and the interface 216. Output to 230.

サーボ制御装置２３０は、ＣＰＵ２０１から入力を受けた駆動指令に基づき、フィードバック制御によるサーボモータ１への電流の出力量を演算し、サーボモータ１へ電流を供給して、ロボットアーム１０１の関節Ｊ１〜Ｊ６の関節角度制御を行う。即ち、ＣＰＵ２０１は、サーボ制御装置２３０を介して、関節Ｊ１〜Ｊ６の角度が目標角度となるように、サーボモータ１による関節Ｊ１〜Ｊ６の駆動を制御する。 The servo control device 230 calculates the output amount of the current to the servo motor 1 by feedback control based on the drive command received from the CPU 201, supplies the current to the servo motor 1, and joints J1 to J1 of the robot arm 101. J6 joint angle control is performed. That is, the CPU 201 controls the drive of the joints J1 to J6 by the servo motor 1 via the servo control device 230 so that the angles of the joints J1 to J6 become the target angles.

ここで、後述する位置決め完了検出プログラムを実行する際に、ＣＰＵ２０１及びＨＤＤ２０４が有する機能について、図４を参照しながら説明する。図４は、本発明の第１実施形態に係るロボット装置５００の制御系を示す機能ブロック図である。ここで、図３に示すＣＰＵ２０１は、プログラム２４０を実行することにより、図４に示す各演算部４０１，４０２，４０４，４０６として機能する。また、記憶手段としてのＨＤＤ２０４は、各記憶部４０３，４０５として機能する。以下、各部について説明する。 Here, the functions of the CPU 201 and the HDD 204 when executing the later-described positioning completion detection program will be described with reference to FIG. FIG. 4 is a functional block diagram showing a control system of the robot apparatus 500 according to the first embodiment of the present invention. Here, the CPU 201 shown in FIG. 3 functions as the arithmetic units 401, 402, 404, 406 shown in FIG. 4 by executing the program 240. Further, the HDD 204 as a storage unit functions as the storage units 403 and 405. Hereinafter, each part will be described.

実出力角演算部４０１は、出力側エンコーダ１６からパルス信号の入力を受け、出力角度（検出角度）θを演算する。入力側エンコーダ１０により出力側の回転角度を演算する方法と比較して、この出力角度θは減速機１１の撓み分を含んだ値となっており、よりロボットアーム１０１の先端に近い位置情報を得ることが可能となる。 The actual output angle calculation unit 401 receives a pulse signal input from the output encoder 16 and calculates an output angle (detection angle) θ. Compared with the method of calculating the rotation angle on the output side by the input side encoder 10, this output angle θ has a value including the bending amount of the speed reducer 11, and the position information closer to the tip of the robot arm 101 can be obtained. It becomes possible to obtain.

傾き演算部４０６は、出力側エンコーダ１６のスケール５２０とセンサユニット５０７のギャップの測定値より関節軸の傾き（減速機１１の出力軸の傾き）を演算する。関節軸の傾きはクロスローラベアリング１５のガタや撓みによる傾きを含む値となる。 The tilt calculator 406 calculates the tilt of the joint axis (the tilt of the output shaft of the speed reducer 11) from the measured value of the gap between the scale 520 of the output encoder 16 and the sensor unit 507. The inclination of the joint shaft has a value including the inclination of the cross roller bearing 15 due to backlash or bending.

先端位置演算部４０２は、実出力角演算部４０１の値、即ち各関節Ｊ１〜Ｊ６の関節角度である各出力側エンコーダ１６の検出角度と、傾き演算部４０６により算出される関節軸の傾きに基づき、順運動学によりロボット１００の先端（ＴＣＰ）の位置を演算する。 The tip position calculation unit 402 calculates the value of the actual output angle calculation unit 401, that is, the detected angle of each output side encoder 16 which is the joint angle of each joint J1 to J6, and the inclination of the joint axis calculated by the inclination calculation unit 406. Based on this, the position of the tip (TCP) of the robot 100 is calculated by forward kinematics.

位置決め完了判定値記憶部４０３は、ロボット１００の先端の位置決めが完了したと判定するときに用いる、先端位置の許容範囲（所定の値に収束する範囲、以下、所定範囲内）を決める、位置決め完了判定値を記憶する。この値は、ユーザが事前に位置決め完了判定値記憶部４０３に記憶させておくのがよい。この判定値は、ロボットアーム１０１の動作状況に応じて、直交座標系（世界座標系）における座標ｘ，ｙ，ｚの各軸方向に個別で設定するとよい。つまり、ロボット１００の先端の位置を直交座標系で表したときに、直交座標系の各軸方向で個別に所定範囲を設定すればよい。例えば、高い動作精度を要求される方向のみ判定値を小さく設定すれば、最適な位置決め完了判定時間を得ることが可能となる。この値は、ユーザが事前実験や図面などから導出したものであってもよいし、より簡易的に、使用状況別に数段階の既定の値を用意し、選択して適用するようにしてもよい。 The positioning completion determination value storage unit 403 determines an allowable range of the tip position (range to converge to a predetermined value, hereinafter, within a predetermined range), which is used when it is determined that the positioning of the tip of the robot 100 is completed, the positioning completion Memorize the judgment value. This value is preferably stored in the positioning completion determination value storage unit 403 by the user in advance. This determination value may be individually set in each axis direction of the coordinates x, y, z in the Cartesian coordinate system (world coordinate system) according to the operating condition of the robot arm 101. That is, when the position of the tip of the robot 100 is represented by the orthogonal coordinate system, the predetermined range may be set individually in each axial direction of the orthogonal coordinate system. For example, if the determination value is set small only in the direction in which high operation accuracy is required, it is possible to obtain the optimum positioning completion determination time. This value may be derived by the user from a preliminary experiment, a drawing, or the like, or more simply, a preset value of several levels may be prepared according to the usage status, and selected and applied. .

位置決め時間演算部４０４は、先端位置演算部４０２からの値と、位置決め完了判定値記憶部４０３からの値を基にして、ロボット１００の対象動作の位置決め時間（ロボット１００を停止させておく停止時間）を求める。 The positioning time calculation unit 404 determines the positioning time of the target operation of the robot 100 (stop time for stopping the robot 100 based on the value from the tip position calculation unit 402 and the value from the positioning completion determination value storage unit 403). ).

パラメータ記憶部４０５は、位置決め時間演算部４０４が求めた値を記憶する。記憶した値は、ロボット装置５００を連続運転させる際に、サーボ制御装置２３０を介して当該動作の位置決め完了時間（停止時間）として適用される。 The parameter storage unit 405 stores the value calculated by the positioning time calculation unit 404. The stored value is applied as the positioning completion time (stop time) of the operation via the servo controller 230 when the robot apparatus 500 is continuously operated.

ここで、図１に示すように、ロボット１００の先端（ＴＣＰ）を教示点Ｐ１，Ｐ２，Ｐの順に動作させて部品Ｗ１を部品Ｗ２に組付けて組付物品を製造する場合について説明する。図１において、教示点Ｐ１（第１位置）から教示点Ｐ２（第２位置）に向かう動作を第１動作、教示点Ｐ２から教示点Ｐ３に向かう動作を第２動作とする。制御装置２００は、ロボット１００を第１動作から第２動作に移行させる際に、ロボット１００の振動を収束させる目的で教示点Ｐ２の位置でロボット１００を一旦、ある停止時間だけ停止させる。 Here, as shown in FIG. 1, a case will be described in which the tip (TCP) of the robot 100 is operated in the order of the teaching points P1, P2, and P to assemble the part W1 to the part W2 to manufacture an assembled article. In FIG. 1, an operation from the teaching point P1 (first position) to the teaching point P2 (second position) is referred to as a first operation, and an operation from the teaching point P2 to the teaching point P3 is referred to as a second operation. When the control device 200 shifts the robot 100 from the first motion to the second motion, the control device 200 temporarily stops the robot 100 at a position of the teaching point P2 for a certain stop time in order to converge the vibration of the robot 100.

つまり、制御装置２００は、第１動作では、各入力側エンコーダ１０により検出された検出角度が、コンフィグレーション空間で指定された教示点Ｐ２（つまり、各関節Ｊ１〜Ｊ６の目標角度）となるようにフィードバック制御する。制御装置２００は、入力側エンコーダ１０の検出結果に基づいてフィードバック制御するので、出力側エンコーダ１６の検出結果に基づいてフィードバック制御する際よりも速く教示点Ｐ２に到達する。しかし、ロボット１００は、第１動作に伴う慣性力の影響を受け、教示点Ｐ２に到達したときにロボット１００が振動する。 That is, in the first operation, the control device 200 causes the detection angle detected by each input encoder 10 to become the teaching point P2 (that is, the target angle of each joint J1 to J6) designated in the configuration space. Feedback control. Since the control device 200 performs the feedback control based on the detection result of the input side encoder 10, the control device 200 reaches the teaching point P2 faster than when performing the feedback control based on the detection result of the output side encoder 16. However, the robot 100 is affected by the inertial force associated with the first motion, and vibrates when reaching the teaching point P2.

振動は、減速機１１の撓みによる各関節の回転方向成分の振動と、クロスローラベアリング１５のガタや撓みやリンク１２１〜１２６の撓みによる各関節の出力軸方向の成分の振動が発生する。この振動は、モータ１の角度（入力側エンコーダ１０による検出角度）では正確に求めることは困難である。ロボット１００の振動が収束していない状態で第２動作、即ち部品Ｗ１を部品Ｗ２に組付けて組付物品を製造する動作を行うと、組み付けが失敗することがある。よって、第１実施形態では、ロボット１００に第１動作を行わせて教示点Ｐ２に到達した時点から予め設定された停止時間だけ停止させた後に、次の第２動作を実行するようにしている。この停止時間のデータは、前述した位置決め時間演算部４０４に記憶させておく。 As for the vibration, the vibration of the rotational direction component of each joint due to the flexure of the reduction gear 11 and the vibration of the component in the output axis direction of each joint due to the backlash and the flexure of the cross roller bearing 15 and the flexure of the links 121 to 126 are generated. It is difficult to accurately obtain this vibration by the angle of the motor 1 (angle detected by the input side encoder 10). If the second operation, that is, the operation of assembling the component W1 with the component W2 to manufacture the assembled article, is performed in a state where the vibration of the robot 100 is not converged, the assembly may fail. Therefore, in the first embodiment, the robot 100 is caused to perform the first operation and is stopped for a preset stop time from the time when the teaching point P2 is reached, and then the next second operation is executed. . The stop time data is stored in the positioning time calculation unit 404 described above.

以下、この停止時間の設定動作について説明する。図５は、制御装置２００が実施するロボット制御方法の各工程を示すフローチャートである。 The operation of setting the stop time will be described below. FIG. 5 is a flowchart showing each step of the robot control method executed by the control device 200.

まず、サーボ制御装置２３０は、ＣＰＵ２０１の指令により検出対象動作を実行する（Ｓ１：フィードバック制御工程）。サーボ制御装置２３０は、ＣＰＵ２０１からの指令に基づき、入力側エンコーダ１０からの位置情報をフィードバックして制御する。詳述すると、サーボ制御装置２３０は、ＣＰＵ２０１の制御の下、各入力側エンコーダ１０により検出された検出角度が、ロボット１００の先端が第１動作の位置決め完了位置に移動する目標角度となるよう、各サーボモータ１をフィードバック制御する。なお、第１動作の位置決め完了位置とは、教示点Ｐ２の位置座標ｘ，ｙ，ｚであり、以下、目標位置ともいう。 First, the servo control device 230 executes a detection target operation according to a command from the CPU 201 (S1: feedback control step). The servo controller 230 feeds back and controls the position information from the input side encoder 10 based on a command from the CPU 201. More specifically, under the control of the CPU 201, the servo control device 230 sets the detection angle detected by each input-side encoder 10 to a target angle at which the tip of the robot 100 moves to the positioning completion position of the first operation. Feedback control is performed on each servo motor 1. The positioning completion position of the first operation is the position coordinates x, y, z of the teaching point P2, and is hereinafter also referred to as the target position.

次に、各関節の出力軸の回転角度を検出する位置検出モードに切り替える（Ｓ２）。 Next, the mode is switched to the position detection mode for detecting the rotation angle of the output shaft of each joint (S2).

続いて、実出力角演算部４０１は、ステップＳ１にて各関節Ｊ１〜Ｊ６の入力側エンコーダ１０の検出角度を目標角度にした時点からタイマ２０６による計時を開始させ、各関節Ｊ１〜Ｊ６の出力側エンコーダ１６の検出角度を取り込む。 Subsequently, the actual output angle calculation unit 401 starts the time counting by the timer 206 from the time when the detection angle of the input side encoder 10 of each joint J1 to J6 is set to the target angle in step S1, and the output of each joint J1 to J6 is output. The detection angle of the side encoder 16 is fetched.

そして、実出力角演算部４０１は、各時刻における各関節Ｊ１〜Ｊ６の減速機１１の出力軸の回転角度を、エンコーダ１６からのパルス信号に基づき計算する（Ｓ３）。即ち、実出力角演算部４０１は、各出力側エンコーダ１６により検出された検出角度を取得していることになる。 Then, the actual output angle calculation unit 401 calculates the rotation angle of the output shaft of the reduction gear 11 of each joint J1 to J6 at each time based on the pulse signal from the encoder 16 (S3). That is, the actual output angle calculation unit 401 has acquired the detection angle detected by each output encoder 16.

次に、各関節の出力軸の傾きを検出する傾き検出モードに切り替える（Ｓ４）。傾き検出モードに切り替えると、出力側エンコーダ１６は回転角度検出からギャップの測定に切り替わる。 Next, the mode is switched to the tilt detection mode for detecting the tilt of the output shaft of each joint (S4). When switching to the tilt detection mode, the output side encoder 16 switches from rotation angle detection to gap measurement.

次に、傾き演算部４０６は出力側エンコーダ１６のスケール５２０とセンサユニット５０７のギャップの測定値を取り込み、センサユニット５０７の配置情報から各時刻における各関節Ｊ１〜Ｊ６の軸の傾きを計算する（Ｓ５）。 Next, the tilt calculator 406 takes in the measured values of the gap between the scale 520 of the output encoder 16 and the sensor unit 507, and calculates the tilts of the axes of the joints J1 to J6 at each time from the arrangement information of the sensor unit 507 ( S5).

次に、先端位置演算部４０２は、ステップＳ３で求めた各関節の減速機１１の出力軸の回転角度とステップＳ５で求めた各関節の軸の傾きにより、ロボット１００の先端位置（ＴＣＰの位置）を求める（Ｓ６：先端位置算出工程）。この先端位置は、各関節の回転角度と傾きを順運動学で変換して得られるものである。つまり、先端位置演算部４０２は、各出力側エンコーダ１６により検出された検出角度および傾きに基づきロボット１００の先端の位置を求める。 Next, the tip position calculation unit 402 determines the tip position (TCP position) of the robot 100 based on the rotation angle of the output shaft of the reduction gear 11 of each joint obtained in step S3 and the inclination of each joint axis obtained in step S5. ) Is obtained (S6: tip position calculation step). This tip position is obtained by converting the rotation angle and inclination of each joint by forward kinematics. That is, the tip position calculation unit 402 obtains the position of the tip of the robot 100 based on the detected angle and inclination detected by each output encoder 16.

また、動作完了まで位置検出モード（Ｓ２）と傾き検出モードに（Ｓ４）を高速で交互に切り替えて各時間の先端位置を算出し続けていく（Ｓ７）。 Further, until the operation is completed, the position detection mode (S2) and the inclination detection mode (S4) are alternately switched at high speed, and the tip position of each time is continuously calculated (S7).

図６は、ロボットの先端の振動状態を示すグラフである。横軸は時間ｔ、縦軸はロボットの先端位置ｘ，ｙ，ｚの３方向の内、１方向における先端位置を示している。ステップＳ６により、図６に示すような減衰振動するロボット先端位置のデータが時刻毎に作成される。このロボット先端位置のデータは、前記減速機の回転角度と前記減速機の出力軸の傾きに基づき求められたものである。 FIG. 6 is a graph showing the vibration state of the tip of the robot. The horizontal axis represents time t, and the vertical axis represents the tip position in one of the three tip positions x, y, z of the robot. By step S6, data of the robot tip position that undergoes damping vibration as shown in FIG. 6 is created at each time. The data of the robot tip position is obtained based on the rotation angle of the speed reducer and the inclination of the output shaft of the speed reducer.

次に、位置決め時間演算部４０４は、記憶部４０３から位置決め完了判定値を読み出す（Ｓ８）。そして、位置決め時間演算部４０４は、この値を、図６に示すように、検出対象動作の到達位置における各座標ｘ，ｙ，ｚの目標位置を中心値として正負に分布させ、位置決め完了幅（所定範囲）を設定する。 Next, the positioning time calculation unit 404 reads the positioning completion determination value from the storage unit 403 (S8). Then, as shown in FIG. 6, the positioning time calculation unit 404 distributes this value positively or negatively with the target position of each coordinate x, y, z at the arrival position of the detection target operation as the center value, and the positioning completion width ( (Predetermined range) is set.

次に、位置決め時間演算部４０４は、図６に示すようなロボット１００の先端位置の振動データから、目標位置に対し位置決め完了幅以内（所定範囲内）に収束する時間ｔ１を求める（Ｓ９：計時工程）。図６に示すように、ステップＳ１にて各入力側エンコーダ１０の検出角度を目標角度（教示点Ｐ２）にした時点を時刻Ｔ０とする。また、図６に示すように、ステップＳ３にて算出したロボット１００の先端の位置の位置決め完了位置に対する振れ幅が所定範囲内に収束する時点を時刻Ｔ１とする。位置決め時間演算部４０４は、時刻Ｔ０から時刻Ｔ１までの時間ｔ１を求める。つまり、ステップＳ９では、ステップＳ６で求めた先端位置が、ステップＳ８で読み出し設定した位置決め完了幅内に収束する時間ｔ１を確認する。 Next, the positioning time calculation unit 404 obtains the time t1 at which the target position converges within the positioning completion width (within the predetermined range) from the vibration data of the tip position of the robot 100 as shown in FIG. Process). As shown in FIG. 6, the time point at which the detection angle of each input side encoder 10 is set to the target angle (teaching point P2) in step S1 is time T0. Further, as shown in FIG. 6, a time point when the swing width of the position of the tip of the robot 100 calculated in step S3 with respect to the positioning completion position converges within a predetermined range is time T1. The positioning time calculation unit 404 obtains a time t1 from time T0 to time T1. That is, in step S9, the time t1 at which the tip position obtained in step S6 converges within the positioning completion width read and set in step S8 is confirmed.

次に、位置決め時間演算部４０４は、ステップＳ８にて求めた時間ｔ１を、パラメータ記憶部４０５に送信し格納することで、ロボット１００の停止時間（位置決め完了時間）に設定する（Ｓ１０：設定工程）。 Next, the positioning time calculation unit 404 sets the stop time (positioning completion time) of the robot 100 by transmitting and storing the time t1 obtained in step S8 in the parameter storage unit 405 (S10: setting step). ).

以上のステップＳ１〜Ｓ１０を実行して停止時間を設定した後は、組立物品の製造工程（連続運転）に移行する。つまり、ロボット１００に第１部品Ｗ１を把持させて第１動作を行い、予め設定した停止時間、ロボット１００を停止させた後に、第２動作にて第１部品Ｗ１を第２部品Ｗ２に組付けて組立物品を製造する。 After performing the above steps S1 to S10 and setting the stop time, the process proceeds to the manufacturing process (continuous operation) of the assembled article. That is, the robot 100 grips the first part W1 to perform the first operation, and after the robot 100 is stopped for a preset stop time, the first part W1 is assembled to the second part W2 in the second operation. To manufacture an assembled article.

以上のステップＳ１〜Ｓ１０の処理は、実際の連続運転時（組立作業時）に行ってもよいし、連続運転とは別に連続運転に先立って、連続運転時の動作と同様の動作を行う試運転時に行ってもよい。連続運転の場合は、停止時間を設定した後の次のロットの組立時から、設定した停止時間に基づいてロボット１００を制御すればよい。 The above-described processing of steps S1 to S10 may be performed during actual continuous operation (during assembly work), or a trial operation in which the same operation as the operation during continuous operation is performed prior to continuous operation separately from continuous operation. You may go at times. In the case of continuous operation, the robot 100 may be controlled based on the set stop time from the time of assembly of the next lot after setting the stop time.

試運転時の場合、ロボット１００のロボットハンド１０２には、実際の部品Ｗ１または部品Ｗ１に相当する治具を把持させて上記ステップＳ１〜Ｓ１０の処理を行うことで、教示点Ｐ２に動作させた場合におけるロボット１００の停止時間を設定すればよい。そして、連続運転時に設定した停止時間に基づいてロボット１００を制御すればよい。 In the case of trial operation, when the robot hand 102 of the robot 100 is operated to the teaching point P2 by gripping an actual component W1 or a jig corresponding to the component W1 and performing the processes of steps S1 to S10 The stop time of the robot 100 in 1 may be set. Then, the robot 100 may be controlled based on the stop time set during continuous operation.

このように、ロボットアーム１０１をティーチングプレイバック方式で動作させる場合、動作軌道には再現性がある。この性質を利用すると、次回以降の動作における位置決め完了幅内に収束する時間も略同一となる。よってこの時間を検出対象動作における位置決め完了時間と見なすことができる。 As described above, when the robot arm 101 is operated by the teaching playback method, the operation trajectory has reproducibility. If this property is used, the time to converge within the positioning completion width in the subsequent operations will be substantially the same. Therefore, this time can be regarded as the positioning completion time in the detection target operation.

次に、ロボット１００の各関節における出力側エンコーダ１６の配置について図７を用いて詳細を説明する。図７は、ロボット１００の関節Ｊ２部分の分解斜視図である。 Next, the arrangement of the output side encoder 16 in each joint of the robot 100 will be described in detail with reference to FIG. FIG. 7 is an exploded perspective view of the joint J2 portion of the robot 100.

図７に示すように、ロボット１００のＪ２関節部のリンク１２２側には出力側エンコーダ１６のセンサユニット５０７がＪ２関節の回転軸を中心とした同一円周上に略９０°の角度で２つ配置されている。なお、センサユニット５０７とＪ２関節の回転軸までの距離を予め計測しておけば、必ずしも同一円周上に配置する必要はない。 As shown in FIG. 7, on the side of the link 122 of the J2 joint of the robot 100, two sensor units 507 of the output side encoder 16 are provided on the same circumference centering on the rotation axis of the J2 joint at an angle of approximately 90 °. It is arranged. In addition, if the distance between the sensor unit 507 and the rotation axis of the J2 joint is measured in advance, it is not always necessary to arrange them on the same circumference.

次に図８を参照して、本実施例における出力側エンコーダ１６の構成について説明する。図８は、出力側エンコーダ１６の概略構成図である。出力側エンコーダ１６は、スケール５２０、２つのセンサユニット５０７（５０７ａ、５０７ｂ）、信号処理装置４０７、および、記憶装置４０８を備えて構成される。スケール５２０は測定対象物である可動部（図７ではベース部１２１）に取り付けられ、センサユニット５０７は固定部（図７ではリンク１２２）に取り付けられる。ただし本実施例はこれに限定されるものではなく、スケール５２０を固定部に取り付け、センサユニット５０７を可動部に取り付けてもよい。 Next, the configuration of the output side encoder 16 in this embodiment will be described with reference to FIG. FIG. 8 is a schematic configuration diagram of the output encoder 16. The output encoder 16 includes a scale 520, two sensor units 507 (507a and 507b), a signal processing device 407, and a storage device 408. The scale 520 is attached to a movable portion (base portion 121 in FIG. 7) that is an object to be measured, and the sensor unit 507 is attached to a fixed portion (link 122 in FIG. 7). However, the present embodiment is not limited to this, and the scale 520 may be attached to the fixed portion and the sensor unit 507 may be attached to the movable portion.

信号処理装置４０７は、センサユニット５０７で得られたエンコーダ信号（検出信号）の内挿処理、記憶装置４０８への信号の書き込み、記憶装置４０８からの信号の読み出し、位置信号の出力などを行う。なお、本実施例では反射型の光学式エンコーダについて説明するが、これに限定されるものではない。本実施例は、透過型のエンコーダにも適用可能である。 The signal processing device 407 performs interpolation processing of the encoder signal (detection signal) obtained by the sensor unit 507, writing of a signal to the storage device 408, reading of a signal from the storage device 408, output of a position signal, and the like. In addition, although a reflective optical encoder will be described in the present embodiment, the present invention is not limited to this. This embodiment is also applicable to a transmissive encoder.

センサユニット５０７は、ＬＥＤを有する光源５０１、および、フォトダイオードアレイ５０９を有する受光素子５０３を備える。また、センサユニット５０７は、フォトダイオードアレイ５０９で受光した光の光電変換などの信号処理を行う信号処理回路５０２ａを内蔵した半導体素子５０２（フォトＩＣチップ）を備える。ただし本実施例はこれに限定されるものではなく、このような信号処理回路５０２ａを受光素子５０３に内蔵してもよい。 The sensor unit 507 includes a light source 501 having an LED, and a light receiving element 503 having a photodiode array 509. Further, the sensor unit 507 includes a semiconductor element 502 (photo IC chip) having a built-in signal processing circuit 502a that performs signal processing such as photoelectric conversion of light received by the photodiode array 509. However, the present embodiment is not limited to this, and such a signal processing circuit 502a may be incorporated in the light receiving element 503.

光源５０１、半導体素子５０２、および受光素子５０３は、プリント基板５０４上に実装され、樹脂５０５を用いて封止されている。また、樹脂５０５の上には透明ガラス基板５０６が取り付けられている。このようにセンサユニット５０７は、これらの部材をパッケージ化して構成された受発光一体型のセンサユニットである。このように、フォトダイオードアレイ５０９は光源５０１と一体的に形成されており、スケール５２０に対して相対的に変位可能である。 The light source 501, the semiconductor element 502, and the light receiving element 503 are mounted on a printed board 504 and sealed with a resin 505. A transparent glass substrate 506 is attached on the resin 505. As described above, the sensor unit 507 is a light emitting / receiving integrated sensor unit configured by packaging these members. As described above, the photodiode array 509 is formed integrally with the light source 501 and can be displaced relative to the scale 520.

スケール５２０は、光源５０１からの光を反射（または透過）し、光源５０１に対して相対的に変位可能に構成されている。本実施の形態では反射型の方式について説明する。スケール５２０には、ガラス基板上にクロム反射膜をパターニングすることによりスケールトラック５０８が形成されている。センサユニット５０７はスケール５２０と対向するように配置されており、センサユニット５０７内の光源５０１から出射した発散光束は、スケール５２０のスケールトラック５０８に照射される。また、スケールトラック５０８から反射した光束は、センサユニット５０７内のフォトダイオードアレイ５０９に向けて反射される。フォトダイオードアレイ５０９上で、スケールトラック５０８からの反射率分布が２倍拡大された像として受光される。フォトダイオードアレイ５０９により受光された光束は電気信号に変換され（光電変換）、エンコーダ信号（検出信号）としてセンサユニット５０７から信号処理装置４０７へ送られる。 The scale 520 is configured to reflect (or transmit) light from the light source 501 and be relatively displaceable with respect to the light source 501. In this embodiment, a reflection type method will be described. On the scale 520, a scale track 508 is formed by patterning a chromium reflective film on a glass substrate. The sensor unit 507 is arranged so as to face the scale 520, and the divergent light flux emitted from the light source 501 in the sensor unit 507 is applied to the scale track 508 of the scale 520. The light flux reflected from the scale track 508 is reflected toward the photodiode array 509 in the sensor unit 507. On the photodiode array 509, the reflectance distribution from the scale track 508 is received as an image in which the reflectance distribution is doubled. The light flux received by the photodiode array 509 is converted into an electric signal (photoelectric conversion), and sent as an encoder signal (detection signal) from the sensor unit 507 to the signal processing device 407.

ここで、図９を参照して、オートパワーコントロール回路（ＡＰＣ回路）の動作の概略について説明する。５５０はフォトダイオードであり、前述のフォトダイオードアレイ５０９に相当する。５５１は増幅器である。ＲはＩ−Ｖ変換抵抗である。５４０は、ＡＰＣ用の利得Ａを有する差動増幅器である。５５２は、光源１を駆動する駆動回路を構成するＦＥＴ（電界効果トランジスタ）である。ＦＥＴ５５２のゲートに差動増幅器５４０の出力電圧が印加されることにより、光源５０１の駆動電流Ｉ_０が制御される。 Here, the outline of the operation of the automatic power control circuit (APC circuit) will be described with reference to FIG. 9. A photodiode 550 corresponds to the above-mentioned photodiode array 509. Reference numeral 551 is an amplifier. R is an IV conversion resistance. A differential amplifier 540 has a gain A for APC. Reference numeral 552 is an FET (field effect transistor) that constitutes a drive circuit that drives the light source 1. The drive voltage I ₀ of the light source 501 is controlled by applying the output voltage of the differential amplifier 540 to the gate of the FET 552.

光源５０１とフォトダイオード５５０（フォトダイオードアレイ５０９）は、図８に示されるように、スケール５２０を介して光学的に結合している。ここで、結合利得（駆動電流Ｉ_０に対するフォトダイオード５５０の出力電流Ｉ_ｐｄの比）をＫとし、ＡＰＣ用の差動増幅器５４０の基準電圧をＶｆ_３とする。このとき、差動増幅器５４０の利得Ａが十分に大きいとすると、駆動電流Ｉ_０は以下の式（１）のように表される。 The light source 501 and the photodiode 550 (photodiode array 509) are optically coupled via the scale 520, as shown in FIG. Here, the coupling gain (the ratio of the output current I _pd of the photodiode 550 to the drive current I ₀ ) is K, and the reference voltage of the APC differential amplifier 540 is Vf ₃ . At this time, assuming that the gain A of the differential amplifier 540 is sufficiently large, the drive current I ₀ is expressed by the following equation (1).

駆動電流Ｉ_０は、基準電圧Ｖｆ_３の増加とともに減少し、結合利得Ｋに反比例する。光源５０１の光量が減少した場合や、ゴミなどでフォトダイオード５５０に入力される光量が減少した場合には、結合利得Ｋが減少するため、駆動電流Ｉ_０が増加するように制御される。なお、ＡＰＣ回路は本方式に限られるものではなく、例えば抵抗を追加して基準電圧を設定する等の方法もある。 The drive current I ₀ decreases as the reference voltage Vf ₃ increases, and is inversely proportional to the coupling gain K. When the light amount of the light source 501 decreases or when the light amount input to the photodiode 550 due to dust or the like decreases, the coupling gain K decreases, so that the drive current I ₀ is controlled to increase. The APC circuit is not limited to this method, and there is a method of setting a reference voltage by adding a resistor, for example.

次に、出力側エンコーダの模式図である図１０を参照して、位置検出モード（Ｓ２）と傾き検出モードに（Ｓ４）について詳細に説明する。 Next, the position detection mode (S2) and the tilt detection mode (S4) will be described in detail with reference to FIG. 10 which is a schematic diagram of the output side encoder.

位置検出モード（Ｓ２）のときは、ロータリエンコーダが一般的に使用される方法と同様に、スケール５２０とセンサユニット５０７の相対回転をエンコーダ信号（検出信号）として検出する。それによりＺ軸周りの回転方向である回転角Ｒｚを検出する。 In the position detection mode (S2), the relative rotation between the scale 520 and the sensor unit 507 is detected as an encoder signal (detection signal), as in a method in which a rotary encoder is generally used. Thereby, the rotation angle Rz, which is the rotation direction around the Z axis, is detected.

傾き検出モード（Ｓ４）のＪ１〜Ｊ６の軸の傾きを計算（Ｓ５）方法について説明する。まず、位置検出モードで用いたオートパワーコントロール回路（ＡＰＣ回路）をスイッチングにより解除してからフォトダイオード５５０で受光した光量の変化を測定する。このとき、センサユニット５０７とスケール５２０の距離（ギャップ）に応じて受光量は変化する。ここで、ＡＰＣ回路は、自動的に光量調整され、ギャップ方向の距離測定の精度に影響を与えるためスイッチングにより解除している。光量とギャップの関係は予め測定して記憶させておく。 A method of calculating (S5) the tilts of the axes J1 to J6 in the tilt detection mode (S4) will be described. First, the automatic power control circuit (APC circuit) used in the position detection mode is released by switching, and then the change in the amount of light received by the photodiode 550 is measured. At this time, the amount of received light changes according to the distance (gap) between the sensor unit 507 and the scale 520. Here, the APC circuit automatically adjusts the light quantity and affects the accuracy of the distance measurement in the gap direction, so that the APC circuit is released by switching. The relationship between the light amount and the gap is measured and stored in advance.

測定された光量から２つのセンサユニット５０７（５０７ａ、５０７ｂ）それぞれのスケール５２０とのギャップ距離Ｌ１およびＬ２と、センサユニット５０７が配置されている同心円の直径の値よりスケールの傾きを幾何学的に計算して軸の傾き（θ）を求める。２つのセンサユニット５０７（５０７ａ、５０７ｂ）は関節の回転軸を中心とした同一円周上に９０°の角度で配置されているため２軸分の傾きを測定することができる。 From the measured light amount, the gap distances L1 and L2 with the scale 520 of each of the two sensor units 507 (507a, 507b) and the value of the diameter of the concentric circle where the sensor unit 507 is arranged are used to geometrically determine the slope of the scale. Calculate to obtain the inclination (θ) of the axis. Since the two sensor units 507 (507a, 507b) are arranged at an angle of 90 ° on the same circumference centered on the rotation axis of the joint, the tilts of the two axes can be measured.

算出した軸の傾き（θ）と、計算しておいたリンク長さ（Ｌ）とリンクの剛性値（Ｋ）の掛け算によりリンク先の位置（ΔＬ）が計算できる。 The link destination position (ΔL) can be calculated by multiplying the calculated axis inclination (θ) by the calculated link length (L) and the link rigidity value (K).

なお、光量のみならず、電流値や電圧値、エンコーダ信号の振幅情報からもギャップを測定することが可能であるため、それらを用いても構わない。 Note that the gap can be measured not only from the light amount but also from the current value, the voltage value, and the amplitude information of the encoder signal, and therefore these may be used.

以上説明したように、第１実施形態に係るロボット装置５００によれば、ロボット先端位置を、出力側エンコーダ１６の検出角度および傾きに基づいて算出することにより、先端の振動が収束する時間ｔ１を正確に求められるようになっている。 As described above, according to the robot apparatus 500 according to the first embodiment, by calculating the robot tip position based on the detection angle and the inclination of the output encoder 16, the time t1 at which the tip vibration converges is calculated. It is being demanded accurately.

そして、ティーチングプレイバック方式の動作再現性を利用することで、位置決め完了判定までの時間を短縮できる。よって、従来にあるような、減速機入力側の値を基とした方法と比較して、精度が保証される最速の位置決め完了条件を、正確に求めることが可能となる。 Then, by utilizing the operation reproducibility of the teaching playback method, it is possible to shorten the time until the determination of positioning completion. Therefore, as compared with the conventional method based on the value on the input side of the speed reducer, the fastest positioning completion condition that guarantees accuracy can be accurately obtained.

つまり、第１動作の始点となる教示点Ｐ１から終点となる教示点Ｐ２にロボット１００を動作させた時、ｔ１に設定した停止時間、ロボット１００を停止させることで、ロボット１００の先端の振動が収束する。よって、次の第２動作の始点となる教示点Ｐ２から第２動作の終点となる教示点Ｐ３へ移動するロボット１００の動作が安定する。したがって、部品Ｗ１を部品Ｗ２に組付ける組付け作業を行う際には、組み付け作業の精度が低下するのを抑制することができる。このように、設定した停止時間に亘ってロボット１００を停止させることでロボット１００の振動が収束するため、ロボット１００の作業精度が向上する。 That is, when the robot 100 is moved from the teaching point P1 which is the start point of the first operation to the teaching point P2 which is the end point, the robot 100 is stopped for the stop time set to t1 so that the vibration of the tip of the robot 100 is reduced. Converge. Therefore, the operation of the robot 100 that moves from the teaching point P2 that is the starting point of the next second operation to the teaching point P3 that is the ending point of the second operation is stable. Therefore, when the assembling work for assembling the component W1 with the component W2 is performed, it is possible to prevent the accuracy of the assembling work from decreasing. As described above, by stopping the robot 100 for the set stop time, the vibration of the robot 100 is converged, so that the working accuracy of the robot 100 is improved.

更に、設定する停止時間にマージンを多めに設ける必要がないので、従来よりも停止時間が短縮され、ロボット１００の作業効率が向上する。 Further, since it is not necessary to provide a large margin for the set stop time, the stop time is shortened as compared with the conventional case, and the work efficiency of the robot 100 is improved.

［第２実施形態］
図１１は、本発明の第２実施形態に係るロボット装置のＪ２関節を示す分解斜視図である。なお、第２実施形態のロボット装置の全体構成は、第１実施形態のロボット装置５００と同様であるため説明を省略し、第１実施形態と異なる点を中心に説明する。第２実施形態において、第１実施形態と異なるのは、出力側エンコーダ１６のセンサユニット５０７の配置である。 [Second Embodiment]
FIG. 11 is an exploded perspective view showing a J2 joint of the robot apparatus according to the second embodiment of the present invention. The overall configuration of the robot apparatus according to the second embodiment is the same as that of the robot apparatus 500 according to the first embodiment, and therefore a description thereof will be omitted, and points different from the first embodiment will be mainly described. The second embodiment is different from the first embodiment in the arrangement of the sensor unit 507 of the output side encoder 16.

第２実施形態では、ロボット１００のＪ２関節部のリンク１２２側に出力側エンコーダ１６のセンサユニット５０７がＪ２関節の回転軸を中心として、リンク１２２の軸線上（Ｊ２関節とＪ３関節を結ぶ線）に沿って２つ対向配置されている。換言すると、ロボット１００のＪ２関節部のリンク１２２側には出力側エンコーダ１６のセンサユニット５０７がＪ２の回転軸を中心とした同一円周上に略１８０°の角度で２つ配置されている。 In the second embodiment, the sensor unit 507 of the output side encoder 16 is located on the link 122 side of the J2 joint of the robot 100 on the axis of the link 122 (the line connecting the J2 joint and the J3 joint) about the rotation axis of the J2 joint. Two are arranged facing each other. In other words, two sensor units 507 of the output side encoder 16 are arranged on the link 122 side of the J2 joint portion of the robot 100 at an angle of about 180 ° on the same circumference centered on the rotation axis of J2.

ロボット１００を動作させると図１１の矢印Ｖ方向に力が働く。このとき、Ｊ２軸にはねじり方向Ｔと撓み方向Ｕの力が働く。しかし、リンク１２２は円筒パイプや角型パイプ形状で形成されるため、ねじり方向Ｔの剛性は、撓み方向Ｕの剛性に比べて十分に大きい。そのため変形はリンク１２２の撓み方向Ｕが支配的になる。 When the robot 100 is operated, a force acts in the direction of arrow V in FIG. At this time, forces in the twisting direction T and the bending direction U act on the J2 axis. However, since the link 122 is formed in the shape of a cylindrical pipe or a rectangular pipe, the rigidity in the twisting direction T is sufficiently higher than the rigidity in the bending direction U. Therefore, the deformation is dominated by the bending direction U of the link 122.

よってリンク１２２の軸線上（Ｊ２関節とＪ３関節を結ぶ線）に沿って２つ対向配置することにより先端位置を計算することが可能となる。 Therefore, it is possible to calculate the tip position by disposing two facing each other along the axis of the link 122 (the line connecting the J2 joint and the J3 joint).

また、リンク軸の撓みをあらかじめ計算し外力に対する変位と傾きの関係を記憶しておき比較することにより、センサユニット５０７が一つでも同様の効果が得られる。 Further, by calculating the deflection of the link shaft in advance and storing the relationship between the displacement and the inclination with respect to the external force and comparing them, the same effect can be obtained even with one sensor unit 507.

［第３実施形態］
第３実施形態のロボット装置の全体構成は、第１実施形態のロボット装置５００と同様であるため説明を省略し、第１実施形態と異なる点を中心に説明する。第３実施形態では、ロボット１００に第１動作を行わせて教示点Ｐ２に到達した時点から、ロボット１００の先端の振動が所定の範囲に収束する時間を推定して、推定した時間、一時停止させた後に、次の第２動作を実行するようにしている。 [Third Embodiment]
The overall configuration of the robot apparatus according to the third embodiment is the same as that of the robot apparatus 500 according to the first embodiment, and therefore the description thereof will be omitted, and the points different from the first embodiment will be mainly described. In the third embodiment, from the time when the robot 100 is caused to perform the first motion and reaches the teaching point P2, the time when the vibration of the tip of the robot 100 converges within a predetermined range is estimated, and the estimated time is temporarily stopped. After that, the following second operation is executed.

図１２は、第３実施形態に係るロボット装置５００の制御系を示す機能ブロック図である。第３実施形態に係るロボット装置５００の制御系は、位置決め完了判定値記憶部４０３、位置決め時間演算部４０４、パラメータ記憶部４０５の替わりに、収束判定値記憶部４０９および収束判定部４１０を備えるという点が第１実施形態と異なる。 FIG. 12 is a functional block diagram showing a control system of the robot apparatus 500 according to the third embodiment. The control system of the robot apparatus 500 according to the third embodiment includes a convergence determination value storage unit 409 and a convergence determination unit 410 instead of the positioning completion determination value storage unit 403, the positioning time calculation unit 404, and the parameter storage unit 405. The point is different from the first embodiment.

収束判定値記憶部４０９は、ロボット１００の先端位置における、位置決め完了位置として許容できるずれ量の幅（即ち、振動が収束したと見做せる許容範囲、所定の範囲）を記憶する。 The convergence determination value storage unit 409 stores a width of a deviation amount that is allowable as a positioning completion position at the tip position of the robot 100 (that is, an allowable range in which it can be considered that vibration has converged, a predetermined range).

収束判定部４１０は、先端位置演算部４０２および収束判定値記憶部４０９の値に基づき、ロボット１００の先端の収束（位置決め完了）を判定し、次動作へ移行する指令をサーボ制御装置２３０に与える。 The convergence determination unit 410 determines the convergence (positioning completion) of the tip of the robot 100 based on the values of the tip position calculation unit 402 and the convergence determination value storage unit 409, and gives the servo controller 230 a command to move to the next operation. .

図１３は、本発明の第３実施形態に係るロボット装置におけるロボット制御方法（収束時間検出方法）の各工程を示すフローチャートである。第１実施形態におけるロボット制御方法のＳ１〜Ｓ６と、第３実施形態におけるＳ１１〜１６は同じである。 FIG. 13 is a flowchart showing each step of the robot control method (convergence time detection method) in the robot apparatus according to the third embodiment of the present invention. S1 to S6 of the robot control method in the first embodiment and S11 to 16 in the third embodiment are the same.

ここで、ステップＳ１７の推定工程について具体的に説明する。図１４は、本発明の第３実施形態に係るロボット制御方法における推定工程を示すフローチャートである。図１５は推定工程を説明するための概念図である。図１５のグラフにおいて、横軸は時間ｔ、縦軸は先端位置ｘ，ｙ，ｚの３軸方向のうち１軸方向（ｘ軸方向）の先端位置ｘを示している。なお、他の軸（ｙ，ｚ軸）についても同様であるので、説明を省略する。 Here, the estimation process of step S17 will be specifically described. FIG. 14 is a flowchart showing an estimation step in the robot control method according to the third embodiment of the present invention. FIG. 15 is a conceptual diagram for explaining the estimation process. In the graph of FIG. 15, the horizontal axis represents time t, and the vertical axis represents the tip position x in one axial direction (x-axis direction) of the three axial directions x, y, and z. Since the same applies to the other axes (y, z axes), the description thereof will be omitted.

図１５に示すように、ロボット１００の先端位置は、目標位置ｘ０を中心とする所定範囲（２本の破線の間の許容範囲）の内外を往復しながら徐々に減衰して目標位置ｘ０に収束する。よって、第３実施形態では、ロボット１００の振動で収束幅を逸脱しない時間ｔｘを推定する。 As shown in FIG. 15, the tip position of the robot 100 gradually attenuates while reciprocating in and out of a predetermined range (allowable range between two broken lines) centered on the target position x0 and converges to the target position x0. To do. Therefore, in the third embodiment, the time tx at which the vibration of the robot 100 does not deviate from the convergence width is estimated.

具体的に説明すると、まず、ＣＰＵ２０１は、ステップＳ１６にて算出したロボット１００の先端の位置ｘの時間変化によりロボット１００の先端の振動の少なくとも２つ、本実施形態では２つのピーク値ｘ１，ｘ２を算出する（Ｓ１７１）。具体的に説明すると、ＣＰＵ２０１は、所定の時間間隔で各出力側エンコーダ１６から各関節Ｊ１〜Ｊ６の回転角度と軸の傾きを取得して先端位置を求めており、先端位置が各ピーク値ｘ１，ｘ２を過ぎてｘ０に近づいた時点で各ピーク値ｘ１，ｘ２を割り出す。即ち、ＣＰＵ２０１は、時系列で先に割り出した少なくとも２つのピーク値ｘ１，ｘ２を求める。 More specifically, first, the CPU 201 causes at least two of the vibrations at the tip of the robot 100, that is, two peak values x1 and x2 in the present embodiment due to the time change of the position x of the tip of the robot 100 calculated in step S16. Is calculated (S171). More specifically, the CPU 201 obtains the tip position by obtaining the rotation angles and the inclinations of the axes of the joints J1 to J6 from the output encoders 16 at predetermined time intervals, and the tip position is the peak value x1. , X2 and approaching x0, the peak values x1 and x2 are calculated. That is, the CPU 201 obtains at least two peak values x1 and x2 that have been previously calculated in time series.

次に、ＣＰＵ２０１は、ピーク値ｘ１，ｘ２に基づき、収束時間ｔｘを推定する。具体的に説明すると、まず、ＣＰＵ２０１は、ピーク値ｘ２を割り出した後、２つのピーク値ｘ１，ｘ２から対数減衰率を算出する（Ｓ１７２）。対数減衰率は、隣り合う振幅の比の自然対数で表される。図１５のロボットアーム先端位置の波形においては、対数減衰率σは以下の式で表される。 Next, the CPU 201 estimates the convergence time tx based on the peak values x1 and x2. More specifically, the CPU 201 first calculates the peak value x2, and then calculates the logarithmic attenuation rate from the two peak values x1 and x2 (S172). The logarithmic decay rate is represented by the natural logarithm of the ratio of adjacent amplitudes. In the waveform of the robot arm tip position in FIG. 15, the logarithmic decay rate σ is represented by the following equation.

次いで、ＣＰＵ２０１は、算出した対数減衰率から収束時間ｔｘを算出する（Ｓ１７３）。 Next, the CPU 201 calculates the convergence time tx from the calculated logarithmic decay rate (S173).

以上のステップＳ１７１〜Ｓ１７３により、収束時間ｔｘが推定される。収束時間ｔｙ，ｔｚについても同様に推定する。なお、推定方法として対数減衰率を求める場合について説明したが、これに限定するものではない。先端位置の複数のサンプリングデータから減衰波形の関数を求めて、収束時間を推定してもよい。また、２つのピーク値から次以降のピーク値及びそのタイミングを推定して、収束時間を推定してもよい。 The convergence time tx is estimated by the above steps S171 to S173. The convergence times ty and tz are similarly estimated. Although the case where the logarithmic attenuation rate is obtained has been described as the estimation method, the present invention is not limited to this. The convergence time may be estimated by obtaining a function of the attenuation waveform from a plurality of sampling data of the tip position. Further, the convergence time may be estimated by estimating the next peak value and its timing from the two peak values.

このように、第３実施形態では、ＣＰＵ２０１は、ワールド座標系Σにおける各ｘ，ｙ，ｚ軸方向について各々収束時間ｔｘ，ｔｙ，ｔｚを推定する。 As described above, in the third embodiment, the CPU 201 estimates the convergence times tx, ty, and tz in the x, y, and z axis directions in the world coordinate system Σ, respectively.

次に、図１３においてステップＳ１７で収束時間ｔｘ，ｔｙ，ｔｚの推定を終えた後、収束判定部４１０は、収束時間ｔｘ，ｔｙ，ｔｚが経過したか否かを判断する（Ｓ１８）。具体的には、収束判定部４１０は、収束時間ｔｘ，ｔｙ，ｔｚのうち、最も長い収束時間（第３実施形態では収束時間ｔｘ）が経過したか否かを判断する。 Next, in FIG. 13, after the estimation of the convergence times tx, ty, tz is completed in step S17, the convergence determination unit 410 determines whether or not the convergence times tx, ty, tz have passed (S18). Specifically, the convergence determination unit 410 determines whether or not the longest convergence time (convergence time tx in the third embodiment) of the convergence times tx, ty, and tz has elapsed.

収束判定部４１０は、タイマ２０６による計時が収束時間ｔｘを経過していなければ（Ｓ１８：Ｎｏ）、そのままロボット１００の各モータ１の一時停止を続行する。収束判定部４１０は、タイマ２０６による計時が収束時間ｔｘに達したと判断した場合には（Ｓ１８：Ｙｅｓ）、組立物品の製造工程（連続運転）に移行する。つまり、ロボット１００に第１部品Ｗ１を把持させて第１動作を行い、予め設定した停止時間、ロボット１００を停止させた後に、第２動作にて第１部品Ｗ１を第２部品Ｗ２に組付けて組立物品を製造する。 If the time measured by the timer 206 has not passed the convergence time tx (S18: No), the convergence determination unit 410 continues the temporary stop of each motor 1 of the robot 100. When the convergence determination unit 410 determines that the time measured by the timer 206 has reached the convergence time tx (S18: Yes), the process proceeds to the manufacturing process (continuous operation) of the assembled article. That is, the robot 100 grips the first part W1 to perform the first operation, and after the robot 100 is stopped for a preset stop time, the first part W1 is assembled to the second part W2 in the second operation. To manufacture an assembled article.

第３実施形態によれば、少なくともステップＳ１７で推定した収束時間の分、各モータ１の回転動作を一時停止させるので、予め定めた所定時間の分、各モータ１の回転動作を一時停止させるよりも一時停止させておく時間を短縮できる。そして、少なくとも収束時間の分、ロボットの各モータの回転動作を停止させておけば、ロボット１００の先端の振動は許容範囲に収束するので、次の第２動作におけるロボット１００の動作精度を確保することができる。そして、所定時間とする場合に比べて余分な停止時間を削減することができるので、第１動作及び第２動作の一連のロボット１００の動作が短時間となり、ロボット１００による組立部品の組立作業の効率が向上する。 According to the third embodiment, the rotation operation of each motor 1 is temporarily stopped for at least the convergence time estimated in step S17. Therefore, the rotation operation of each motor 1 is temporarily stopped for a predetermined time. Also, the time to suspend can be shortened. Then, if the rotation operation of each motor of the robot is stopped for at least the convergence time, the vibration of the tip of the robot 100 converges within the allowable range, so that the operation accuracy of the robot 100 in the next second operation is ensured. be able to. Since the extra stop time can be reduced as compared with the case of setting the predetermined time, the series of operations of the robot 100 of the first operation and the second operation becomes short, and the assembly work of the assembly parts by the robot 100 is shortened. Efficiency is improved.

また、第３実施形態では、ロボット１００の先端の振動のピーク値を少なくとも２つ割り出して収束時間を推定しているので、次以降のピーク値を減衰特性に基づいて容易に求めることができ、推定される収束時間の精度がより向上する。特に、対数減衰率を計算することで、２つのピーク値だけで済み、より迅速に収束時間を求めることができる。 Further, in the third embodiment, at least two peak values of the vibration of the tip of the robot 100 are calculated and the convergence time is estimated, so that it is possible to easily obtain the next and subsequent peak values based on the damping characteristics. The accuracy of the estimated convergence time is further improved. In particular, by calculating the logarithmic decay rate, only two peak values are needed, and the convergence time can be obtained more quickly.

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。 Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments. Further, the effects described in the embodiments of the present invention only list the most suitable effects that result from the present invention, and the effects according to the present invention are not limited to those described in the embodiments of the present invention.

例えば、上記実施形態においては、多関節ロボット１００が、垂直多関節ロボットである場合について説明したが、水平多関節ロボット（スカラロボット）、パラレルリンクロボットなどであってもよい。 For example, in the above embodiment, the case where the articulated robot 100 is a vertical articulated robot has been described, but it may be a horizontal articulated robot (scalar robot), a parallel link robot, or the like.

また、上記実施形態の各処理動作は具体的には制御装置２００のＣＰＵ２０１により実行されるものである。従って、上述した機能を実現するプログラムを記録した記録媒体を制御装置２００に供給し、制御装置２００のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。 Further, each processing operation of the above embodiment is specifically executed by the CPU 201 of the control device 200. Therefore, it is achieved by supplying a recording medium recording a program that realizes the above-described functions to the control device 200 and causing a computer (CPU or MPU) of the control device 200 to read and execute the program stored in the recording medium. You may In this case, the program itself read from the recording medium realizes the functions of the above-described embodiments, and the program itself and the recording medium recording the program constitute the present invention.

また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム２４０が格納される場合について説明したが、これに限定するものではない。プログラム２４０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３に示すＲＯＭ２０２、外部記憶装置３１２、記録ディスク２４１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。 Further, in the above embodiment, the case where the computer-readable recording medium is the HDD 204 and the program 240 is stored in the HDD 204 has been described, but the present invention is not limited to this. The program 240 may be recorded in any recording medium as long as it is a computer-readable recording medium. For example, as the recording medium for supplying the program, the ROM 202, the external storage device 312, the recording disk 241 or the like shown in FIG. 3 may be used. Explaining with a specific example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a CD-ROM, a CD-R, a magnetic tape, a non-volatile memory card, a ROM, or the like can be used as the recording medium. Further, the program in the above embodiment may be downloaded via a network and executed by a computer.

また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。 The functions of the above-described embodiments are not limited to being realized by executing the program code read by the computer. An OS (operating system) or the like running on a computer may perform a part or all of actual processing based on the instructions of the program code, and the processing may realize the functions of the above-described embodiments. .

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。 Furthermore, the program code read from the recording medium may be written in a memory provided in a function expansion board inserted in the computer or a function expansion unit connected to the computer. This also includes a case where the CPU or the like included in the function expansion board or the function expansion unit performs some or all of the actual processing based on the instructions of the program code, and the processing realizes the functions of the above-described embodiments.

１１減速機
１６出力側エンコーダ（ロータリエンコーダ）
１００ロボット（多関節ロボット）
１１０関節駆動装置
２００制御装置（制御手段）
２０４ＨＤＤ（記憶手段）
２４０プログラム
５００ロボット装置
５０７センサユニット
５２０スケール 11 Reducer 16 Output encoder (rotary encoder)
100 robots (articulated robots)
110 joint drive device 200 control device (control means)
204 HDD (storage means)
240 program 500 robot device 507 sensor unit 520 scale

Claims

A joint drive device provided in each joint that drives each joint of the multi-joint robot has a reduction gear and an encoder that detects a rotation angle of the reduction gear, respectively.
A robot control method in which the control means executes a first operation for reaching the second position from the first position and then stops the multi-joint robot for a stop time, and then executes the second operation. There
The stop time is
From the time when the multi-joint robot reaches the second position in the first motion, the multi-joint is calculated based on the rotation angle of the speed reducer detected by each encoder and the inclination of the output shaft of the speed reducer. A robot control method comprising: a time until a swing width of a position of a tip of the robot converges within a predetermined range.

The encoder is an optical rotary encoder having a scale and a sensor,
The robot control method according to claim 1, wherein the inclination of the output shaft is calculated from a measurement value of a gap between the scale and the sensor.

The rotary encoder has at least two sensors,
The robot control method according to claim 2, wherein the at least two sensors are arranged at an angle of 90 ° on the same circumference centered on the rotation axis of the joint drive device.

The rotary encoder has at least two sensors,
The robot control method according to claim 2, wherein the at least two sensors are arranged at an angle of 180 ° on the same circumference centered on a rotation axis of the joint drive device.

The robot control method according to any one of claims 1 to 4, wherein the stop time is stored in a storage unit.

The robot control method according to any one of claims 1 to 4, wherein the control unit estimates a time until a swing width of a position of a tip of the articulated robot converges within a predetermined range.

A joint drive device that has a reduction gear and an encoder that detects a rotation angle of the reduction gear, and is provided to each joint that drives each joint of the articulated robot;
Control means for causing the articulated robot to perform a first operation from a first position to a second position, and then stopping the articulated robot for a stop time, and then performing a second operation;
A robot apparatus having:
The control means is
The stop time is calculated based on the rotation angle of the speed reducer detected by the encoder and the inclination of the output shaft of the speed reducer from the time when the articulated robot reaches the second position in the first motion. A robot apparatus comprising: a time until the swing width of the position of the tip of the multi-joint robot converges within a predetermined range.

A program for causing a computer to execute the robot control method according to any one of claims 1 to 6.

A computer-readable recording medium in which the program according to claim 8 is recorded.

After the articulated robot grips the first part and performs the first operation from the first position to the second position, the articulated robot is stopped for a stop time, and then the first part is operated in the second operation. A method of manufacturing an article, comprising:
The multi-joint robot has a joint drive device provided in each joint for driving each joint,
The joint drive device includes a speed reducer and an encoder that detects a rotation angle of the speed reducer, respectively.
The stop time is
From the time when the multi-joint robot reaches the second position in the first motion, the multi-joint is calculated based on the rotation angle of the speed reducer detected by each encoder and the inclination of the output shaft of the speed reducer. A method for manufacturing an article, comprising: a time until a swing width of a position of a tip of the robot converges within a predetermined range.