JP7186571B2

JP7186571B2 - Apparatus and method for determining operation completion time of robot

Info

Publication number: JP7186571B2
Application number: JP2018191367A
Authority: JP
Inventors: 正志花岡
Original assignee: Nidec Sankyo Corp
Current assignee: Nidec Instruments Corp
Priority date: 2018-10-10
Filing date: 2018-10-10
Publication date: 2022-12-09
Anticipated expiration: 2038-10-10
Also published as: KR20200040663A; CN111015627A; CN111015627B; KR102294070B1; JP2020059082A

Description

本発明は、アームを備えるロボットがその指令された位置に到達した時刻である動作完了時刻を判定する装置及び方法に関する。 The present invention relates to an apparatus and method for determining a motion completion time, which is the time when a robot with an arm reaches its commanded position.

例えば液晶パネルの製造に用いられるガラス基板等のワークは、アームの先端にハンドを備えたロボットを用いて、ハンドの上に保持されることによって搬送される。この搬送の際、種々の要因でハンドやアームが振動することがある。特に水平多関節ロボットの場合、アームを伸展することによってワークを搬送して搬送先の所定位置に配置するので、アームを搬送先に向けて伸展させたのちの残留振動が収束するまでの時間が長いと、ワークを所定位置に配置する際に位置ずれが生じてしまう、あるいは待機時間が長くなる分、作業効率を低下させてしまう懸念がある。そのため、ロボットにおけるアームまたはハンドの残留振動の収束時間についての評価が行われる。残留振動の収束時間の判定は、例えば、ハンドに加速度センサを取り付け、ロボットに対する指令に応じたアームまたはハンドの伸展の動作が完了してから規定時間後のハンドの振動振幅を計測することによって行われる。アームまたはハンドの伸展の動作とは、ワークを搬送位置に位置決めするためにアームまたはハンドを駆動する動作のことである。そしてロボットの動作完了時刻とは、ワークを搬送位置に位置付けるためにアームまたはハンドを駆動する動作が完了した時刻のことである。 For example, a work such as a glass substrate used for manufacturing a liquid crystal panel is transported by being held on a hand of a robot having a hand at the tip of an arm. During this transportation, the hands and arms may vibrate due to various factors. Especially in the case of horizontal articulated robots, the workpiece is transported and placed at a predetermined position at the destination by extending the arm, so the time required for the residual vibration to converge after the arm is extended toward the destination. If it is long, there is a concern that the workpiece may be displaced when it is placed at a predetermined position, or that the work efficiency will be reduced due to the longer waiting time. Therefore, the convergence time of the residual vibration of the arm or hand of the robot is evaluated. The judgment of the convergence time of the residual vibration is performed, for example, by attaching an acceleration sensor to the hand and measuring the vibration amplitude of the hand after a specified time has elapsed since the extension of the arm or hand in response to a command to the robot is completed. will be The action of extending the arm or hand is the action of driving the arm or hand in order to position the workpiece at the transfer position. The operation completion time of the robot is the time when the operation of driving the arm or hand to position the workpiece at the transfer position is completed.

残留振動の収束時間の評価を行うためには、ロボットの動作完了時刻を正確に判定できることが必要である。ロボットの動作完了時刻の判定では、例えば、アームやハンドに取り付けた加速度センサからのデータをメモリに蓄積し、蓄積したデータのうちアームやハンドの進行方向成分のデータに着目し、振動や雑音が重畳しているこのデータの包絡線の中心線の値がゼロとなった時刻を動作完了時刻としている。 In order to evaluate the convergence time of residual vibration, it is necessary to be able to accurately determine the operation completion time of the robot. In the determination of robot operation completion time, for example, data from acceleration sensors attached to arms and hands are accumulated in memory, and among the accumulated data, the components in the movement direction of the arms and hands are focused on, and vibration and noise are detected. The time when the value of the central line of the envelope of the superimposed data becomes zero is defined as the operation completion time.

本発明に関連するものとして、特許文献１は、ロボットのテスト運転を行って残留振動の周波数と移動時間別の残留振動の大きさとを測定し、振動の大きさが最小となる最短の移動時間を求めてロボットの位置決めを行うことを開示している。特許文献２は、ロボットの手先に加速度計を設け、加速度計の出力をオシロスコープで観測してロボットの実加速度を求め、ロボットの動作時間を予測することを開示している。特許文献３は、アームのたわみが無視できないときの位置決め方法として、アームの先端に加速度センサを設け、加速度センサの出力をローパスフィルタで処理し、ローパスフィルタで処理された後の加速度センサの出力値に基づいてアームのたわみ量を演算し、演算されたたわみ量に基づいてアームを駆動することを開示している。 As related to the present invention, Patent Document 1 discloses that a test operation of a robot is performed to measure the frequency of residual vibration and the magnitude of residual vibration for each movement time, and the shortest movement time that minimizes the magnitude of vibration. It is disclosed to position the robot by obtaining the . Patent Document 2 discloses that an accelerometer is provided at the hand of a robot, the output of the accelerometer is observed with an oscilloscope, the actual acceleration of the robot is obtained, and the operating time of the robot is predicted. Patent document 3 discloses a positioning method when the deflection of the arm cannot be ignored. An acceleration sensor is provided at the tip of the arm, the output of the acceleration sensor is processed with a low-pass filter, and the output value of the acceleration sensor after being processed with the low-pass filter. and to drive the arm based on the calculated deflection amount.

特表２００３－５１７１６７号公報Japanese Patent Publication No. 2003-517167 特開平４－２６０１０２号公報JP-A-4-260102 特開平１－１７３１１６号公報JP-A-1-173116

ロボットの動作完了時刻を求める従来の方法は、アームまたはハンドに取り付けられた加速度データを蓄積して加速度波形として、出力された加速度波形に基づいて目視によって動作完了時刻を求めている。加速度波形には振動などが重畳しており、これらの影響によって、目視で動作完了時刻を決定した場合には人為的な誤差やばらつきが発生する。その結果、振幅の変化を正確に測定できたとしても残留振動の起点である動作完了時刻を正確に決定できていないことになるので、残留振動の収束時間を正しく評価できなくなる。また、目視での判定であるので作業効率もよくない。 A conventional method for determining the operation completion time of a robot accumulates acceleration data attached to an arm or hand to obtain an acceleration waveform, and visually determines the operation completion time based on the outputted acceleration waveform. Vibrations and the like are superimposed on the acceleration waveform, and due to these influences, human errors and variations occur when the operation completion time is determined visually. As a result, even if the change in amplitude can be accurately measured, the operation completion time, which is the starting point of the residual vibration, cannot be determined accurately, so the convergence time of the residual vibration cannot be evaluated correctly. In addition, since the determination is made by visual observation, work efficiency is not good.

本発明の目的は、人為的な誤差の要因を排除し、高い作業効率でロボットの動作完了時刻を正確に決定することができる、ロボットの動作完了時刻の判定装置及び判定方法を提供することにある。 SUMMARY OF THE INVENTION It is an object of the present invention to provide an apparatus and method for determining a robot operation completion time, which eliminates the factor of human error and can accurately determine the robot operation completion time with high work efficiency. be.

本発明の判定装置は、指令された位置へのロボットの移動動作の動作完了時刻を判定する判定装置であって、ロボットのアームまたはハンドの速度または加速度を検出する動作検出手段と、動作検出手段からの信号に含まれる振動成分を除去して平滑化データを生成するフィルタ手段と、を備え、フィルタ手段は、位相遅れを有しないフィルタによって構成されている。 A determination device of the present invention is a determination device for determining a motion completion time of a movement motion of a robot to a commanded position, and comprises motion detection means for detecting the velocity or acceleration of an arm or hand of the robot, and motion detection means. and filter means for removing an oscillating component contained in the signal from the source to generate smoothed data, the filter means being composed of a filter having no phase delay.

本発明の判定装置では、速度または加速度を検出した信号から、位相遅れを有しないフィルタを用いて振動成分を除去するので、位相遅れの補償などを行うことなく、平滑化データによって表される波形から動作完了時刻を容易に識別することができる。なお、以下の説明において、ロボットのアームまたはハンドの減速のことを「ロボットの減速」と呼ぶ。加速度を検出した信号からはその符号により、速度を検出した信号からはその信号を微分して符号を判定することにより、ロボットが減速中であるか否かを判定することができる。 In the determination device of the present invention, the vibration component is removed from the signal obtained by detecting the velocity or acceleration using a filter that does not have a phase delay. , the operation completion time can be easily identified. In the following description, deceleration of the arm or hand of the robot will be referred to as "deceleration of the robot". Whether or not the robot is decelerating can be determined by determining the sign of the acceleration detection signal and by differentiating the signal from the speed detection signal.

本発明の判定装置では、動作検出手段は、ロボットのアームまたはハンドに取り付けられた加速度センサとすることができる。加速度センサによって加速度を検出する構成とすることにより、大掛かりな装置などを必要とせずに簡単に動作完了時刻を判別できるようになる。 In the determination device of the present invention, the motion detection means can be an acceleration sensor attached to the arm or hand of the robot. By adopting a configuration in which acceleration is detected by an acceleration sensor, it becomes possible to easily determine the operation completion time without requiring a large-scale device or the like.

本発明の判定装置のうちの第１の判定装置は、動作検出手段によりロボットが減速中であることを検出した後に平滑化データの値が最後に所定の閾値範囲外の値からこの閾値範囲内の値となったとき以降の平滑化データの値の平均値を求め、平滑化データの値から平均値を差し引くことによりオフセットとドリフトの補正を行う演算部を有する。このような演算部を設けることにより、速度または加速度の検出におけるオフセットとドリフトの影響を除去できて、より正確な動作完了時刻を得ることができる。 In the first determination device among the determination devices of the present invention, after the motion detection means detects that the robot is decelerating, the value of the smoothed data finally changes from a value outside the predetermined threshold range to within the threshold range. It has a calculation unit that calculates the average value of the smoothed data values after the value of , and subtracts the average value from the smoothed data values to correct the offset and drift. By providing such a computing unit, the effects of offset and drift in detecting velocity or acceleration can be eliminated, and more accurate operation completion time can be obtained.

第１の判定装置においては、演算部は、動作完了時刻であると推定される時刻の近傍において、オフセットとドリフトの補正を行った平滑化データのうち動作完了時刻であると推定される時刻以前の期間の平滑化データを近似対象平滑化データとして時刻の関数で近似し、その関数の値がゼロとなる時刻を動作完了時刻として判定することが好ましい。関数近似の手法を採用することにより、平滑化データに残存する振動成分の影響を除去してさらに正確に動作完了時刻を決定することができる。さらに本発明の判定装置では、演算部を設けずにオフセットとドリフトの補正を明示的に行わない場合（第２の判定装置の場合）においても、関数近似の手法を採用することができる。オフセットとドリフトの補正を明示的に行わない場合には、動作検出手段によりロボットが減速中であることを検出した後に平滑化データの値が最後に所定の閾値範囲外の値からこの閾値範囲内の値となったときを基準時として、基準時以降の平滑化データを定数値で近似し、基準時以前の平滑化データを近似対象平滑化データとして時刻の関数で近似し、この関数の値が、基準時以降の平滑化データを近似する定数値となる時刻を動作完了時刻として判定する。 In the first determination device, in the vicinity of the time estimated to be the operation completion time, the calculation unit selects the smoothed data corrected for the offset and the drift before the time estimated to be the operation completion time. It is preferable to approximate the smoothed data in the period of (1) as the smoothed data to be approximated by a function of time, and determine the time when the value of the function becomes zero as the operation completion time. By adopting the function approximation method, it is possible to remove the influence of the vibration component remaining in the smoothed data and determine the operation completion time more accurately. Furthermore, in the determination device of the present invention, even when the offset and drift are not explicitly corrected without providing the calculation unit (in the case of the second determination device) , the method of function approximation can be adopted. can. If offset and drift correction is not explicitly performed, the value of the smoothed data finally changes from a value outside the predetermined threshold range to within the threshold range after the motion detection means detects that the robot is decelerating. Using the time when the value of is a constant value approximating the smoothed data after the reference time is determined as the operation completion time.

関数近似の関数としては、例えば１次関数や２次関数がある。１次関数で近似するときは、近似対象平滑化データにおいてゼロ加速度検出用の閾値に対応する点と任意定数を用いて決定される１つの点とを選択して、１次関数が表す直線がこれらの２点を通るように１次関数を決定するようにすることができる。１次関数での近似には、演算量が少なくてすみ、また任意定数の選択において経験や実験結果を反映させることで、より高い精度で動作完了時刻を決定できるという利点がある。一方、２次関数で近似するときは、近似対象平滑化データにおいて上述のゼロ加速度検出用の閾値に対応する点とロボットの減速での極値に対応する点とを２次関数が表す放物線が通るようにして、２次関数を決定することができる。２次関数での近似では、任意定数に基づく点を選択しないので、経験や実験結果がない場合においても近似関数を決定でき、正確な動作完了時刻を求めることができる。 Functions for function approximation include, for example, linear functions and quadratic functions. When approximating with a linear function, a point corresponding to the threshold value for zero acceleration detection and one point determined using an arbitrary constant are selected in the smoothed data to be approximated, and the straight line represented by the linear function is A linear function can be determined so as to pass through these two points. The approximation with a linear function has the advantage that the operation completion time can be determined with higher accuracy by reducing the amount of calculation and by reflecting experience and experimental results in the selection of arbitrary constants. On the other hand, when approximating with a quadratic function, a parabola representing a point corresponding to the above-mentioned threshold value for detecting zero acceleration and a point corresponding to the extremum in deceleration of the robot in the smoothed data to be approximated is By doing so, the quadratic function can be determined. In the approximation with a quadratic function, points based on arbitrary constants are not selected, so the approximation function can be determined even if there is no experience or experimental results, and an accurate operation completion time can be obtained.

本発明の判定装置において、位相遅れを有しないフィルタとして中心化移動平均フィルタを用いることが好ましい。中心化移動平均フィルタによる処理では、現在のデータ値を挟んだ前後のデータ値だけを用いてフィルタ処理を行えるので、位相遅れを有しないフィルタ処理に必要な演算量を軽減することができる。中心化移動平均フィルタを用いる場合、計算または測定により取得されたロボットの振動成分の振動周波数の逆数を中心化移動平均フィルタの移動平均時間として設定することが好ましい。このように移動平均時間を設定することにより、ロボットの振動における主たる振動成分やそれよりも高い周波数の振動成分を効果的に除去することができる。 In the determination device of the present invention, it is preferable to use a centered moving average filter as the filter having no phase delay. In processing using a centered moving average filter, filtering can be performed using only the data values before and after the current data value, so the amount of computation required for filtering without phase lag can be reduced. When using the centered moving average filter, it is preferable to set the reciprocal of the vibration frequency of the vibration component of the robot obtained by calculation or measurement as the moving average time of the centered moving average filter. By setting the moving average time in this way, it is possible to effectively remove the main vibration component in the vibration of the robot and the vibration component with a higher frequency than that.

本発明の判定方法は、指令された位置へのロボットの移動動作の動作完了時刻を判定する判定方法であって、ロボットのアームまたはハンドの速度または加速度を示す信号を検出する工程と、信号に含まれる振動成分を除去して平滑化データを生成するフィルタ処理を行う工程と、を備え、フィルタ処理は、位相遅れを有しないフィルタによって実行される。 A determination method of the present invention is a determination method for determining a movement completion time of a movement operation of a robot to a commanded position, and comprises the steps of detecting a signal indicating the velocity or acceleration of an arm or hand of the robot; filtering to remove included vibration components to generate smoothed data, the filtering being performed by a filter having no phase lag.

本発明の判定方法では、速度または加速度を検出した信号から、位相遅れを有しないフィルタを用いて振動成分を除去するので、位相遅れの補償などを行うことなく、平滑化データによって表される波形から動作完了時刻を容易に識別することができる。 In the determination method of the present invention, the vibration component is removed from the signal obtained by detecting the velocity or acceleration using a filter that does not have a phase delay. , the operation completion time can be easily identified.

本発明の判定方法では、動作完了時刻の判定のために用いる信号は、ロボットのアームまたはハンドに設けられた加速度センサからの加速度を示す信号であることが好ましい。加速度センサによって得られる信号を利用することにより、大掛かりな装置などを必要とせずに簡単に動作完了時刻を判別できるようになる。 In the determination method of the present invention, the signal used for determining the motion completion time is preferably a signal indicating acceleration from an acceleration sensor provided on the arm or hand of the robot. By using the signal obtained by the acceleration sensor, it becomes possible to easily determine the operation completion time without requiring a large-scale device.

本発明の判定方法のうちの第１の判定方法は、ロボットが減速中であることを検出した後に平滑化データの値が最後に所定の閾値範囲外の値からこの閾値範囲内の値となったとき以降の平滑化データの値の平均値を求め、平滑化データから平均値を差し引くことによりオフセットとドリフトの補正を行う工程を有する。このような補正を行うことにより、速度または加速度の検出におけるオフセットとドリフトの影響を除去できて、より正確な動作完了時刻を得ることができる。 In the first determination method among the determination methods of the present invention, after detecting that the robot is decelerating, the value of the smoothed data finally changes from a value outside a predetermined threshold range to a value within this threshold range. A step of obtaining an average value of the values of the smoothed data after the time of the smoothing and subtracting the average value from the smoothed data corrects the offset and the drift. By performing such correction, the effects of offset and drift in the detection of velocity or acceleration can be removed, and more accurate operation completion time can be obtained.

第１の判定方法では、動作完了時刻であると推定される時刻の近傍において、オフセットとドリフトの補正を行った平滑化データのうち動作完了時刻と推定される時刻以前の期間の平滑化データを時刻の関数で近似し、この関数の値がゼロとなる時刻を動作完了時刻として判定する工程を設けることが好ましい。関数近似の手法を採用することにより、平滑化データに残存する振動成分の影響を除去してさらに正確に動作完了時刻を決定することができる。さらに本発明の判定方法では、オフセットとドリフトの補正を明示的に行わない場合（第２の判定方法の場合）においても、関数近似の手法を採用することができる。オフセットとドリフトの補正を明示的に行わない場合には、ロボットが減速中であることを検出した後に平滑化データの値が最後に所定の閾値範囲外の値からこの閾値範囲内の値となったときを基準時として、基準時以降の平滑化データを定数値で近似し、基準時以前の平滑化データを時刻の関数で近似し、この関数の値が、基準時以降の平滑化データを近似する定数値となる時刻を動作完了時刻として判定する。 In the first determination method, in the vicinity of the time estimated to be the operation completion time, the smoothed data for the period before the time estimated to be the operation completion time among the smoothed data corrected for the offset and the drift is selected. It is preferable to provide a step of approximating with a function of time and determining the time at which the value of this function becomes zero as the operation completion time. By adopting the function approximation method, it is possible to remove the influence of the vibration component remaining in the smoothed data and determine the operation completion time more accurately. Furthermore, in the determination method of the present invention, the function approximation technique can be employed even when offset and drift correction is not explicitly performed (in the case of the second determination method) . Without explicit offset and drift correction, the value of the smoothed data will eventually change from a value outside a predetermined threshold range to a value within this threshold range after detecting that the robot is decelerating. The smoothed data after the reference time is approximated by a constant value, the smoothed data before the reference time is approximated by a function of time, and the value of this function is the smoothed data after the reference time. The time when the approximate constant value is reached is determined as the operation completion time.

本発明によれば、人為的な誤差の要因を排除し、高い作業効率でロボットの動作完了時刻を正確に決定することができるようになる。 According to the present invention, it is possible to eliminate the factor of human error and accurately determine the operation completion time of the robot with high work efficiency.

ロボットの一例を示す図であって、（ａ）は平面図、（ｂ）は正面図、（ｃ）は原点位置にあるロボットの正面図である。It is a figure which shows an example of a robot, (a) is a top view, (b) is a front view, (c) is a front view of the robot in an origin position. 加速度を実測した結果と、実測した加速度に対して中心化移動平均フィルタによるフィルタ処理を行って得た結果とを示すグラフである。7 is a graph showing results of actually measured acceleration and results obtained by filtering the actually measured acceleration using a centered moving average filter; 本発明の実施の一形態の判定装置を説明するブロック図である。It is a block diagram explaining the determination apparatus of one embodiment of this invention. オフセット及びドリフトの補正を説明するグラフである。FIG. 10 is a graph illustrating offset and drift correction; FIG. 動作完了時刻の判定を説明するグラフである。7 is a graph for explaining determination of operation completion time;

次に、本発明の好ましい実施の形態について、図面を参照して説明する。本発明は搬送用ロボットなどのロボットの動作完了時刻の判定に関するものであるが、本発明の理解のため、まず、本発明の適用対象となるロボットについて説明する。図１は、本発明の適用対象のロボットの一例である搬送用ロボットを示している。 Preferred embodiments of the present invention will now be described with reference to the drawings. The present invention relates to determination of the operation completion time of a robot such as a transport robot. For understanding of the present invention, first, the robot to which the present invention is applied will be described. FIG. 1 shows a carrier robot, which is an example of a robot to which the present invention is applied.

図１に示すロボットは、水平多関節ロボットであり、ロボットの周囲に配置されている複数の処理室（不図示）の間で例えば略長方形のガラス基板であるワークを搬送するために用いられる。それぞれの処理室は、ワークに対して成膜やエッチングを行うためのものである。図１（ａ），（ｂ）は、アームやハンドを伸ばした状態でのロボットを示す平面図及び正面図である。図１に示されるロボットは、特開２０１５－１３９８５４号公報に開示された搬送用の水平多関節ロボットと同様のものであって、基台１１と、基台１１に取り付けられた第１アーム１２と、第１アーム１２の先端に取り付けられた第２アーム１３と、第２アーム１３の先端に取り付けられたハンド１４とを備えている。ハンド１４はワークを保持するものであって、フォーク(fork)状に形成されている。基台１１に対して第１アーム１２は軸Ａの周りで回転可能であり、第１アーム１２に対して第２アーム１３は軸Ｂの周りを回転可能であり、第２アーム１３に対してハンド１４は軸Ｃの周りで回転可能である。ロボットの関節軸である軸Ａ～Ｃの周りでの回転を可能にするために、ロボットには軸ごとにモータが備えられている。さらにロボットは、基台１１に設けられて第１アーム１２を図示Ｚ方向で昇降する機構が設けられ、この昇降機構も昇降用のモータによって駆動される。軸Ａ～Ｃは、いずれもＺ方向に平行である。 The robot shown in FIG. 1 is a horizontal multi-joint robot, and is used to transfer works such as substantially rectangular glass substrates between a plurality of processing chambers (not shown) arranged around the robot. Each processing chamber is for performing film formation and etching on a work. 1(a) and 1(b) are a plan view and a front view showing the robot with its arms and hands extended. The robot shown in FIG. 1 is similar to the horizontal articulated robot for transportation disclosed in Japanese Patent Application Laid-Open No. 2015-139854. , a second arm 13 attached to the tip of the first arm 12 , and a hand 14 attached to the tip of the second arm 13 . The hand 14 holds a work and is formed in a fork shape. The first arm 12 is rotatable about the axis A with respect to the base 11, the second arm 13 is rotatable about the axis B with respect to the first arm 12, and the second arm 13 is rotatable with respect to the second arm 13. The hand 14 is rotatable around the axis C. The robot is provided with a motor for each axis in order to enable rotation around the axes AC, which are the joint axes of the robot. Further, the robot is provided with a mechanism that is provided on the base 11 and moves the first arm 12 up and down in the illustrated Z direction, and this elevation mechanism is also driven by a motor for elevation. Axes A to C are all parallel to the Z direction.

図１に示すロボットには、ロボットの動作の基準となる原点位置が定められており、原点位置ではロボットはアームやハンドが所定の折り畳まれた姿勢となる。図１（ｃ）は原点位置でのロボットの姿勢を示しており、第１アーム１２上に第２アーム１３及びハンド１４が重なるように、第２アーム１３及びハンド１４が折り畳まれている。そしてハンド１４上に保持されているワークをある処理室に搬入するときには、アーム１２，１３が伸展してハンド１４がその処理室内に入り込み、その後、アーム１２，１３及びハンド１４の全体を昇降機構によって下降させることによって、ワークがハンド１４から離れて処理室内の所定の位置に配置されることになる。アーム１２，１３の伸展の完了後、残留振動が収束しないうちにアーム１２，１３及びハンド１４を下降させると、処理室内に配置されるワークが位置ずれを起こすことになる。処理室内でのワークの位置ずれは、その処理室においてワークに対して処理を行う際の障害要因となり得る。十分に時間が経過して残留振動が収束してからアーム１２，１３及びハンド１４を下降させてワークを処理室内に配置してもよいが、時間をかける分、作業効率が低下する。 The robot shown in FIG. 1 has an origin position that serves as a reference for the robot's motion, and at the origin position, the robot assumes a posture in which the arms and hands are folded in a predetermined manner. FIG. 1(c) shows the posture of the robot at the origin position, and the second arm 13 and hand 14 are folded so that the second arm 13 and hand 14 overlap on the first arm 12. FIG. When the workpiece held on the hand 14 is carried into a processing chamber, the arms 12 and 13 are extended so that the hand 14 enters the processing chamber. By lowering the workpiece by the hand 14, the workpiece is separated from the hand 14 and placed at a predetermined position in the processing chamber. After the extension of the arms 12 and 13 is completed, if the arms 12 and 13 and the hand 14 are lowered before the residual vibration is settled, the work placed in the processing chamber will be displaced. Positional deviation of the work in the processing chamber can be a hindrance factor when processing the work in the processing chamber. The arms 12 and 13 and the hand 14 may be lowered to place the work in the processing chamber after a sufficient amount of time has passed and the residual vibration has settled.

そこで、残留振動の収束時間の評価を行う必要が生じるが、そのためには、処理室ごとに定められている位置にアーム１２，１３やハンド１４が伸展する動作が完了した時刻、すなわちロボットの動作完了時刻を知る必要がある。以下、本発明に基づいて図１に示すロボットの動作完了時刻を判定する方法について説明する。 Therefore, it is necessary to evaluate the convergence time of the residual vibration. I need to know the completion time. A method for determining the operation completion time of the robot shown in FIG. 1 will be described below based on the present invention.

本実施形態においては、ロボットのアーム１２，１３及びハンド１４の伸展の動作を検出するために、ハンド１４に加速度センサ２１を取り付ける。ロボットの残留振動の特性は、ハンド１４上に保持されるワークの質量などによっても変化するので、ワークを載せた状態で残留振動の収束時間を判定することがあり得る。そこで、加速度センサ２１としては、小型であってロボットの振動特性に影響を与えないものをハンド１４の下面側に取り付ける。このように加速度センサ２１を取り付けたロボットを動作させると、加速度センサ２１で検出される加速度のうち、ロボットへの指令に基づくハンド１４の移動方向での加速度成分は、例えば図２において灰色の線で示すようなものとなる。大局的にはハンド１４が加速しているときは加速度は正であり、定速状態では加速度はほぼゼロであり、減速しているときは負である。実際には振動の影響が大きく重畳した加速度波形となる。特に、アーム１２，１３やハンド１４が駆動されているときは、モータや減速機などによって発生する大きな振動があたかもノイズであるかのように重畳している。さらに加速度センサ２１の出力には、実際に印加される加速度がゼロであっても出力がゼロとならないというオフセットによる成分と、実際に印加される加速度は一定であるが出力値が徐々に変化するというドリフトによる成分とが含まれている。図２では、停止しているはずのときの加速度が０．０５ｍ／ｓ²程度になっているが、これがオフセットの影響である。なお図２では、説明のため、オフセット及びドリフトの影響を誇張して描いており、加速度センサ２１が正常であれば、通常は、振動の振幅に比べてオフセットやドリフトはずっと小さくなる。市販の加速度センサでは、オフセットやドリフトの要因は主として温度変化であると考えられている。 In this embodiment, an acceleration sensor 21 is attached to the hand 14 in order to detect the extension motion of the arms 12 and 13 and the hand 14 of the robot. Since the characteristics of the residual vibration of the robot also change depending on the mass of the workpiece held on the hand 14, etc., it is possible that the convergence time of the residual vibration is determined with the workpiece placed thereon. Therefore, as the acceleration sensor 21 , a small sensor that does not affect the vibration characteristics of the robot is attached to the lower surface of the hand 14 . When the robot equipped with the acceleration sensor 21 is operated in this way, the acceleration component in the moving direction of the hand 14 based on the command to the robot, among the acceleration detected by the acceleration sensor 21, is represented by the gray line in FIG. is as shown in Overall, the acceleration is positive when the hand 14 is accelerating, nearly zero when the hand is at a constant speed, and negative when the hand is decelerating. In reality, the acceleration waveform is greatly influenced by vibration and is superimposed. In particular, when the arms 12 and 13 and the hand 14 are being driven, large vibrations generated by motors, reduction gears, etc. are superimposed as if they were noise. Furthermore, the output of the acceleration sensor 21 includes an offset component in which the output does not become zero even if the acceleration actually applied is zero, and the output value gradually changes even though the acceleration actually applied is constant. A drift component is included. In FIG. 2, the acceleration is about 0.05 m/s ² when the vehicle should be stopped, but this is the effect of the offset. In FIG. 2, the effects of offset and drift are exaggerated for explanation. If the acceleration sensor 21 is normal, the offset and drift are usually much smaller than the amplitude of vibration. Temperature changes are considered to be the main cause of offset and drift in commercial accelerometers.

従来は、振動が大きく重畳した加速度波形から目視で動作完了時刻を推定していた。図２において破線は、目視で推定された動作完了時刻の例を示している。動作が完了したと思われる時刻以降において加速度波形が振動しているが、これは残留振動を検出したものである。本実施形態の判定方法が目標とするところは、減速後、加速度がゼロとなった時刻として原理上は定義される動作終了時刻を、振動が大きく重畳している加速度波形から、目視などに頼らずに客観的な操作によって自動的に抽出し判定することである。一般にノイズなどが重畳した波形からノイズを取り除くために、ローパスフィルタを用いる平滑化操作が知られている。しかしながら通常のローパスフィルタは積分操作に基づくものであって必ず位相遅れを伴うものであり、通常のローパスフィルタを使用して加速度波形を平滑化して得られた動作完了時刻は、本来の動作完了時刻よりも遅れたものとなる。位相遅れを補償することも考えられるが、正確な位相遅れ補償を行うことは難しい。したがって、本実施形態の目的には、通常のローパスフィルタを使用することはできない。 Conventionally, the operation completion time has been estimated visually from the acceleration waveform on which large vibrations are superimposed. In FIG. 2, the dashed line indicates an example of the visually estimated operation completion time. The acceleration waveform oscillates after the time when the motion is thought to have completed, but this is due to the detection of residual vibration. The target of the determination method of the present embodiment is to determine the operation end time, which is defined in principle as the time when the acceleration becomes zero after deceleration, from the acceleration waveform on which large vibrations are superimposed, without relying on visual observation or the like. It is to automatically extract and judge by objective operation without any effort. A smoothing operation using a low-pass filter is generally known for removing noise from a waveform on which noise and the like are superimposed. However, a normal low-pass filter is based on an integral operation and always accompanies a phase delay. will be later than Compensating for the phase lag can be considered, but it is difficult to perform accurate phase lag compensation. Therefore, a normal low-pass filter cannot be used for the purpose of this embodiment.

ところで、ロボットの残留振動の収束時間について、実際にロボットを動作させたときにリアルタイムで求めることは少なく、通常は、ロボットの振動波形を記録した上で、記録された振動波形に基づいて判定することが多い。特にデジタル処理を前提とすれば、加速度センサからの加速度信号をアナログ／デジタル変換によりデジタル信号に変換してメモリに格納し、メモリに格納された時系列のデータに基づいて加速度波形を例えばディスプレイなどにグラフィック表示し、その表示された波形に基づいて動作完了時刻を目視で決定し、収束時間を求めている。加速度波形を表すデータがあらかじめメモリに格納されているのであれば、デジタル信号処理により、位相遅れを有しないフィルタ処理を行うことが可能である。そこで本発明では、位相遅れを有しないフィルタを用いて平滑化処理を行うこととする。位相遅れを有しないフィルタの例として、中心化移動平均フィルタ、ゼロ位相フィルタなどがある。図２において黒色の実線は、灰色の線で示された加速度波形に対して中心化移動平均フィルタを適用した結果を示している。位相遅れを有しないフィルタによって処理された後の信号からなるデータを平滑化データと呼び、また平滑化データによって表される加速度波形を平滑化波形と呼ぶ。中心化移動平均フィルタを用いることによって加速度波形における振動成分がほとんど除去される。オフセットやドリフトが無視できるものとすれば、加速度センサ２１に減速を検出した後に中心化移動平均フィルタによって得られた平滑化波形において加速度がゼロとなって時刻を動作完了時刻と判定することができる。ここではオフセットやドリフトを無視し、また、完全には除去されなかった振動成分を無視しているが、それでも加速度波形から目視によって動作完了時刻を推定する場合よりは、例えば残留振動の収束時間の判定において、良好な結果を得ることができる。 By the way, the convergence time of the residual vibration of the robot is rarely obtained in real time when the robot is actually operated. There are many things. In particular, assuming digital processing, the acceleration signal from the acceleration sensor is converted to a digital signal by analog/digital conversion and stored in a memory, and the acceleration waveform is displayed, for example, on a display based on the time-series data stored in the memory. , and based on the displayed waveform, the operation completion time is visually determined, and the convergence time is obtained. If the data representing the acceleration waveform is stored in memory in advance, digital signal processing can be used to perform filtering without phase delay. Therefore, in the present invention, the smoothing process is performed using a filter having no phase delay. Examples of filters with no phase lag include centered moving average filters, zero phase filters, and the like. In FIG. 2, the black solid line indicates the result of applying the centered moving average filter to the acceleration waveform indicated by the gray line. Data composed of signals after being processed by a filter having no phase delay are called smoothed data, and an acceleration waveform represented by the smoothed data is called a smoothed waveform. The use of a centered moving average filter removes most of the oscillating component in the acceleration waveform. Assuming that the offset and drift can be ignored, the acceleration becomes zero in the smoothed waveform obtained by the centered moving average filter after deceleration is detected by the acceleration sensor 21, and the time can be determined as the operation completion time. . Here, offsets and drifts are ignored, and vibration components that have not been completely removed are ignored. Good results can be obtained in the determination.

図３は、本発明の実施の一形態の判定装置２０を示している。この判定装置２０は、ロボットに取り付けられた加速度センサ２１からの加速度信号に基づいてロボットの動作完了時刻を求めるものであり、残留時間の収束時間を評価する収束時間評価装置に対し、求めた動作完了時刻を出力するように構成されている。判定装置２０は、加速度センサ２１と、加速度センサ２１から出力されるアナログでの加速度信号をデジタル信号に変換するアナログ／デジタル変換器２２と、アナログ／デジタル変換器２２からの出力信号をデジタルでの加速度データとして逐次記憶する波形記憶部２３と、波形記憶部２３に記憶された加速度データに対し、位相遅れのないフィルタによるフィルタ処理を行って平滑化データを出力するフィルタ処理部２４と、を備えている。加速度センサ２１としてデジタルデータを出力するものを使用する場合には、アナログ／デジタル変換器２２を設ける必要はない。波形記憶部２３は、通常、デジタルメモリによって構成される。上述したように、フィルタ処理部２４からの平滑化データで表される平滑化波形によっても動作完了時刻を求めることができるが、図３に示す判定装置２０は、オフセットやドリフトの影響を取り除く処理と、除去されずに平滑化波形に残っている振動成分などの影響を取り除く処理とを実行する演算部２５をさらに備えており、より正確に動作完了時刻を求めることができるようになっている。演算部２５で実行する処理については後述する。 FIG. 3 shows a determination device 20 according to one embodiment of the invention. This determination device 20 determines the motion completion time of the robot based on the acceleration signal from the acceleration sensor 21 attached to the robot. Configured to output completion time. The determination device 20 includes an acceleration sensor 21, an analog/digital converter 22 that converts an analog acceleration signal output from the acceleration sensor 21 into a digital signal, and a digital signal output from the analog/digital converter 22. A waveform storage unit 23 for sequentially storing acceleration data, and a filter processing unit 24 for filtering the acceleration data stored in the waveform storage unit 23 with a filter having no phase delay and outputting smoothed data. ing. If a sensor that outputs digital data is used as the acceleration sensor 21, the analog/digital converter 22 need not be provided. The waveform storage unit 23 is normally composed of a digital memory. As described above, the operation completion time can also be obtained from the smoothed waveform represented by the smoothed data from the filter processing unit 24. However, the determination device 20 shown in FIG. and removing the influence of vibration components remaining in the smoothed waveform without being removed. . The processing executed by the calculation unit 25 will be described later.

ここでフィルタ処理部２４について説明する。フィルタ処理部２４は、位相遅れのないフィルタによるフィルタ処理を行うものである。位相遅れのないフィルタとして中心化移動平均フィルタを使用する場合、そのフィルタの特性を表すパラメータとして、移動平均時間がある。中心化移動平均フィルタは、移動平均時間の逆数で表される周波数の成分とその整数倍の周波数の成分とをもっとも強く除去する。移動平均時間の逆数をノッチ周波数と呼ぶ。ゼロ位相フィルタについても同様に周波数で表されるパラメータがある。ロボットの振動の周波数成分ごと強度を考えると、アーム１２，１３及びハンド１４（とこれらに加えてワーク）からなる振動系の共振周波数が卓越していると考えられる。そこで、中心化移動平均フィルタのノッチ周波数は、ロボットの振動系の共振周波数と一致するように設定することが好ましい。ロボットの振動系の共振周波数は、ロボットの構造パラメータから計算によって算出できるので、その算出した共振周波数に基づいてフィルタのノッチ周波数を設定すればよい。あるいは、加速度センサ２１により実測された加速度波形に対してＦＦＴ（高速フーリエ変換）を行うことなどにより実際の振動の周波数を求め、求められた周波数に基づいてノッチ周波数を設定してもよい。ＦＦＴによって実際の振動周波数を求めてノッチ周波数を設定する場合、ＦＦＴによって得られた周波数スペクトルにおいてピーク値が規定のピーク値を超えるものの中の最小の振動周波数をノッチ周波数としてもよいし、周波数スペクトルにおいて最大のピークを有する振動周波数をノッチ周波数としてもよい。 Here, the filter processing unit 24 will be described. The filtering unit 24 performs filtering using a filter with no phase delay. When a centered moving average filter is used as a filter with no phase delay, there is a moving average time as a parameter representing the characteristics of the filter. The centered moving average filter most strongly removes the frequency component represented by the reciprocal of the moving average time and the frequency component that is an integral multiple thereof. The reciprocal of the moving average time is called the notch frequency. The zero-phase filter also has parameters expressed in frequency. Considering the strength of each frequency component of vibration of the robot, it is considered that the resonance frequency of the vibration system consisting of the arms 12, 13 and the hand 14 (and the workpiece in addition to these) is dominant. Therefore, it is preferable to set the notch frequency of the centered moving average filter so as to match the resonance frequency of the vibration system of the robot. Since the resonance frequency of the vibration system of the robot can be calculated from the structural parameters of the robot, the notch frequency of the filter can be set based on the calculated resonance frequency. Alternatively, the actual vibration frequency may be obtained by performing FFT (Fast Fourier Transform) on the acceleration waveform actually measured by the acceleration sensor 21, and the notch frequency may be set based on the obtained frequency. When the actual vibration frequency is obtained by FFT and the notch frequency is set, the minimum vibration frequency among those whose peak value exceeds the specified peak value in the frequency spectrum obtained by FFT may be the notch frequency, or the frequency spectrum may be the notch frequency.

このように、フィルタ処理部２４において位相遅れのないフィルタによって加速度波形をフィルタ処理して振動成分を除去し平滑化波形を生成することによって、動作完了時間の識別が容易になり、位相遅れ補償などを行う必要もなくなる。位相遅れのないフィルタとして中心化移動平均フィルタを用いる場合には、時刻ごとにその時刻の前後の時刻でのデータのみを用いて演算を行うことができるので、処理の簡素化を行え、その分、短時間での処理を実現できる。 In this way, the filter processing unit 24 filters the acceleration waveform with a filter having no phase delay to remove the vibration component and generate a smoothed waveform. no longer need to do When a centered moving average filter is used as a filter with no phase delay, calculations can be performed using only the data at times before and after each time, so processing can be simplified. can be processed in a short time.

次に、より正確に動作完了時刻を求めるために演算部２５で実行する処理について説明する。演算部２５では、まず、加速度波形に含まれるオフセット及びドリフトを除去するために平滑化波形を補正する。動作完了時刻の判定には、ロボットのアーム１２，１３やハンド１４が減速を開始してから移動が完了し、残留振動も収束するまでの期間での加速度データを考えればよく、この期間の開始時点と終了時点との間でドリフト量には大きな差は生じないと考えられるので、オフセットとドリフトとを一括して補正することができる。図４は、オフセット及びドリフトの補正を説明する図であり、動作完了であると推定される時刻の前後での平滑化波形を拡大して示している。動作完了であると推定される時刻は、例えば、平滑化データの値がゼロに接近し、その後はほとんど変化しなくなるような時刻として定めることができる。 Next, a description will be given of the processing executed by the calculation unit 25 in order to obtain the operation completion time more accurately. The calculator 25 first corrects the smoothed waveform to remove the offset and drift contained in the acceleration waveform. To determine the operation completion time, it is sufficient to consider the acceleration data in the period from when the robot arms 12, 13 and hand 14 start decelerating until the movement is completed and the residual vibration converges. Since it is considered that there is no large difference in the amount of drift between the time point and the end time point, the offset and the drift can be corrected collectively. FIG. 4 is a diagram for explaining correction of offset and drift, showing an enlarged smoothed waveform before and after the time when the operation is estimated to be completed. The time at which the operation is estimated to be completed can be determined, for example, as the time at which the value of the smoothed data approaches zero and hardly changes after that.

まず、図２に示す平滑化波形の全区間における加速度の値が最大値Ｍａｘとなる時刻と最小値Ｍｉｎとなる時刻とを求める。加速度センサ２１の取り付け方向やロボットの移動方向によっては、出力される加速度信号の符号が加速時には負となり減速時には正となることがあることに注意して、最大値Ｍａｘとなる時刻と最小値Ｍｉｎとなる時刻のうちの遅い方の時刻を検出し、これが減速に対応するものとする。図示したものでは、最小値Ｍｉｎの時刻が減速に対応している。そしてこの時刻以降、平滑化波形は細かい変動はあるものの全体としてはゼロに向かって変化する。そこで、図４に示すように、加速度ゼロを中心とする閾値領域Ａを設定すると、平滑化波形を表す平滑化データの値は、最初のうちは閾値領域Ａの外側にあるが、時間の経過とともに閾値領域Ａの外側と内側との間を行き来するようになり、最終的には閾値領域Ａの中にのみ含まれるようになる。平滑化データの値がこの閾値領域Ａに最後に入った時刻をｘとして、時刻ｘ以降の平滑化データの値の平均値を算出する。この平均値はオフセットとドリフトの量に対応するものと考えられる。そこで平均値がゼロとなるように、平滑化データの値から平均値を差し引く。言い換えれば、平滑化波形を平均値の分だけ図示上下方向にシフトさせる。その結果、図４において破線Ｂで示されるように、オフセットとドリフトが除去された平滑化波形が得られる。閾値領域Ａの幅は、オフセットとドリフトの補正の処理によっては除去されない振動やオフセット、ドリフトの影響を受けないことを条件として、できるだけ小さな幅とすることが好ましい。このようにオフセットとドリフトの補正を行うことによって、加速度センサ２１における温度変化などの影響を排除して動作完了時刻をより精度よく識別できるようになる。 First, the time at which the acceleration value reaches the maximum value Max and the time at which the acceleration value reaches the minimum value Min in all sections of the smoothed waveform shown in FIG. 2 are obtained. Note that the sign of the output acceleration signal may become negative during acceleration and positive during deceleration depending on the mounting direction of the acceleration sensor 21 and the movement direction of the robot. , and the later time is detected, and this corresponds to deceleration. In the illustration, the time of the minimum value Min corresponds to deceleration. After this time, the smoothed waveform changes toward zero as a whole, although there are fine fluctuations. Therefore, as shown in FIG. 4, when a threshold area A centered at zero acceleration is set, the values of the smoothed data representing the smoothed waveform are outside the threshold area A at first, but with the passage of time , it comes and goes between the outside and inside of the threshold area A, and finally comes to be contained only in the threshold area A. Assuming that the time when the value of the smoothed data last entered the threshold region A is x, the average value of the values of the smoothed data after the time x is calculated. This average value is considered to correspond to the amount of offset and drift. Therefore, the average value is subtracted from the smoothed data values so that the average value becomes zero. In other words, the smoothed waveform is shifted vertically in the figure by the average value. As a result, a smoothed waveform from which the offset and drift are removed is obtained, as indicated by the dashed line B in FIG. The width of the threshold region A is preferably as small as possible provided that it is not affected by vibrations, offsets, and drifts that are not removed by the offset and drift correction process. By correcting the offset and the drift in this way, the effect of temperature change on the acceleration sensor 21 can be eliminated, and the operation completion time can be identified with higher accuracy.

ある閾値による範囲を定めて、平滑化波形が最後にこの範囲に入ったときが動作完了時刻であるとすると、閾値の設定に動作完了時刻が依存することとなり、動作完了時刻を正しく算出することができない。そこで、動作完了時刻と推定される時刻の近傍の平滑化波形を、その推定される時刻を基準として時刻に関して前後に２分割し、分割して得られた２つの波形をそれぞれ別の関数で近似し、それらの関数の交点を座標を求めて動作完了時刻とする。ここでいう２つの関数の一方は、１次関数または２次関数であり、他方の関数は、加速度を表す変数をｙとして、オフセットとドリフトの補正を行っているときは、ｙ＝０であり、オフセットとドリフトの補正を行っていないときは、ｙ＝ａである。ａは、動作完了時刻と推定される時刻以降における平滑化データを近似する定数値である。オフセットとドリフトの補正を行っていないときは、平滑化波形が上述した閾値範囲Ａに最後に入った時刻を基準時として、基準時以前の平滑化波形を１次関数または２次関数で近似し、基準時以降の平滑化波形を定数値ａで近似することが好ましい。 If a range is defined by a certain threshold and the operation completion time is when the smoothed waveform finally enters this range, the operation completion time depends on the threshold setting, and it is necessary to calculate the operation completion time correctly. can't Therefore, the smoothed waveform near the time estimated as the operation completion time is divided into two before and after with respect to the estimated time, and the two waveforms obtained by dividing are approximated by different functions. Then, the coordinates of the point of intersection of these functions are obtained and set as the operation completion time. One of the two functions referred to here is a linear function or a quadratic function, and the other function is y=0 when the offset and drift are being corrected, with the variable representing the acceleration being y. , y=a when no offset and drift correction is performed. a is a constant value that approximates the smoothed data after the time estimated as the operation completion time. When the offset and drift are not corrected, the smoothed waveform before the reference time is approximated by a linear function or a quadratic function, with the time when the smoothed waveform last entered the threshold range A described above as the reference time. , the smoothed waveform after the reference time is preferably approximated by a constant value a.

図５は、上述のようにして演算部２５によって最終的に動作完了時刻を決定する処理を説明しており、図５に示される平滑化波形は、オフセットとドリフトの補正を行った後のものである。動作完了時刻は、ロボットの減速が終了する時刻であり、この時刻の近傍ではロボットの減速での加速度変化（減速曲線と呼ぶ）を１次関数で近似する。この１次関数で表される直線をＬとすれば、直線Ｌと加速度ゼロとの交点Ｅの時刻Ｔ_endが動作完了時刻であるとすることができる。そこで、オフセットとドリフトの補正を行った後の平滑化波形における極小値Ｍｉｎの座標を（ｔ_m，ａ_m）とする。ゼロ加速度検出用の閾値ａ_wcを予め定めておく。オフセットとドリフトを補正した後の平滑化波形において閾値ａ_wcに対応する時刻をｔ_wcとし、直線Ｌは点Ｃ（ｔ_wc，ａ_wc）も通過するものとする。そして、０以上１以下の任意の定数ｒ_aを用いて、直線Ｌのもう１つの通過点Ｈ（ｔ_h，ａ_h）を
ａ_h＝ｒ_a×（ａ_m－ａ_wc）＋ａ_wc
で定める。そして、オフセットとドリフトを補正した後の平滑化データの加速度がａ_wc及びａ_hとなる時刻をサーチしてｔ_wc及びｔ_hを求める。これにより、直線Ｌが点Ｃ及び点Ｈを通ることから直線Ｌを表す１次関数が一意に定まったことになり、動作完了時刻ｔ_endを
ｔ_end＝ｔ_wc－ａ_wc×（ｔ_h－ｔ_wc）／（ａ_h－ａ_wc）
により求めることができる。 FIG. 5 illustrates the process of finally determining the operation completion time by the computing unit 25 as described above, and the smoothed waveform shown in FIG. is. The motion completion time is the time at which deceleration of the robot ends. Near this time, the change in acceleration (called a deceleration curve) due to the deceleration of the robot is approximated by a linear function. Assuming that the straight line represented by this linear function is L, the time T _end at the intersection E of the straight line L and zero acceleration can be regarded as the operation completion time. Therefore, let (t _m , _am ) be the coordinates of the minimum value Min in the smoothed waveform after offset and drift correction. A threshold value _awc for detecting zero acceleration is determined in advance. Let _twc be the time corresponding to the threshold value _awc in the smoothed waveform after correcting the offset and drift, and the straight line L also passes through the point C (twc _, _awc ). Then, using an arbitrary constant r _a from 0 to 1, another passing point H(t _h , a _h ) of the straight line L is set as follows: a _h =r _a ×(a _m −a _wc )+a _wc
Determined by Then, t _wc and t _h are obtained by searching for the time when the acceleration of the smoothed data after offset and drift correction becomes a _wc and a _h . As a result, since the straight line L passes through the points C and H, the linear function representing the straight line L is uniquely determined, and the operation completion time t _end is t _end =t _wc −a _wc ×(t _h − t _wc )/(a _h −a _wc )
can be obtained by

ここで説明した方法では、１次関数によって減速曲線を近似して動作完了時刻ｔ_endを求めるので、また加速度に対応する時刻を求めるサーチの範囲も狭いので、演算が容易であり、またそのためのプログラム作成も容易である。ゼロ加速度検出用の閾値ａ_wcは、平滑化波形に残る振動の影響を受けない範囲でできるだけ小さい値であることが好ましい。定数ｒ_aとしては、通常、０．２５を好ましく使用することができる。定数ｒ_aが大きすぎると、減速曲線を１次関数で近似するという仮定が崩れることとなり、また、小さすぎると、平滑化波形に残る振動の影響を受けやすくなる。経験や実験結果に基づいて定数ｒ_aを定めることにより、より精度よく動作完了時刻ｔ_endを求めることができる。 In the method described here, since the operation completion time t _end is obtained by approximating the deceleration curve with a linear function, and the search range for obtaining the time corresponding to the acceleration is narrow, the calculation is easy, and the calculation is easy. Program creation is also easy. The threshold value _awc for detecting zero acceleration is preferably as small as possible within a range that is not affected by vibrations remaining in the smoothed waveform. As the constant _ra , 0.25 can usually be preferably used. If the constant _ra is too large, the assumption that the deceleration curve is approximated by a linear function will be lost, and if it is too small, the smoothed waveform will be susceptible to vibrations remaining. By determining the constant r _a based on experience and experimental results, the operation completion time t _end can be obtained with higher accuracy.

上述の減速曲線を１次関数で近似する方法では、オフセットとドリフトとを補正した後の平滑化波形上の２点Ｃ，Ｈを通るものとして直線Ｌを定めている。その代わりに、点Ｃと点Ｈとの間の平滑化波形を最小二乗法によって１次関数にフィッティングし、最小二乗フィッティングによって得られた１次関数に基づいて動作完了時刻ｔ_endを求めてもよい。最小二乗法を用いるほうがより精度よく動作完了時刻ｔ_endを求めることができるが、その分、演算量が多くなる。 In the above method of approximating the deceleration curve with a linear function, a straight line L is defined as passing through two points C and H on the smoothed waveform after the offset and drift have been corrected. Alternatively, the smoothed waveform between point C and point H may be fitted to a linear function by the least squares method, and the operation completion time t _end may be obtained based on the linear function obtained by the least squares fitting. good. Using the least-squares method makes it possible to obtain the operation completion time t _end with higher accuracy, but the amount of calculation increases accordingly.

減速曲線を２次関数に近似して動作完了時刻ｔ_endを求めることもできる。オフセットとドリフトの補正を行った後の平滑化波形における極小値Ｍｉｎと上述の点Ｃを使用し、極小値Ｍｉｎが２次関数が表す放物線の頂点に対応し、かつ放物線上に点Ｃがあるとすればよく、時刻ｔ_m以降で２次関数が表す放物線が加速度ゼロと交わる点の時刻を動作完了時刻ｔ_endとすればよい。すなわち下記式によってｔ_endを定めればよい。 The operation completion time t _end can also be obtained by approximating the deceleration curve to a quadratic function. Using the local minimum Min in the smoothed waveform after offset and drift correction and the above point C, the local minimum Min corresponds to the vertex of the parabola represented by the quadratic function, and there is a point C on the parabola. Then, the time at which the parabola represented by the quadratic function intersects zero acceleration after time t _m is set as the operation completion time t _end . That is, t _end can be determined by the following formula.

この場合は任意定数ｒ_aを用いることなく動作完了時刻ｔ_endを決定できるので、経験や実験によらずに精度よく動作完了時刻ｔ_endを決定できる。

In this case, since the operation completion time t _end can be determined without using the arbitrary constant _ra , the operation completion time t _end can be determined with high accuracy without relying on experience or experiments.

以上のように判別装置２０において動作完了時刻が求められると、動作完了時刻は収束時間評価装置３０において利用される。収束時間評価装置３０は、例えば、波形記憶部２３内の加速度データを参照して、動作完了時刻から加速度が最後に収束判定閾値以内となる時刻ｔ_dまでの時間を求めて残留振動収束時間を決定する。また時刻ｔ_d以降の加速度データにおける加速度の最大値及び最小値を解析して、位置における残留振動振幅を算出する。さらに、加速度データを２重積分して変位を求め、さらにハイパスフィルタを通して変位から加速度センサの加速度オフセットの影響を除去することにより、変位における残留振動を分析することができる。このような処理を行う収束時間評価装置３０は、例えばパーソナルコンピュータ（ＰＣ）上で実行されるソフトウェアによって実現することができる。特に、ソフトウェアにより判別装置２０を実現する場合であれば、判別装置２０の機能と収束時間評価装置３０とを同じソフトウェアに組み込んで、動作完了時刻の判別と残留信号の評価とを有機的に結合して実行することができる。 When the operation completion time is obtained in the determination device 20 as described above, the operation completion time is used in the convergence time evaluation device 30 . The convergence time evaluation device 30, for example, refers to the acceleration data in the waveform storage unit 23, obtains the time from the operation completion time to the time _td when the acceleration finally falls within the convergence determination threshold value, and determines the residual vibration convergence time. decide. Also, the maximum and minimum values of acceleration in the acceleration data after time _td are analyzed to calculate the residual vibration amplitude at the position. In addition, residual vibration in displacement can be analyzed by double integrating the acceleration data to obtain the displacement, and then passing the displacement through a high-pass filter to remove the effects of the acceleration offset of the acceleration sensor. The convergence time evaluation device 30 that performs such processing can be realized, for example, by software executed on a personal computer (PC). In particular, if the discriminating device 20 is implemented by software, the functions of the discriminating device 20 and the convergence time evaluation device 30 are incorporated into the same software to organically combine the determination of the operation completion time and the evaluation of the residual signal. can be run as

以上説明した実施形態においては、ロボットに加速度センサ２１を取り付けた上で、加速度センサ２１から出力される加速度波形に対し、位相遅れのないフィルタ（特に中心化移動平均フィルタ）によるフィルタ処理を行って平滑化波形を得ることにより、加速度波形に重畳する振動の影響を除去してロボットの動作完了時刻を容易に識別できるようになる。平滑化波形に対してオフセットとドリフトの補正を行えば、より正確に動作完了時刻を判別できるようになる。さらに、推定される動作完了時刻の近傍で減速曲線を１次関数または２次関数で近似することにより、一層正確に動作完了時刻を判別できるようになる。また、フィルタ処理部２４及び演算部２５はソフトウェアによって実現でき、波形記憶部２３も一般的なコンピュータにおけるメモリを使用して実現できるから、例えば本実施形態の判別装置２０は、加速度センサ２１からのデータがデジタルデータとして得られるのであれば、パーソナルコンピュータ（ＰＣ）上でソフトウェアを実行することにより実現できる。また本実施形態では、ロボットに取り付けた加速度センサ２１からのデータをＰＣソフトウェアで分析することにより動作完了時刻を判別することができるので、ロボットのアームやハンドの駆動を制御するロボットコントローラから内部動作状態や信号を取得する必要なく、簡便に動作完了時刻を判別することができる。 In the embodiment described above, after the acceleration sensor 21 is attached to the robot, the acceleration waveform output from the acceleration sensor 21 is filtered by a filter without phase delay (especially a centered moving average filter). By obtaining the smoothed waveform, it becomes possible to remove the influence of the vibration superimposed on the acceleration waveform and easily identify the operation completion time of the robot. By correcting the offset and drift of the smoothed waveform, the operation completion time can be determined more accurately. Furthermore, by approximating the deceleration curve with a linear function or a quadratic function in the vicinity of the estimated motion completion time, the motion completion time can be determined more accurately. Further, the filter processing unit 24 and the calculation unit 25 can be implemented by software, and the waveform storage unit 23 can be implemented using a memory in a general computer. If the data is obtained as digital data, it can be realized by executing software on a personal computer (PC). In this embodiment, the operation completion time can be determined by analyzing the data from the acceleration sensor 21 attached to the robot using PC software. It is possible to easily determine the operation completion time without the need to acquire the state or signal.

上述の実施形態ではロボットに加速度センサを取り付けて加速度に基づいて動作完了時刻を判別している。しかしながら本発明では、加速度以外にも、動作完了時刻の直前ではゼロでなく動作完了時刻以降はゼロとなる物理量を用いて動作完了時刻を判別することもできる。例えば、ロボットのアームやハンドの速度を検出して上述と同様の処理を行うことにより、動作完了時刻を判別することができる。すなわち本発明の実施のためには、最低限、ロボットのアームまたはハンドの速度または加速度を検出する動作検出手段と、位相遅れを有しないフィルタによって構成されて動作検出手段からの信号に含まれる振動成分を除去して平滑化信号を生成するフィルタ手段とを設ければよいことになる。速度を使用する場合には、例えば、レーザトラッカーなどによってロボットのアームまたはハンドの位置を追跡し、検出された位置データに対して数値微分を行って速度に変換した波形データを取得し、この波形データに対して上述と同様の処理を行えばよい。実際には、レーザトラッカーなどを設けることは設備が大掛かりになるから、小型化なものを容易に入手できる加速度センサ２１をロボットに取り付けて加速度に基づいて動作完了時刻を判別する方が実施が容易である。 In the above-described embodiment, an acceleration sensor is attached to the robot and the operation completion time is determined based on the acceleration. However, in the present invention, the motion completion time can also be determined using a physical quantity that is not zero immediately before the motion completion time but is zero after the motion completion time, other than the acceleration. For example, the operation completion time can be determined by detecting the speed of the arm or hand of the robot and performing the same processing as described above. That is, in order to carry out the present invention, at least, a motion detecting means for detecting the velocity or acceleration of the arm or hand of the robot, and a filter having no phase delay, which is composed of the vibration contained in the signal from the motion detecting means. Filter means for removing components to generate a smoothed signal is sufficient. When velocity is used, for example, the position of the arm or hand of the robot is tracked by a laser tracker or the like, and the detected position data is numerically differentiated to obtain waveform data converted into velocity. The data may be processed in the same manner as described above. In practice, installing a laser tracker or the like requires a large-scale facility. Therefore, it is easier to determine the operation completion time based on the acceleration by attaching the acceleration sensor 21, which can be easily obtained in a small size, to the robot. is.

１１…基台；１２，１３…アーム；１４…ハンド；２０…判定装置；２１…加速度センサ；２２…アナログ／デジタル変換器；２３…波形記憶部；２４…フィルタ処理部；２５…演算部；３０…収束時間評価装置。 11 -- Base; 12, 13 -- Arm; 14 -- Hand; 20 -- Judging device; 21 -- Acceleration sensor; 22 -- Analog/digital converter; 30 Convergence time evaluation device.

Claims

A determination device for determining an operation completion time of a movement operation of a robot to a commanded position,
motion detection means for detecting the speed or acceleration of the arm or hand of the robot;
filter means for removing vibration components contained in the signal from the motion detection means to generate smoothed data;
After the motion detection means detects that the robot is decelerating, the smoothed data value last changes from a value outside the predetermined threshold range to a value within the predetermined threshold range. an arithmetic unit that calculates an average value of the values of the smoothed data and subtracts the average value from the smoothed data to correct offset and drift;
wherein said filter means is composed of a filter having no phase delay.

In the vicinity of the time estimated to be the operation completion time, the calculation unit calculates, in the smoothed data corrected for the offset and drift, the period before the time estimated to be the operation completion time. 2. The determination device according to claim 1 , wherein the smoothed data is approximated by a function of time as the smoothed data to be approximated, and the time when the value of the function becomes zero is determined as the operation completion time.

A determination device for determining a motion completion time of a movement motion of a robot to a commanded position, the determination device comprising motion detection means for detecting velocity or acceleration of an arm or hand of the robot;
filter means for removing vibration components contained in the signal from the motion detection means to generate smoothed data;
with
the filter means is configured by a filter having no phase delay,
The reference time is the time when the value of the smoothed data finally changes from a value outside the predetermined threshold range to a value within the predetermined threshold range after the motion detection means detects that the robot is decelerating. approximating the smoothed data after the reference time by a constant value, approximating the smoothed data before the reference time as smoothed data to be approximated by a function of time, and the value of the function becomes the constant value A determination device that determines a time as an operation completion time.

The function is a linear function, selects a point corresponding to a threshold for detecting zero acceleration and one point determined using an arbitrary constant in the smoothed data to be approximated, and converts the selected point to the 4. The determination device according to claim 2 , wherein the linear function is determined such that a straight line represented by the linear function passes through.

The function is a quadratic function, and a point corresponding to a threshold value for detecting zero acceleration and a point corresponding to an extreme value of deceleration of the robot are selected in the smoothed data to be approximated, and the quadratic function is expressed 4. The determination device according to claim 2 , wherein said quadratic function is determined such that a parabola passes through a point corresponding to said threshold value and a point corresponding to said extreme value.

6. The determination device according to any one of claims 1 to 5 , wherein said motion detection means is an acceleration sensor attached to said arm or said hand.

7. The determination device according to any one of claims 1 to 6 , wherein said filter having no phase lag is a centered moving average filter.

8. The determination device according to claim 7 , wherein the reciprocal of the vibration frequency of said vibration component obtained by calculation or measurement is set as the moving average time of said centered moving average filter.

A determination method for determining an operation completion time of a movement operation of a robot to a commanded position, comprising:
detecting a signal indicative of velocity or acceleration of the robot arm or hand;
a filtering process for removing vibration components contained in the signal to generate smoothed data;
After detecting that the robot is decelerating, the value of the smoothed data last changed from a value outside a predetermined threshold range to a value within the predetermined threshold range. obtaining an average value and subtracting the average value from the smoothed data values to correct for offset and drift;
wherein said filtering is performed by a filter with no phase lag.

In the vicinity of the time estimated to be the operation completion time, the smoothed data for a period before the time estimated to be the operation completion time among the smoothed data corrected for the offset and drift. 10. The determination method according to claim 9 , further comprising the step of approximating with a function of time and determining the time when the value of said function becomes zero as the operation completion time.

A determination method for determining an operation completion time of a movement operation of a robot to a commanded position, comprising:
detecting a signal indicative of velocity or acceleration of the robot arm or hand;
a filtering process for removing vibration components contained in the signal to generate smoothed data;
with
the filtering is performed by a filter with no phase lag;
After the reference time when the value of the smoothed data finally changes from a value outside a predetermined threshold range to a value within the predetermined threshold range after detecting that the robot is decelerating, the reference time is approximated by a constant value, the smoothed data before the reference time is approximated by a function of time, and the time when the value of the function becomes the constant value is determined as the operation completion time . Method.

12. The determination method according to any one of claims 9 to 11 , wherein said signal is a signal indicating said acceleration from an acceleration sensor provided on said arm or said hand.