JP3122399B2

JP3122399B2 - INDUSTRIAL ROBOT, FAULT DETECTION METHOD, AND RECORDING MEDIUM CONTAINING INDUSTRIAL ROBOT FAULT DETECTION PROGRAM

Info

Publication number: JP3122399B2
Application number: JP09312884A
Authority: JP
Inventors: 耕治大野
Original assignee: Nachi Fujikoshi Corp
Current assignee: Nachi Fujikoshi Corp
Priority date: 1997-10-30
Filing date: 1997-10-30
Publication date: 2001-01-09
Anticipated expiration: 2017-10-30
Also published as: JPH11129186A

Description

DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【発明の属する技術分野】本発明は、サーボモータによ
り駆動される産業用ロボットについて、生産ラインの稼
働中にその作業内容に依存することなく、サーボ制御系
及びロボット機構部の故障の有無を検出することができ
る産業用ロボット及びその故障検出方法並びに故障検出
プログラムを記録した記録媒体に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an industrial robot driven by a servomotor which detects whether or not a servo control system and a robot mechanism have a failure during the operation of a production line without depending on the contents of the work. The present invention relates to an industrial robot capable of performing the same, a failure detection method thereof, and a recording medium on which a failure detection program is recorded.

【０００２】[0002]

【従来の技術】産業用ロボットは、長期にわたる使用に
より、アーム及びその駆動モータを主体とするロボット
機構部の駆動力伝達系を構成する歯車等が摩耗し、これ
によりその動作や制御精度等に悪影響を来すことにな
る。一般に、このような現象は外部から容易に予測でき
ず、また悪影響が現れるまでは放置されがちになるの
で、以前は歯車等が破損するような状況が生ずるように
なってから対策がとられることが頻繁にあった。2. Description of the Related Art In an industrial robot, the gears and the like constituting a driving force transmission system of a robot mechanism mainly including an arm and a driving motor thereof wear due to long-term use, thereby reducing the operation and control accuracy. It will have adverse effects. In general, such phenomena cannot be easily predicted from the outside, and tend to be left unattended until adverse effects appear. There was often.

【０００３】これに対処するために、例えば特開昭６３
−１２３１０５号では、ロボットが正常な状態のとき、
例えばロボット本体の新規導入時や定期のメンテナンス
終了時に、予め設定された基準となる動作パターンにて
ロボットを実際に動作させ、このときの動作データを基
準データとして予め測定し、この基準データをロボット
制御装置の記憶装置に記録しておく。そして、所定の動
作時間を経過した後に、定期的に、再度同様にして基準
となる動作パターンにてロボットを実際に動作させ、こ
のときの動作データを測定し、この動作データを前述の
基準データと比較し、その差異から故障の有無を診断す
るようにしている。この動作データとして、特開昭６３
−１２３１０５号では、サーボ制御ずれ（偏差）を用い
ているため、故障の有無を診断するための専用のセンサ
を設ける必要がないとされ、また、アーム及びその駆動
モータ・減速機等を主体とするロボット機構部、及びこ
のロボット機構部を制御するサーボ制御系を総合的に診
断することができるという利点があるとされている。In order to deal with this, for example, Japanese Patent Application Laid-Open
In -123105, when the robot is in a normal state,
For example, at the time of new introduction of the robot body or at the end of regular maintenance, the robot is actually operated according to a preset reference operation pattern, the operation data at this time is measured in advance as reference data, and this reference data is It is recorded in the storage device of the control device. Then, after a predetermined operation time has elapsed, the robot is actually operated again in the same manner as the reference operation pattern, and the operation data at this time is measured. And the difference is diagnosed based on the difference. As this operation data,
According to -123105, servo control deviation (deviation) is used, so it is not necessary to provide a dedicated sensor for diagnosing the presence or absence of a failure. In addition, the arm and its drive motor, reduction gear, etc. are mainly used. It is said that there is an advantage that it is possible to make a comprehensive diagnosis of the robot mechanism that performs and the servo control system that controls the robot mechanism.

【０００４】[0004]

【発明が解決しようとする課題】しかし、この特開昭６
３−１２３１０５号の方法では、ロボットの正常状態に
おける動作データを基準データとして個々のロボット毎
に測定し、この基準データを記憶しておく必要性があ
り、また、診断過程においては基準となる動作パターン
でロボットを動作させなければならないために、生産ラ
インの稼働中には診断を行えないという問題点があり、
さらには、生産ラインの稼働時における実際の動作パタ
ーンに特化したような故障については、診断精度を上げ
ることが難しいという問題点もあった。However, Japanese Patent Application Laid-Open Publication No.
In the method of 3-123105, it is necessary to measure the operation data in the normal state of the robot for each individual robot as reference data and store the reference data. Since the robot must be operated in a pattern, there is a problem that diagnosis cannot be performed while the production line is operating.
Further, there is a problem that it is difficult to improve the diagnosis accuracy for a failure that is specialized in an actual operation pattern during the operation of the production line.

【０００５】本発明は従来技術のこれらの問題点を解決
するためになされたものであり、サーボモータにより駆
動される産業用ロボットのロボット機構部及びこのロボ
ット機構部を制御するサーボ制御系の故障について、生
産ラインの稼働中にその作業内容に依存することなく、
特に、ロボットの正常状態での動作パターンにおける動
作データをを基準データとして予め測定することなく、
また、生産ラインの稼働中においても随時診断を行うこ
とができ、さらに、生産ラインの稼働時における実際の
動作パターンに特化したような故障についても高精度な
診断を行うことができるような、産業用ロボット及びそ
の故障検出方法並びに故障検出プログラムを記録した記
録媒体を提供することを目的とする。SUMMARY OF THE INVENTION The present invention has been made to solve these problems of the prior art, and is intended to solve the problems of a robot mechanism of an industrial robot driven by a servomotor and a servo control system for controlling the robot mechanism. , Without depending on the content of the work during the operation of the production line,
In particular, without previously measuring the operation data in the operation pattern in the normal state of the robot as reference data,
In addition, diagnosis can be performed at any time even during the operation of the production line, and further, it is possible to perform high-precision diagnosis even for a failure specialized in an actual operation pattern during the operation of the production line. An object of the present invention is to provide an industrial robot, a failure detection method thereof, and a recording medium on which a failure detection program is recorded.

【０００６】[0006]

【課題を解決するための手段】上記の目的を達成するた
めに、本発明では、ロボットアーム、このロボットアー
ムを減速機を介して駆動するサーボモータ、及びサーボ
モータの位置を検出するエンコーダからなるロボット機
構部と、このロボット機構部を制御する位置ループ、速
度ループ、及び電流ループからなるサーボ制御系を有す
る産業用ロボットにおいて、ロボット機構部を構成する
各駆動軸の指令角度θ_dを出力する指令位置計算部と、
指令角度θ_d及び前記エンコーダにより得られたサーボ
モータの実角度θ_iに基づいて位置指令を作成し出力す
る位置ループと、位置指令及び実角度θ_iより得られた
実角速度ω_iに基づいて速度指令を作成し出力する速度
ループと、速度指令及びサーボモータの駆動電流Ｉ_iに
基づいて電流指令を作成し出力する電流ループと、指令
角度θ_d、実角度θ_i、及び駆動電流Ｉ_iに基づいて、
サーボ制御系及びロボット機構部の故障の有無を判定す
る故障検出部と、を有する産業用ロボットにおいて、前
記故障検出部を以下の構成とすることにした。 In order to achieve the above object, the present invention provides a robot arm, a servomotor for driving the robot arm via a speed reducer, and an encoder for detecting the position of the servomotor. a robot mechanism, a position loop for controlling the robot mechanism, speed loop, and the industrial robot having the servo control system consisting of the current loop, and outputs the command angle theta _d of the drive shafts constituting the robot mechanism Command position calculator,
A position loop for creating and outputting a position command based on the actual angle theta _i of the servo motor obtained by an instruction angle theta _d and the encoder, based on the actual angular velocity omega _i obtained from the position command and the actual angle theta _i a speed loop to create a velocity command output, and a current loop that creates and outputs a current command based on the drive current I _i of the speed command and the servomotor, the command angle theta _d, actual angle theta _i, and the drive current I _i On the basis of the,
An industrial robot having a servo control system and a failure detection unit for determining the presence or absence of a failure in the robot mechanism.
The failure detection unit has the following configuration.

【０００７】すなわち、前記故障検出部では、指令角度
θ_d、実角度θ_i、及び駆動電流Ｉ_iと、この指令角度
θ_d及び実角度θ_iの微分値、すなわち指令角度θ_dを
微分して得られる指令角速度ω_d及び指令角加速度α_d
と、実角度θ_iを微分して得られる実角速度ω_i及び実
角加速度α_iとに基づいて算出されるサーボ制御系の入
力側及び出力側の仕事率よりサーボ制御系の故障の有無
を判定するとともに、同様にして算出されるロボット機
構部の入力側及び出力側の仕事率よりロボット機構部の
故障の有無を判定するようにした（請求項１）。 That is, the failure detecting section differentiates the command angle θ _d , the actual angle θ _i , and the drive current I _i and the differential value of the command angle θ _d and the actual angle θ _i , that is, the command angle θ _d. command angular velocity ω _d and command angle acceleration α _d obtained Te
When the presence or absence of a failure of the servo control system from the input side and the output side of the work rate of the servo control system is calculated based on the actual angular velocity omega _i and the actual angular acceleration alpha _i obtained by differentiating the actual angle theta _i with determination and to determine the presence or absence of a failure of the robot mechanism from the input side and the output side of the work rate of the robot mechanism which is calculated in the same manner (claim 1).

【０００８】この故障検出部において行われる故障検出
方法では、ロボット機構部を構成する各駆動軸を駆動す
るサーボモータの駆動電流Ｉ_i及び実角度θ_iに基づい
て各駆動軸の駆動側の仕事率Ｗ_iを算出し、ロボット機
構部の質点モデルに関する運動方程式と実角度θ_iに基
づいて各駆動軸の負荷側の仕事率Ｗ_oを算出し、一方、
ロボット機構部の質点モデルに関する運動方程式と各駆
動軸の指令角度θ_dに基づいて各駆動軸の指令側の仕事
率Ｗ_dを算出し、これら負荷側の仕事率Ｗ_o及び駆動側
の仕事率Ｗ_iより求められる各駆動軸のロボット機構部
の仕事率の比（Ｗ_o／Ｗ_i）又は差（Ｗ_o−Ｗ_i）を予
め設定された第１の判定値と比較することによりロボッ
ト機構部の故障の有無を判定するとともに、指令側の仕
事率Ｗ_d及び駆動側の仕事率Ｗ_iより求められる各駆動
軸のサーボ制御系の仕事率の比（Ｗ_i／Ｗ_d）又は差
（Ｗ_i−Ｗ_d）を予め設定された第２の判定値と比較す
ることによりサーボ制御系の故障の有無を判定するよう
にした（請求項２）。[0008] In failure detection method performed in the fault detection section, the work of the drive side of the drive shaft on the basis of the drive current I _i and the actual angle theta _i of the servo motor for driving the drive shaft constituting the robot mechanism calculating the rate W _i, to calculate the work rate W _o of the load side of each drive shaft based on the actual angle theta _i and motion equation relating mass model of the robot mechanical unit, whereas,
Calculating a work rate W _d of command side of each drive shaft based the motion equation relating mass model of the robot mechanical unit to a command angle theta _d of the drive shaft, the work rate of work rate W _o and the drive side of the load-side the ratio of the work rate of the robot mechanism of the drive shaft is obtained from _{_{_{W i (W o / W i}}} ) or a difference (W _o -W _i) a preset robot mechanism by comparing the first determination value In addition to determining the presence or absence of a failure of the unit, the ratio (W _i / W _d ) or the difference (W _i / W _d ) of the power of the servo control system of each drive shaft obtained from the power W _d on the command side and the power W _i on the drive side. W _i -W _d) was adapted to determine the presence or absence of a failure of the servo control system by comparing with a preset second determination value (claim 2).

【０００９】より具体的には、以下の処理手順となる。
第１の処理ステップでは、ロボット機構部を構成する各
駆動軸を駆動するサーボモータの駆動電流Ｉ_i、実角度
θ_i、実角度θ_iの微分値としての実角速度ω_i、及び
実角速度ω_iの微分値としての実角加速度α_iを取り込
む。第２の処理ステップでは、第１の処理ステップにて
取り込まれた駆動電流Ｉ_iにサーボモータのトルク定数
ｋ_iを掛けることにより各駆動軸の駆動トルクＴ_iを算
出する。第３の処理ステップでは、第２の処理ステップ
にて算出された駆動トルクＴ_iと第１の処理ステップに
て取り込まれた実角速度ω_iとの積である各駆動軸の駆
動側の仕事率Ｗ_iを算出する。More specifically, the processing procedure is as follows.
In a first process step, the drive current I _i of the servo motor for driving the drive shaft constituting the robot mechanism, actual angle theta _i, real angular velocity omega _i as a differential value of the actual angle theta _i, and the actual angular velocity omega _i take in the actual angular acceleration α _i as a differential value of the. In a second process step, to calculate the drive torque T _i of the respective drive shaft by multiplying the torque constant k _i of the servo motor to the drive current I _i taken in the first process step. In the third processing step, the power on the driving side of each drive shaft is the product of the driving torque T _i calculated in the second processing step and the actual angular velocity ω _i taken in the first processing step. to calculate the W _i.

【００１０】第４の処理ステップでは、ロボット機構部
の質点モデルに関する運動方程式と、第１の処理ステッ
プにて取り込まれた実角度θ_i、実角速度ω_i、及び実
角加速度α_iとにより各駆動軸の負荷トルクＴ_oを算出
する。第５の処理ステップでは、第４の処理ステップに
て算出された負荷トルクＴ_oと第１の処理ステップにて
取り込まれた実角速度ω_iとの積である各駆動軸の負荷
側の仕事率Ｗ_oを算出する。第６の処理ステップでは、
第５の処理ステップにて算出された負荷側の仕事率Ｗ_o
と第３の処理ステップにて算出された駆動側の仕事率Ｗ
_iとにより、各駆動軸のロボット機構部の仕事率の比
（Ｗ_o／Ｗ_i）を算出する。第７の処理ステップでは、
第６の処理ステップにて算出された仕事率の比（Ｗ_o／
Ｗ_i）を予め設定された第１の判定値と比較することに
より、ロボット機構部の故障の有無を判定する。In a fourth processing step, each of the equations of motion relating to the mass model of the robot mechanism and the actual angle θ _i , the actual angular velocity ω _i , and the actual angular acceleration α _i fetched in the first processing step are used. calculating the load torque T _o of the drive shaft. In the fifth process step, the load side of the work rate of each drive shaft is the product of the fourth processing actual angular velocity omega _i taken by the calculated and the load torque T _o the first processing step at step Calculate W _o . In the sixth processing step,
Load-side power W _o calculated in the fifth processing step
And the power W on the drive side calculated in the third processing step
By and _i, calculating the ratio of the work rate of the robot mechanism of the drive shaft (W _o / W _i). In the seventh processing step,
The power ratio calculated in the sixth processing step (W _o /
By comparing _Wi ) with a preset first determination value, it is determined whether or not the robot mechanism has a failure.

【００１１】第８の処理ステップでは、ロボット機構部
を構成する各駆動軸の指令角度θ_d、指令角度θ_dの微
分値としての指令角速度ω_d、及び指令角速度ω_dの微
分値としての指令角加速度α_dを取り込む。第９の処理
ステップでは、ロボット機構部の質点モデルに関する運
動方程式と、第８の処理ステップにて取り込まれた指令
角度θ_d、指令角速度ω_d、及び指令角加速度α_dとに
より、各駆動軸の指令トルクＴ_dを算出する。第１０の
処理ステップでは、第９の処理ステップにて算出された
指令トルクＴ_dと第８の処理ステップにて取り込まれた
指令角速度ω_dとの積である各駆動軸の指令側の仕事率
Ｗ_dを算出する。第１１の処理ステップでは、第１０の
処理ステップにて算出された指令側の仕事率Ｗ_dと第３
の処理ステップにて算出された駆動側の仕事率Ｗ_iとに
より、各駆動軸のサーボ制御系の仕事率の比（Ｗ_i／Ｗ
_d）を算出する。第１２の処理ステップでは、第１１の
処理ステップにて算出された仕事率の比（Ｗ_i／Ｗ_d）
を予め設定された第２の判定値と比較することにより、
サーボ制御系の故障の有無を判定する（請求項３）。[0011] In the eighth process step, the command of the differential value of the command angular velocity omega _d, and the command angular velocity omega _d as a differential value of the command angle theta _d, command angle theta _d of the drive shafts constituting the robot mechanism Capture the angular acceleration α _d . In the ninth processing step, each drive axis is obtained based on the equation of motion relating to the mass model of the robot mechanism, the command angle θ _d , the command angular velocity ω _d , and the command angular acceleration α _d taken in the eighth processing step. calculating a command torque T _d. In the tenth processing step, the command side of work rate of the drive shaft is the product of the ninth processing step command torque calculated by T _d and command angular velocity omega _d taken at the eighth processing step Calculate W _d . In the eleventh processing step, the power Wd on the command side calculated in the tenth processing step is compared with the third power _Wd .
The ratio of the processing by the work rate W _i of the calculated driven-side step, the work rate of the servo control system of the drive shaft (W _i / W
_d ) is calculated. In the twelfth processing step, the power ratio (W _i / W _d ) calculated in the eleventh processing step
Is compared with a preset second determination value,
It is determined whether there is a failure in the servo control system ( claim 3 ).

【００１２】なお、第６及び第７の処理ステップにおい
ては、請求項３に記載のように、負荷側の仕事率Ｗ_o及
び駆動側の仕事率Ｗ_iより各駆動軸のロボット機構部の
仕事率の比（Ｗ_o／Ｗ_i）を算出し、これを予め設定さ
れた第１の判定値と比較するようにする代わりに、負荷
側の仕事率Ｗ_o及び駆動側の仕事率Ｗ_iより各駆動軸の
ロボット機構部の仕事率の差（Ｗ_o−Ｗ_i）を算出し、
これを予め設定された第１の判定値と比較するようにし
てもよい（請求項４）。[0012] In the sixth and seventh processing steps, as described in claim 3, the work of the robot mechanism portion of the drive shaft than the work rate W _i on the load side of the work rate W _o and drive side calculating the rate ratio (W _o / W _i), instead of to compare with the first determination value set this in advance, from the work rate W _i on the load side of the work rate W _o and drive side Calculate the difference (W _o −W _i ) in the power of the robot mechanism of each drive shaft,
This may be compared with a preset first determination value ( claim 4 ).

【００１３】同様にして、第１１及び第１２の処理ステ
ップにおいては、請求項３に記載のように、指令側の仕
事率Ｗ_d及び駆動側の仕事率Ｗ_iより各駆動軸のサーボ
制御系の仕事率の比（Ｗ_i／Ｗ_d）を算出し、これを予
め設定された第２の判定値と比較するようにする代わり
に、指令側の仕事率Ｗ_d及び駆動側の仕事率Ｗ_iより各
駆動軸のサーボ制御系の仕事率の差（Ｗ_i−Ｗ_d）を算
出し、これを予め設定された第２の判定値と比較するよ
うにしてもよい（請求項５）。[0013] Similarly, in the eleventh and twelfth process step, as described in claim 3, the servo control system of the drive shaft than the work rate W _i of command side of work rate W _d and drive side the ratio of the work rate (W _i / W _d) is calculated, instead of to compare the second judgment value set this in advance, the command side of power W _d and the driving side of the power W _i was calculated from the difference between the work rate of the servo control system of the drive shaft (W _i -W _d), it may be compared with the second determination value set this in advance (claim 5).

【００１４】ところで、上述の故障検出方法は故障検出
プログラムとしてロボット制御用プログラムに組み込ま
れることにより実現され、この故障検出プログラムは記
録媒体に記録して提供される。すなわち、ロボット機構
部を構成する各駆動軸を駆動するサーボモータの駆動電
流Ｉ_i及び実角度θ_iに基づいて各駆動軸の駆動側の仕
事率Ｗ_iを算出する手順と、ロボット機構部の質点モデ
ルに関する運動方程式と実角度θ_iに基づいて各駆動軸
の負荷側の仕事率Ｗ_oを算出する手順と、ロボット機構
部の質点モデルに関する運動方程式と各駆動軸の指令角
度θ_dに基づいて各駆動軸の指令側の仕事率Ｗ_dを算出
する手順と、負荷側の仕事率Ｗ_o及び駆動側の仕事率Ｗ
_iより求められる各駆動軸のロボット機構部の仕事率の
比（Ｗ_o／Ｗ_i）又は差（Ｗ_o−Ｗ_i）を予め設定され
た第１の判定値と比較することによりロボット機構部の
故障の有無を判定する手順と、指令側の仕事率Ｗ_d及び
駆動側の仕事率Ｗ_iより求められる各駆動軸のサーボ制
御系の仕事率の比（Ｗ_i／Ｗ_d）又は差（Ｗ_i−Ｗ_d）
を予め設定された第２の判定値と比較することによりサ
ーボ制御系の故障の有無を判定する手順と、を有するこ
とを特徴とする産業用ロボット用故障検出プログラムを
記録した記録媒体として提供される（請求項６）。The above-described failure detection method is realized by being incorporated in a robot control program as a failure detection program, and the failure detection program is provided by being recorded on a recording medium. That is, a procedure for calculating the power W _i on the drive side of each drive shaft based on the drive current I _i and the actual angle θ _i of the servo motor that drives each drive shaft constituting the robot mechanism, the basis of the procedure of calculating the work rate W _o of the load side of each drive shaft based on the equation of motion and the actual angle theta _i regarding mass model, the command angle theta _d of the motion equations for mass point model of the robot mechanism section drive shafts a step of calculating the power W _d of command side of the drive shafts Te, load side of the power W _o and the drive side of the power W
_By comparing the ratio (W _o / W _i ) or difference (W _o −W _i ) or difference (W _o −W _i ) of the power of the robot mechanism of each drive shaft obtained from _i with a first judgment value set in advance. And the ratio (W _i / W _d ) or the difference (W _i / W _d ) of the power of the servo control system of each drive shaft obtained from the power W _d on the command side and the power W _i on the drive side. W _i -W _d )
And a procedure for determining the presence or absence of a failure in the servo control system by comparing with a second determination value set in advance, as a recording medium recording a failure detection program for an industrial robot. ( Claim 6 ).

【００１５】以下に、上記の構成による作用を述べる。
一般に、駆動軸への動作指令に対する実際の動作性能を
追従性と呼び、特に加減速時における追従性の善し悪し
はロボット機構部の性能に大きな影響を与えることにな
る。ここで、ロボット機構部とは、図１に示すロボット
機構部２８及びサーボ制御系２９を含む本発明のブロッ
ク図において、ロボットアーム、ロボットアームを駆動
するサーボモータ２５、及びロボットアームとサーボモ
ータ２５との間に介在する減速機等から構成されてい
る。経年変化によりロボット機構部２８の劣化が進行す
ると、駆動力伝達系を構成する歯車等が摩耗し、これに
より特に加減速時の追従性が悪化する。そして加減速時
の追従性が悪化すると、ロボット機構部２８の入力側
（サーボモータ２５の入力側）すなわち駆動側の仕事率
Ｗ_iとロボット機構部２８の出力側（減速機側）すなわ
ち負荷側の仕事率Ｗ_oとの差が大きくなって現れてく
る。一般的には、加速時には駆動側の仕事率Ｗ_iが負荷
側の仕事率Ｗ_oに対して大きくなって現れ、逆に、減速
時には負荷側の仕事率Ｗ_oが駆動側の仕事率Ｗ_iに対し
て大きくなって現れてくる。The operation of the above configuration will be described below.
Generally, the actual operation performance to the operation command to the drive shaft is referred to as followability. Particularly, the quality of the followability during acceleration / deceleration greatly affects the performance of the robot mechanism. Here, in the block diagram of the present invention including the robot mechanism unit 28 and the servo control system 29 shown in FIG. 1, the robot mechanism unit refers to a robot arm, a servo motor 25 for driving the robot arm, and the robot arm and the servo motor 25. And a speed reducer or the like interposed between them. As the deterioration of the robot mechanism 28 progresses due to aging, the gears and the like that constitute the driving force transmission system are worn, thereby deteriorating the responsiveness particularly during acceleration and deceleration. When the followability during acceleration / deceleration deteriorates, the power W _i on the input side (input side of the servo motor 25) of the robot mechanism 28, ie, the drive side, and the output side (reduction gear side) of the robot mechanism 28, ie, the load side The difference between the power and the work rate _Wo increases. In general, during acceleration, the power W _i on the drive side appears larger than the power W _o on the load side, and conversely, during deceleration, the power W _o on the load side becomes higher than the power W _i on the drive side. Appears to grow larger.

【００１６】また、追従性の善し悪しはロボット機構部
２８の劣化だけではなくサーボ制御系２９の故障に起因
する場合もある。ここで、サーボ制御系とは、図１に示
す本発明のブロック図において、ロボット機構部２８を
制御するフィードバック系からなる位置ループ２２、速
度ループ２３、及び電流ループ２４から構成されてい
る。サーボ制御系２９に故障が発生すると、サーボ制御
系２９の入力側（位置ループ２２の入力側）すなわち指
令側の仕事率Ｗ_dとサーボ制御系２９の出力側（電流ル
ープ２４の出力側）すなわち駆動側の仕事率Ｗ_iとの差
が大きくなる。なお、図１に示すように、本発明におい
ては、サーボ制御系２９の出力側（電流ループ２４の出
力側）の仕事率とロボット機構部２８の入力側（サーボ
モータ２５の入力側）の仕事率とは同一であり、これを
駆動側の仕事率Ｗ_iとして統一している。In addition, the tracking performance may be caused not only by the deterioration of the robot mechanism 28 but also by the failure of the servo control system 29. Here, the servo control system in the block diagram of the present invention shown in FIG. 1 includes a position loop 22, a speed loop 23, and a current loop 24 which are a feedback system for controlling the robot mechanism 28. If a fault in the servo control system 29 occurs, the input side of the servo control system 29 i.e. the command side work rate W _d and the output side of the servo control system 29 (the input side of the position loop 22) (output side of the current loop 24) i.e. The difference from the power W _i on the driving side increases. As shown in FIG. 1, in the present invention, the power on the output side of the servo control system 29 (the output side of the current loop 24) and the power on the input side of the robot mechanism unit 28 (the input side of the servo motor 25). The power is the same as the power, and is unified as the power W _i on the driving side.

【００１７】本発明では、上述の特性を利用することに
より、図１に示すロボット制御装置３０内に故障検出部
２７を設け、この故障検出部２７が故障の検出に必要な
データを取り込むことにより、ロボット機構部の入力側
すなわち駆動側の仕事率Ｗ_iとロボット機構部の出力側
すなわち負荷側の仕事率Ｗ_oとを比較することによりロ
ボット機構部の故障の有無が判定されるとともに、サー
ボ制御系の入力側すなわち指令側の仕事率Ｗ_dとサーボ
制御系の出力側すなわち駆動側の仕事率Ｗ_iとを比較す
ることによりサーボ制御系の故障の有無が判定されるよ
うにする。具体的には、仕事率の比（例えばＷ_o／
Ｗ_i）または差（例えばＷ_o−Ｗ_i）を予め設定した判
定値と比較し、その許容範囲内になければ故障が発生し
ているものと判定されるようにする。In the present invention, by utilizing the above characteristics, a failure detection unit 27 is provided in the robot control device 30 shown in FIG. 1, and the failure detection unit 27 takes in data necessary for detecting a failure. By comparing the power W _i on the input side of the robot mechanism, ie, the drive side, with the power W _o on the output side, ie, the load side, of the robot mechanism, the presence or absence of a failure in the robot mechanism is determined. by comparing the work rate W _i of the input side, i.e., the command side of work rate W _d and the servo control system output side or drive side of the control system so that the presence or absence of a failure of the servo control system is determined. Specifically, the power ratio (for example, _Wo /
W _i ) or a difference (for example, W _o −W _i ) is compared with a predetermined determination value, and if it is not within the allowable range, it is determined that a failure has occurred.

【００１８】故障検出部２７における指令側の仕事率Ｗ
_d、駆動側の仕事率Ｗ_i及び負荷側の仕事率Ｗ_oの算出
方法としては、駆動側の仕事率Ｗ_iは、サーボモータへ
供給される駆動電流Ｉ_i、及びサーボモータに付属ある
いは併設して設けられたエンコーダ（位置検出器）によ
り検出された駆動軸の実際の位置（実角度θ_i）の微分
値としての実角速度ω_iより算出し、また、指令側の仕
事率Ｗ_dは、指令位置計算部などの指令位置の出力装置
より出力されるロボット機構部を構成する各駆動軸の指
令位置としての指令角度θ_d、この指令角度θ_dの微分
値としての指令角速度ω_d、及びこの指令角速度ω_dの
微分値としての指令角加速度α_dと、ロボット機構部の
質点モデルに関する運動方程式とに基づいて算出し、さ
らに、負荷側の仕事率Ｗ_oは、前述の実角度θ_i、実角
速度ω_i、及びこの実角速度ω_iの微分値としての実角
加速度α_iと、ロボット機構部の質点モデルに関する運
動方程式とに基づいて算出する。The power W on the command side in the failure detection unit 27
_d , the drive-side power W _i and the load-side power W _o are calculated as follows: the drive-side power W _i is the drive current I _i supplied to the servo motor, and the drive power I _i is attached to or attached to the servo motor. actual position of the detected drive shaft by the encoder (position detector) provided with calculated from the actual angular velocity omega _i as a differential value of (actual angle theta _i), also work rate W _d directives side , command angle theta _d as commanded position of the respective drive shafts constituting the robot mechanism which is output from the output device command position, such as command position calculation unit, command angular velocity omega _d as a differential value of the command angle theta _d, and a command angular acceleration alpha _d as a differential value of the command angular velocity omega _d, is calculated based on the equation of motion related to mass model of the robot mechanical unit, further work rate W _o of the load side, the actual angle of the aforementioned θ _i, the actual angular velocity ω _i, and the actual angular velocity ω _i And the actual angular acceleration alpha _i as a differential value is calculated based on the equation of motion relating to the mass point model of the robot mechanical unit.

【００１９】[0019]

【発明の実施の形態】以下、本発明の実施形態について
図面を参照しながら説明する。図１は本発明のロボット
機構部２８及びサーボ制御系２９を含むブロック図であ
り、図中２５はロボット機構部を構成する各駆動軸を動
作させるためのサーボモータ、２６はサーボモータ２５
の位置すなわち駆動軸の実角度θ_iを検出するエンコー
ダ、２１は各駆動軸の指令角度θ_dを出力する指令位置
計算部、２２は指令角度θ_d及び駆動軸の実角度θ_iに
基づいて位置指令を作成し出力する位置ループ、２３は
位置指令と、実角度θ_iを微分器としてのラプラス演算
子Ｓにより微分することにより得られる実角速度ω_iと
に基づいて速度指令を作成し出力する速度ループ、２４
は速度指令と電流検出器により検出されたサーボモータ
の駆動電流Ｉ_iとに基づいて電流指令を作成し出力する
電流ループ、２７はロボット機構部２８及びサーボ制御
系２９の故障の有無を検出する故障検出部である。Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram including a robot mechanism section 28 and a servo control system 29 of the present invention. In the figure, reference numeral 25 denotes a servo motor for operating each drive shaft constituting the robot mechanism section;
Position or encoder for detecting the actual angle theta _i of the drive shaft, the 21 command position calculation unit for outputting a command angle theta _d of the drive shaft, 22 based on the actual angle theta _i command angle theta _d and the drive shaft position loop to create a position command output, 23 creates a position command, a speed command on the basis of the actual angular velocity omega _i obtained by the actual angle theta _i is differentiated by Laplace operator S as differentiator output Speed loop, 24
Current loop to create and output a current command based on the driving current I _i of the servo motor detected by the speed command and the current detector 27 detects the presence or absence of a failure of the robot mechanism 28 and the servo control system 29 It is a failure detection unit.

【００２０】故障検出部２７ではロボット機構部２８及
びサーボ制御系２９の故障の有無を常時監視していなく
てはならないので、故障の検出に必要なデータ、すなわ
ち指令角度θ_d、指令角速度ω_d、指令角加速度α_d、
実角度θ_i、実角速度ω_i、実角加速度α_i、及び駆動
電流Ｉ_iの各データをリアルタイムに取り込む必要があ
る。これらのデータの内、指令角度θ_dは指令位置計算
部２１から、実角度θ_iはエンコーダ２６から、駆動電
流Ｉ_iは電流検出器からそれぞれ取り込む。また、指令
角速度ω_d、指令角加速度α_d、実角速度ω_i、及び実
角加速度α_iの各データについては、指令角度θ_d及び
実角度θ_iを微分することにより求めるが、この微分処
理は故障検出部２７内で行ってもよいし、図１に示すよ
うに故障検出部２７外にある微分器Ｓで行うようにして
もよい。Since the failure detecting section 27 must constantly monitor the robot mechanism 28 and the servo control system 29 for the presence or absence of a failure, data necessary for detecting the failure, that is, the command angle θ _d and the command angular velocity ω _d , Command angular acceleration α _d ,
It is necessary to capture each data of the actual angle θ _i , the actual angular velocity ω _i , the actual angular acceleration α _i , and the drive current I _i in real time. Of these data, the command angle theta _d from the command position calculation unit 21, the actual angle theta _i is the encoder 26, the drive current I _i fetches respectively from the current detector. Further, command angular velocity omega _d, command angular acceleration alpha _d, for each data of the real angular velocity omega _i, and the actual angular acceleration alpha _i is determined by differentiating the command angle theta _d and actual angle theta _i, the differential processing May be performed in the failure detection unit 27, or may be performed in a differentiator S outside the failure detection unit 27 as shown in FIG.

【００２１】図２は、図１において示した故障検出部２
７にて行われる、ロボット機構部２８及びサーボ制御系
２９の故障の有無を判定するに至る処理の概要を示すフ
ローチャートである。ステップ６１では、ロボット機構
部２８を構成する各駆動軸を駆動するサーボモータ２５
の駆動電流Ｉ_i、及びエンコーダ２６により検出された
実角度θ_iに基づいて各駆動軸の駆動側の仕事率Ｗ_iを
算出する。ステップ６２では、ロボット機構部２８の質
点モデルに関する運動方程式と実角度θ_iに基づいて各
駆動軸の負荷側の仕事率Ｗ_oを算出する。ステップ６３
では、ロボット機構部２８の質点モデルに関する運動方
程式と各駆動軸の指令角度θ_dに基づいて各駆動軸の指
令側の仕事率Ｗ_dを算出する。FIG. 2 shows the fault detector 2 shown in FIG.
7 is a flowchart illustrating an outline of a process performed in step 7 to determine whether a failure has occurred in the robot mechanism unit 28 and the servo control system 29. In step 61, the servo motor 25 for driving each drive shaft constituting the robot mechanism 28
The power W _i on the drive side of each drive shaft is calculated based on the drive current I _i and the actual angle θ _i detected by the encoder 26. In step 62, it calculates the load side of the work rate W _o of each drive shaft based on the actual angle theta _i and motion equation relating mass point model of the robot mechanism section 28. Step 63
In calculates a work rate W _d of command side of the drive shaft in accordance with a command angle theta _d equations of motion and the drive shaft about the mass point model of the robot mechanism section 28.

【００２２】ステップ６４では、ステップ６２において
算出された負荷側の仕事率Ｗ_o及びステップ６１におい
て算出された駆動側の仕事率Ｗ_iより求められる各駆動
軸のロボット機構部の仕事率の比（Ｗ_o／Ｗ_i）又は差
（Ｗ_o−Ｗ_i）を、予め設定された第１の判定値と比較
することによりロボット機構部２８の故障の有無を判定
する。ステップ６５では、ステップ６３において算出さ
れた指令側の仕事率Ｗ_d及びステップ６１において算出
された駆動側の仕事率Ｗ_iより求められる各駆動軸のサ
ーボ制御系の仕事率の比（Ｗ_i／Ｗ_d）又は差（Ｗ_i−
Ｗ_d）を、予め設定された第２の判定値と比較すること
によりサーボ制御系２９の故障の有無を判定する。な
お、ステップ６１、６２、及び６３はそれぞれ並行して
処理を行うことが可能であり、また、ステップ６４及び
６５もそれぞれ並行して処理を行うことが可能である。[0022] At step 64, the ratio of the work rate of the robot mechanism of the drive shaft to be determined from work rate W _i of the calculated driven-side in the work rate W _o and S 61 of the calculated load in step 62 ( W _o / W _i) or a difference of (W _o -W _i), it determines the presence or absence of failure of the robot mechanism 28 by comparing with a preset first determination value. In step 65, the ratio (W _i / W) of the power of the servo control system of each drive shaft obtained from the power W _d on the command side calculated in step 63 and the power W _i on the drive side calculated in step 61. W _d ) or the difference (W _i −
By comparing W _d ) with a preset second determination value, it is determined whether the servo control system 29 has a failure. Steps 61, 62 and 63 can be performed in parallel with each other, and steps 64 and 65 can also be processed in parallel with each other.

【００２３】図３は、図２に示した処理の概要をより具
体的に示したフローチャートである。ステップ１（第１
の処理ステップ）では、各駆動軸の駆動電流Ｉ_i、実角
度θ_i、実角速度ω_i、及び実角加速度α_iを取り込
む。駆動電流Ｉ_iは電流ループ２４とサーボモータ２５
との間の伝送路上に設けられた電流検出器より取り込
む。実角度θ_iはサーボモータ２５に付属あるいは併設
されたエンコーダ２６より取り込む。実角速度ω_iはエ
ンコーダ２６により検出された実角度θ_iを微分器Ｓで
微分することにより得られた値として取り込み、さら
に、この実角速度ω_iを微分器Ｓで微分することにより
得られた値を実角加速度α_iとして取り込む。ただし、
実角速度ω_i及び実角加速度α_iは、取り込まれた実角
度θ_iを故障検出部２７内において微分処理することに
より算出するようにしてもよい。FIG. 3 is a flowchart more specifically showing the outline of the processing shown in FIG. Step 1 (first
In processing step), captures the driving current I _i of the drive shafts, actual angle theta _i, real angular velocity omega _i, and the actual angular acceleration alpha _i. The drive current I _i is controlled by the current loop 24 and the servo motor 25.
It is taken in from a current detector provided on the transmission path between. The actual angle θ _i is fetched from an encoder 26 attached to or attached to the servomotor 25. The actual angular velocity ω _i is obtained as a value obtained by differentiating the actual angle θ _i detected by the encoder 26 with the differentiator S, and further obtained by differentiating the actual angular velocity ω _i with the differentiator S. The value is taken as the actual angular acceleration α _i . However,
The actual angular velocity ω _i and the actual angular acceleration α _i may be calculated by differentiating the acquired actual angle θ _i in the failure detection unit 27.

【００２４】ステップ２（第２の処理ステップ）では、
ステップ１にて取り込まれた駆動電流Ｉ_iにサーボモー
タのトルク定数ｋ_iを掛けることにより各駆動軸の駆動
トルクＴ_iを算出する。ステップ３（第３の処理ステッ
プ）では、ステップ２にて算出された駆動トルクＴ_iと
ステップ１にて取り込まれた実角速度ω_iとの積である
各駆動軸の駆動側の仕事率Ｗ_iを算出する。In step 2 (second processing step),
The driving current I _i taken in step 1 to calculate the drive torque T _i of the respective drive shaft by multiplying the torque constant k _i of the servo motor. In step 3 (third processing step), the power W _i on the drive side of each drive shaft is the product of the drive torque T _i calculated in step 2 and the actual angular velocity ω _i taken in step 1. Is calculated.

【００２５】ステップ４（第４の処理ステップ）では、
ロボット機構部２８の質点モデルに関する運動方程式
と、ステップ１にて取り込まれた実角度θ_i、実角速度
ω_i、及び実角加速度α_iとにより各駆動軸の負荷トル
クＴ_oを算出する。ここで、負荷トルクＴ_oの算出過程
について説明する。図４は６軸構成の垂直多関節型ロボ
ットにおける質点モデルの一例を示したものである。図
中４１は負荷の質点モデル、４２〜４６は第２〜６の各
駆動軸の質点モデルを示す。また、５１〜５６は第１〜
６の各駆動軸を示す。この質点モデルに関して、式
（１）で表される運動方程式の各項を求めることによ
り、各駆動軸の負荷側に働くトルクすなわち負荷トルク
を算出する。In step 4 (fourth processing step),
Calculating a motion equation relating to the mass point model of the robot mechanism section 28, the actual angle theta _i taken in step 1, the actual angular velocity omega _i, and the load torque T _o of the drive shaft by the actual angular acceleration alpha _i. Here will be described the process of calculating the load torque T _o. FIG. 4 shows an example of a mass model in a vertical articulated robot having a six-axis configuration. In the figure, reference numeral 41 denotes a mass point model of the load, and reference numerals 42 to 46 denote mass models of the second to sixth drive shafts. In addition, 51 to 56 are the first to
6 shows each drive shaft. With respect to this mass model, the torque acting on the load side of each drive shaft, that is, the load torque, is calculated by obtaining each term of the equation of motion represented by the equation (1).

【００２６】[0026]

【数１】 (Equation 1)

【００２７】式（１）において、左辺は各軸に働く負荷
トルクを表すトルク行列である。右辺第１項は慣性力に
よるトルク、第２項は遠心力及びコリオリ力によるトル
ク、第３項はアンバランス力によるトルクをそれぞれ表
している。なお、式（１）はロボットの質点モデルに関
する一般的な運動方程式であり、本発明に固有の方程式
ではない。In Equation (1), the left side is a torque matrix representing the load torque acting on each axis. The first term on the right side represents torque due to inertial force, the second term represents torque due to centrifugal force and Coriolis force, and the third term represents torque due to unbalance force. Expression (1) is a general equation of motion related to the mass model of the robot, and is not an equation unique to the present invention.

【００２８】ステップ５（第５の処理ステップ）では、
ステップ４にて算出された負荷トルクＴ_oとステップ１
にて取り込まれた実角速度ω_iとの積である各駆動軸の
負荷側の仕事率Ｗ_oを算出する。なお、ステップ２〜３
とステップ４〜５の処理は、それぞれ並行して行わせる
ことができる。In step 5 (fifth processing step),
Step 4 load torque calculated at T _o and Step 1
Calculating a work rate W _o of the load side of the drive shaft is the product of the actual angular velocity omega _i taken in. Steps 2-3
And the processing of steps 4 and 5 can be performed in parallel.

【００２９】ステップ６（第６の処理ステップ）では、
ステップ５にて算出された負荷側の仕事率Ｗ_oとステッ
プ３にて算出された駆動側の仕事率Ｗ_iとにより、各駆
動軸のロボット機構部の仕事率の比（Ｗ_o／Ｗ_iまたは
Ｗ_i／Ｗ_o）を算出する。ステップ７（第７の処理ステ
ップ）では、ステップ６にて算出された仕事率の比（Ｗ
_o／Ｗ_iまたはＷ_i／Ｗ_o）を予め設定された第１の判
定値と比較することにより、ロボット機構部２８の故障
の有無を判定する。具体的には、第１の許容値をβ₁と
し、１＋β₁を判定値の上限値とするとともに１−β₁
をその下限値とすることにより、ロボット機構部の仕事
率の比がこの上限値と下限値の範囲内にあれば、ロボッ
ト機構部２８に故障はないものと判定する。In step 6 (sixth processing step),
Based on the load-side power W _o calculated in step 5 and the drive-side power W _i calculated in step 3, the ratio of the power of the robot mechanism of each drive shaft (W _o / W _i). Or W _i / W _o ). In step 7 (seventh processing step), the ratio of the power calculated in step 6 (W
_o / W _i or W _i / W _o ) is compared with a preset first determination value to determine whether the robot mechanism 28 has a failure. Specifically, the first allowable value is β ₁ , 1 + β ₁ is the upper limit of the determination value, and 1−β ₁
Is set to the lower limit value, and if the ratio of the power of the robot mechanism is within the range between the upper limit and the lower limit, it is determined that the robot mechanism 28 has no failure.

【００３０】なお、ステップ６〜７においては、負荷側
の仕事率Ｗ_o及び駆動側の仕事率Ｗ_iより仕事率の差
（Ｗ_o−Ｗ_iまたはＷ_i−Ｗ_o）を算出し、この仕事率
の差を予め設定された第１の判定値と比較するようにし
てもよい。具体的には、＋β₁を判定値の上限値とする
とともに−β₁をその下限値とし、仕事率の差がこの上
限値と下限値の範囲内にあれば、ロボット機構部２８に
故障はないものと判定する。[0030] In the step 6-7, and calculates the difference between the load side of the work rate W _o and drive side work rate than the work rate W _i of (W _o -W _i or W _i -W _o), this The power difference may be compared with a first determination value set in advance. Specifically, + β ₁ is set as the upper limit value of the determination value and −β ₁ is set as the lower limit value. If the difference between the powers is within the range between the upper limit value and the lower limit value, a failure occurs in the robot mechanism unit 28. It is determined that there is not.

【００３１】ステップ８（第８の処理ステップ）では、
ロボット機構部２８を構成する各駆動軸への指令角度θ
_d、指令角速度ω_d、及び指令角加速度α_dを取り込
む。指令角度θ_dは指令位置計算部２１より取り込む。
指令角速度ω_dは指令位置計算部２１から出力された指
令角度θ_dを微分器Ｓで微分することにより得られた値
として取り込み、さらに、この指令角速度ω_dを微分器
Ｓで微分することにより得られた値を指令角加速度α_d
として取り込む。ただし、指令角速度ω_d及び指令角加
速度α_dは、取り込まれた指令角度θ_dを故障検出部２
７内において微分処理することにより算出するようにし
てもよい。In step 8 (eighth processing step),
Command angle θ to each drive shaft constituting the robot mechanism 28
_d, command angular velocity ω _d, and the command angle acceleration α _d take in. Command angle theta _d fetches from the command position calculation unit 21.
The command angular velocity ω _d is obtained as a value obtained by differentiating the command angle θ _d output from the command position calculation unit 21 with the differentiator S, and further, the command angular velocity ω _d is differentiated by the differentiator S. Use the obtained value as the command angular acceleration α _d
Take in as. However, command angular velocity omega _d and command angle acceleration alpha _d is captured command angle theta _d failure detection unit 2
Alternatively, the calculation may be performed by performing a differentiation process in the processing.

【００３２】ステップ９（第９の処理ステップ）では、
ステップ４における処理と同様に、ロボット機構部２８
の質点モデルに関する運動方程式と、ステップ８にて取
り込まれた指令角度θ_d、指令角速度ω_d、及び指令角
加速度α_dとにより、各駆動軸の指令トルクＴ_dを算出
する。ステップ１０（第１０の処理ステップ）では、ス
テップ９にて算出された指令トルクＴ_dとステップ８に
て取り込まれた指令角速度ω_dとの積である各駆動軸の
指令側の仕事率Ｗ_dを算出する。なお、ステップ８〜１
０とステップ１〜５の処理は、それぞれ並行して行わせ
ることができる。In step 9 (ninth processing step),
Similar to the processing in step 4, the robot mechanism 28
The command torque _Td of each drive shaft is calculated from the equation of motion relating to the mass point model and the command angle θ _d , command angular velocity ω _d , and command angular acceleration α _d taken in step 8. Step 10 In (10th process step), the command side of work rate W _d of the drive shaft is the product of the command angular velocity omega _d taken by the command torque T _d and Step 8 calculated in step 9 Is calculated. Steps 8 to 1
0 and the processing of steps 1 to 5 can be performed in parallel.

【００３３】ステップ１１（第１１の処理ステップ）で
は、ステップ１０にて算出された指令側の仕事率Ｗ_dと
ステップ３にて算出された駆動側の仕事率Ｗ_iとによ
り、各駆動軸のサーボ制御系の仕事率の比（Ｗ_i／Ｗ_d
またはＷ_d／Ｗ_i）を算出する。ステップ１２（第１２
の処理ステップ）では、ステップ１１にて算出された仕
事率の比（Ｗ_i／Ｗ_dまたはＷ_d／Ｗ_i）を予め設定さ
れた第２の判定値と比較することにより、サーボ制御系
２９の故障の有無を判定する。具体的には、第２の許容
値をβ₂とし、１＋β₂を判定値の上限値とするととも
に１−β₂をその下限値とすることにより、サーボ制御
系の仕事率の比がこの上限値と下限値の範囲内にあれ
ば、サーボ制御系２９に故障はないものと判定する。[0033] At step 11 (eleventh processing step), by the work rate W _i of the calculated driven-side in calculated command side of work rate W _d and Step 3 in step 10, of the drive shaft Power ratio of servo control system (W _i / W _d
Alternatively, W _d / W _i ) is calculated. Step 12 (twelfth
In the processing step (2), the servo control system 29 is compared by comparing the power ratio (W _i / W _d or W _d / W _i ) calculated in step 11 with a preset second determination value. The presence or absence of a failure is determined. Specifically, the second allowable value is β ₂ , 1 + β ₂ is the upper limit value of the determination value, and 1−β ₂ is the lower limit value. If it is within the range between the value and the lower limit, it is determined that there is no failure in the servo control system 29.

【００３４】なお、ステップ１１〜１２においては、駆
動側の仕事率Ｗ_i及び指令側の仕事率Ｗ_dより仕事率の
差（Ｗ_i−Ｗ_dまたはＷ_d−Ｗ_i）を算出し、この仕事
率の差を予め設定された第２の判定値と比較するように
してもよい。具体的には、＋β₂を判定値の上限値とす
るとともに−β₂をその下限値とし、仕事率の差がこの
上限値と下限値の範囲内にあれば、サーボ制御系２９に
故障はないものと判定する。In steps 11 to 12, a difference in power (W _i -W _d or W _d -W _i ) is calculated from the power W _i on the driving side and the power W _d on the command side. The difference in the power may be compared with a preset second determination value. Specifically, + β ₂ is set as the upper limit value of the determination value and −β ₂ is set as the lower limit value. If the difference in the power is within the range between the upper limit value and the lower limit value, a fault occurs in the servo control system 29. It is determined that there is not.

【００３５】以上、本発明の一実施形態における産業用
ロボットの故障検出方法について述べたが、上述の故障
検出にかかる処理はリアルタイムに行われ、より具体的
にはロボット制御装置３０のスキャンタイム（一般には
数十ミリ秒）単位で行われるので、故障が発生すれば直
ちに判定検出されることになる。故障が検出された後
は、ロボット制御装置３０においてロボットの動作を停
止させる処理が行われたり、ロボット制御装置３０に接
続された図示しないモニタ上に故障した駆動軸や故障要
因等が表示されることになる。While the method for detecting a failure of an industrial robot according to one embodiment of the present invention has been described above, the processing relating to the above-described failure detection is performed in real time. Generally, it is performed in units of tens of milliseconds), so that if a failure occurs, it is immediately determined and detected. After the failure is detected, a process of stopping the operation of the robot is performed in the robot control device 30, and the failed drive axis, the failure factor, and the like are displayed on a monitor (not shown) connected to the robot control device 30. Will be.

【００３６】ところで、かかる故障検出方法は故障検出
プログラムとしてロボット本体の動作を制御するロボッ
ト制御用プログラムに組み込まれることにより実現さ
れ、この故障検出プログラムは記録媒体に記録して提供
される。すなわち図１に示したロボット制御装置３０に
は、図示しないが、ロボット制御用プログラムの記憶
部、演算部、及びインターフェース部等があり、例え
ば、図示しない外部のプログラム作成装置にて作成され
た故障検出プログラムは、インターフェース部を介して
ロボット制御用プログラムの記憶部に入力され、ロボッ
トの作動に応じて演算部において故障検出処理が行われ
る。Incidentally, such a failure detection method is realized by being incorporated in a robot control program for controlling the operation of the robot body as a failure detection program, and the failure detection program is provided by being recorded on a recording medium. That is, although not shown, the robot control device 30 shown in FIG. 1 includes a storage unit for a robot control program, a calculation unit, an interface unit, and the like. For example, a failure created by an external program creation device (not shown) The detection program is input to the storage unit of the robot control program via the interface unit, and the arithmetic unit performs a failure detection process according to the operation of the robot.

【００３７】したがって、記録媒体として一般によく用
いられているフレキシブルディスク（フロッピーディス
ク）を使用すれば、これに故障検出プログラムを記録さ
せることにより、プログラム作成装置とロボット制御装
置３０との間のプログラム転送手段として利用でき、さ
らに、ロボット制御装置３０内のロボット制御用プログ
ラムの記憶部に記録されている故障検出プログラムのバ
ックアップ手段としても利用できる。また、ロボット制
御用プログラムの記憶部がＲＯＭ（リード・オンリ・メ
モリ）やハードディスクにて構成されていれば、これら
も本発明における記録媒体となりうる。なお、プログラ
ム転送手段やバックアップ手段としては、ＭＯ（光磁気
ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープなど
の他の記録媒体が使用できることは言うまでもない。ま
た、本故障検出プログラムがインターネット等の通信手
段によりプロバイダのサーバよりユーザのパソコンにダ
ウンロードされるような場合は、プロバイダのサーバ
や、ユーザのパソコンのＲＡＭやハードディスク等もこ
こで言う記録媒体に該当する。Therefore, if a flexible disk (floppy disk), which is generally used as a recording medium, is used, a failure detection program is recorded on the flexible disk, and the program transfer between the program creating device and the robot controller 30 is performed. It can be used as a means, and can also be used as a backup means for a failure detection program recorded in a storage unit of a robot control program in the robot control device 30. Further, if the storage unit of the robot control program is constituted by a ROM (Read Only Memory) or a hard disk, these can also be a recording medium in the present invention. Needless to say, other recording media such as an MO (magneto-optical disk), CD-ROM, CD-R, and magnetic tape can be used as the program transfer means and backup means. If the failure detection program is downloaded to the user's personal computer from the provider's server by communication means such as the Internet, the provider's server and the RAM or hard disk of the user's personal computer also correspond to the recording medium referred to herein. I do.

【００３８】[0038]

【発明の効果】本発明によれば、ロボットアーム、ロボ
ットアームを減速機を介して駆動するサーボモータ、及
びサーボモータの位置を検出するエンコーダからなるロ
ボット機構部と、ロボット機構部を制御する位置ルー
プ、速度ループ、及び電流ループからなるサーボ制御系
を有する産業用ロボットにおいて、ロボット機構部を構
成する各駆動軸への指令データとしての指令角度θ_d、
各駆動軸を駆動するサーボモータの位置データとしての
実角度θ_i、及びサーボモータの駆動電流Ｉ_iに基づい
て、サーボ制御系の入力側である指令側の仕事率Ｗ_d、
サーボ制御系の出力側あるいはロボット機構部の入力側
である駆動側の仕事率Ｗ_i、及びロボット機構部の出力
側である負荷側の仕事率Ｗ_oを算出し、負荷側と駆動側
との仕事率の比または差よりロボット機構部の故障の有
無を判定するとともに、指令側と駆動側との仕事率の比
または差よりサーボ制御系の故障の有無を判定するよう
にした。According to the present invention, a robot mechanism comprising a robot arm, a servomotor for driving the robot arm via a speed reducer, and an encoder for detecting the position of the servomotor, and a position for controlling the robot mechanism. In an industrial robot having a servo control system consisting of a loop, a speed loop, and a current loop, a command angle θ _d as command data to each drive axis constituting the robot mechanism,
Based on the actual angle θ _i as position data of the servo motor driving each drive shaft and the drive current I _i of the servo motor, the power W _d on the command side, which is the input side of the servo control system,
Calculate the power W _i on the drive side, which is the output side of the servo control system or the input side of the robot mechanism, and the power W _o , which is the output side of the robot mechanism, on the load side. The presence or absence of a failure in the robot mechanism is determined from the ratio or difference of the power, and the presence or absence of a failure in the servo control system is determined from the ratio or difference of the power between the command side and the drive side.

【００３９】そのため、実際の動作パターンに応じて時
々刻々変化する仕事率を判定に利用したことにより、ロ
ボットの正常状態における基準となる動作パターンにお
ける動作データを基準データとして予め測定することな
く、また、生産ラインの稼働中においても随時診断を行
うことができ、さらに、生産ラインの稼働時における実
際の動作パターンに特化したようなものについても、ロ
ボット機構部とサーボ制御系のそれぞれの故障を診断で
きるようになった。Therefore, by using the power that changes every moment according to the actual operation pattern for the determination, the operation data in the reference operation pattern in the normal state of the robot is not measured in advance as the reference data. Diagnosis can be performed at any time during the operation of the production line.Furthermore, even for those specialized to the actual operation pattern during the operation of the production line, the failure of each of the robot mechanism and the servo control system can be checked. You can now diagnose.

【００４０】本発明の故障検出方法で使用されるデータ
すなわち指令角度θ_d、実角度θ_i、及び駆動電流Ｉ_i
は、既存のロボットシステムにおいても具備されている
サーボモータに付属のエンコーダや電流検出器により取
得しているので、本発明が従来技術に対して追加するハ
ードウエアとしてはロボット制御装置内の故障検出部の
みである。また、ソフトウエアとしては従来のロボット
制御プログラムに対して本発明の故障検出プログラムを
追加するのみでよい。そのため、ロボットの新規製作時
における原価の上昇は僅かであり、また、従来のロボッ
トに本発明の技術を追加する際にも僅かな改造のみで対
応できるという利点がある。The data used in the failure detection method of the present invention, that is, the command angle θ _d , the actual angle θ _i , and the drive current I _i
Is obtained by an encoder and a current detector attached to the servo motor provided in the existing robot system, so that the hardware added by the present invention to the prior art is a failure detection in the robot controller. Department only. As software, it is only necessary to add the failure detection program of the present invention to the conventional robot control program. For this reason, there is an advantage that the cost increase at the time of new production of the robot is slight, and even when the technology of the present invention is added to a conventional robot, it can be dealt with with only a slight modification.

[Brief description of the drawings]

【図１】本発明の一実施形態における、ロボット機構部
２８及びサーボ制御系２９を含むブロック図である。FIG. 1 is a block diagram including a robot mechanism 28 and a servo control system 29 according to an embodiment of the present invention.

【図２】本発明の一実施形態における、ロボット機構部
２８及びサーボ制御系２９の診断手順の概要を示すフロ
ーチャートである。FIG. 2 is a flowchart illustrating an outline of a diagnosis procedure of a robot mechanism unit and a servo control system in an embodiment of the present invention.

【図３】本発明の一実施形態における、ロボット機構部
２８及びサーボ制御系２９の診断手順の具体例を示すフ
ローチャートである。FIG. 3 is a flowchart illustrating a specific example of a diagnosis procedure of the robot mechanism unit 28 and the servo control system 29 according to the embodiment of the present invention.

【図４】６軸構成の垂直多関節型ロボットにおける質点
モデルの一例を示す図である。FIG. 4 is a diagram illustrating an example of a mass model in a vertical articulated robot having a six-axis configuration.

[Explanation of symbols]

２１指令位置計算部２２位置ループ２３速度ループ２４電流ループ２５サーボモータ２６エンコーダ２７故障検出部２８ロボット機構部２９サーボ制御系３０ロボット制御装置 21 Command Position Calculator 22 Position Loop 23 Speed Loop 24 Current Loop 25 Servo Motor 26 Encoder 27 Failure Detector 28 Robot Mechanism 29 Servo Control System 30 Robot Controller

Claims

(57) [Claims]

A robot mechanism comprising a robot arm, a servomotor for driving the robot arm via a speed reducer, and an encoder for detecting a position of the servomotor;
A position loop and a speed loop for controlling the robot mechanism,
And in industrial robot having a servo control system consisting of the current loop, the command position calculation unit for outputting a command angle theta _d of the drive shafts constituting the robot mechanism, obtained by finger-old angle theta _d and the encoder A position loop for creating and outputting a position command based on the actual angle θ _i of the servomotor; and an actual angular velocity ω obtained from the position command and the actual angle θ _i
a velocity loop that creates and outputs a speed command based on _i, a current loop for creating and outputting a current command based on the drive current I _i in the velocity command and the servomotor, the command angle theta _d, actual angle theta _i , and based on the driving current I _i, anda failure detection unit determines the presence or absence of failure of the servo control system and the robot mechanism, in the failure detecting unit, the command angle theta _d, actual angle theta _i ,
And fine drive current I _i, obtained by differentiating the command angle theta _d finger
And the decree angular velocity ω _d and command angle acceleration α _d, the actual angle θ _i fine
Min and the actual angular velocity omega _i and the actual angular acceleration alpha _i and based on obtained
Input side and output side of the servo control system calculated based on
And whether the servo control system has a failure from the power of
The input of the robot mechanism calculated in the same manner
Whether there is a failure in the robot mechanism based on the power on the output side and output side
Industrial robots, characterized in that
G.

2. A robot mechanism comprising a robot arm, a servomotor for driving the robot arm via a speed reducer, and an encoder for detecting the position of the servomotor.
A position loop and a speed loop for controlling the robot mechanism,
And in the fault detecting method for an industrial robot having a servo control system consisting of the current loop of each drive shaft based on the driving current I _i and the actual angle theta _i of the servo motor for driving the drive shaft constituting the robot mechanism The power W _i on the drive side is calculated, and the power W _o on the load side of each drive shaft is calculated based on the equation of motion for the mass model of the robot mechanism and the actual angle θ _i . calculating a work rate W _d of command side of the drive shaft in accordance with a command angle theta _d of the motion equation drive shafts relates to mass model, obtained from the work rate W _i of the load-side work rate W _o and drive side Of the power of the robot mechanism of each drive shaft (W _o
/ W _i) or a difference (W _o -W _i) as well as determining the presence or absence of failure of the robot mechanical unit by comparing the first determination value that is set in advance, the command-side work rate W _d and The ratio of the power of the servo control system of each drive shaft obtained from the power W _i of the drive side (W _i /
W _d) or the difference (W _i -W _d second which) has been set in advance by comparing the determined value of the industrial robot, characterized in that so as to determine the presence or absence of failure of the servo control system Failure detection method.

3. A robot mechanism comprising a robot arm, a servomotor for driving the robot arm via a speed reducer, and an encoder for detecting the position of the servomotor.
A position loop and a speed loop for controlling the robot mechanism,
And in the fault detecting method for an industrial robot having a servo control system consisting of the current loop, the drive current I _i of the servo motor for driving the drive shaft constituting the robot mechanism, actual angle theta _i, of said actual angle theta _i actual angular velocity omega _i as a differential value, and a first processing step of capturing an actual angular acceleration alpha _i as a differential value of said actual angular velocity omega _i, multiplying the torque constant k _i of the servo motor to the driving current I _i A second processing step of calculating a drive torque T _i of each drive shaft according to the following formula; and calculating a power W _i on the drive side of each drive shaft which is a product of the drive torque T _i and the actual angular speed ω _i . and third processing step, a motion equation relating to the mass point model of the robot mechanical unit, the actual angle theta _i, real angular velocity omega _i, and real by the angular acceleration alpha _i of the fourth calculating the load torque T _o of the drive shaft Processing steps and the load A fifth processing step of calculating a work rate W _o of the load side of the drive shaft which is the the torque T _o product of the real angular velocity omega _i, work rate of the load-side W _o and work rate of the driving-side the ratio of the work rate of the robot mechanism of the drive shaft than _{_{_{W i (W o / W i}}} )
Calculating the power ratio of the robot mechanism (W _o / W _i ) with a first determination value set in advance to determine whether the robot mechanism has a failure. a seventh processing step of determining, as the differential value of the command angular velocity omega _d, and finger-old angular velocity omega _d as a differential value of the command angle theta _d, finger-old angle theta _d of the drive shafts constituting the robot mechanism an eighth processing step of capturing command angle acceleration alpha _d, a motion equation relating to the mass point model of the robot mechanical unit, the command angle theta _d, command angular velocity omega _d, and the command angular acceleration alpha
calculating a ninth processing step of calculating a command torque T _d of the drive shaft, the finger-old torque T _d and the command angular velocity ω work rate W _d of command side of the drive shaft is the product of _d by the _d A tenth processing step to calculate a power ratio (W _i / W _d ) of the servo control system of each drive shaft from the power W _d on the command side and the power W _i on the drive side. And a twelfth step of comparing the power ratio (W _i / W _d ) of the servo control system with a preset second determination value to determine whether the servo control system has a failure. A method for detecting a failure of an industrial robot, comprising: a processing step.

4. A claimed sixth and seventh processing step according to claim 3, the difference between the load side of the work rate W _o and work rate of the robot mechanism of the drive shaft than the work rate W _i of the drive-side A sixth processing step of calculating (W _o -W _i ); and comparing the power difference (W _o -W _i ) of the robot mechanism with a first determination value set in advance. The method according to claim 3 , further comprising: a seventh processing step of determining whether there is a failure in the mechanical unit.

Eleventh and twelfth process step according to 5. The method of claim 3, the difference in work rate of the servo control system of the drive shaft than the work rate W _i of work rate W _d and the drive side of the command side An eleventh processing step of calculating (W _i −W _d ); and comparing the power difference (W _i −W _d ) of the servo control system with a preset second determination value. The method according to claim 3 or 4 , wherein a twelfth processing step of determining whether a control system has failed is performed.

6. A robot mechanism comprising a robot arm, a servo motor for driving the robot arm via a speed reducer, and an encoder for detecting the position of the servo motor.
A position loop and a speed loop for controlling the robot mechanism,
And a recording medium recording a control program for performing the failure detection of the industrial robot having the servo control system consisting of the current loop, the driving of the servo motor for driving the drive shaft constituting the robot mechanism current I _i Calculating the power W _i on the drive side of each drive shaft based on the actual angle θ _i and the equation of motion relating to the mass model of the robot mechanism and the load side of each drive shaft based on the actual angle θ _i . Calculating the power W _o , calculating the power W _d on the command side of each drive shaft based on the equation of motion for the mass model of the robot mechanism and the command angle θ _d of each drive shaft, the ratio of the side of the work rate W _o and a robot mechanism section work rate of each drive shaft to be determined from work rate W _i of the drive side (W _o
/ W _i ) or the difference (W _o −W _i ) with a preset first determination value to determine the presence or absence of a failure in the robot mechanism, and a power W _d on the command side. and the ratio of the work rate of the servo control system of the drive shaft to be determined from work rate W _i of the drive side (W _i /
W _d ) or a difference (W _i −W _d ) with a second predetermined determination value to determine whether there is a failure in the servo control system. A recording medium that stores a robot failure detection program.