JP5868266B2

JP5868266B2 - Elastic deformation compensation control apparatus and control method for articulated robot

Info

Publication number: JP5868266B2
Application number: JP2012123241A
Authority: JP
Inventors: 西田　吉晴; 吉晴西田; 尭和田; 芳英井上; 修一稲田
Original assignee: Kobe Steel Ltd
Current assignee: Kobe Steel Ltd
Priority date: 2012-05-30
Filing date: 2012-05-30
Publication date: 2016-02-24
Anticipated expiration: 2032-05-30
Also published as: JP2013248683A

Description

本発明は、たとえば、アーク溶接に用いられる多関節ロボットの制御に関し、特に、高い軌跡精度でウィービング動作を可能とする多関節ロボットの軌跡制御に関する。 The present invention relates to control of an articulated robot used for arc welding, for example, and more particularly to trajectory control of an articulated robot that enables a weaving operation with high trajectory accuracy.

アーク溶接により複数の母材の溶接を行う際には、溶接電極を溶接方向に進ませつつ、溶接線の左右方向に正弦波ウィービング動作をさせながら溶接するウィービング溶接が採用される。このウィービング溶接は、従来から、溶接トーチ自体を左右に揺動させるか、または溶接トーチ自体を中心として左右に傾動させることにより行っている。このようなウィービング溶接を多関節ロボットに行わせる場合、高い軌跡精度が要求される。 When welding a plurality of base materials by arc welding, weaving welding is employed in which welding is performed while a sine wave weaving operation is performed in the left-right direction of the welding line while the welding electrode is advanced in the welding direction. Conventionally, this weaving welding is performed by swinging the welding torch itself to the left or right, or tilting the welding torch left and right about the welding torch itself. When letting an articulated robot perform such weaving welding, high trajectory accuracy is required.

このような多関節ロボットにおいては、各軸単位でサーボ制御されているが、固有振動数が低いため、振動抑制の観点から、速度フィードフォワードなどはほとんど適用されず、目標値に対して実際のフィードバック値の位相遅れは大きく、サーボ制御部の速度制御部の応答特性が軸毎に異なり、軌跡誤差に繋がっていた。また、このような多関節ロボットの各軸を動作させるモータは、減速器を介してアームに結合されている。この減速器の剛性不足などに起因する弾性変形を補正する場合、モータが指令値どおりに動作することが前提となっているが、フィードフォワードなど十分機能していないため、モータが指令値どおりに動作することはほぼ不可能であり、弾性変形補償は十分に機能していなかった。このような多関節ロボットの弾性変形補償制御について、以下のような技術が公知である。 In such an articulated robot, servo control is performed for each axis unit, but since the natural frequency is low, speed feed forward is hardly applied from the viewpoint of vibration suppression, and the actual value is not compared with the target value. The phase delay of the feedback value is large, and the response characteristics of the speed control unit of the servo control unit differ from axis to axis, leading to a trajectory error. Moreover, the motor which operates each axis | shaft of such an articulated robot is couple | bonded with the arm via the speed reducer. When correcting elastic deformation due to insufficient rigidity of the speed reducer, etc., it is assumed that the motor operates according to the command value. It was almost impossible to work and elastic deformation compensation was not functioning well. The following techniques are known for such elastic deformation compensation control of an articulated robot.

特開昭６１−２０１３０４号公報（特許文献１）は、減速器等の関節郡の機械的な剛性が低い場合でも、位置指令値に対してロボットアームを高精度に位置制御する方法を開示する。この位置制御方法は、ロボットを構成する各アームの位置指令値と、それを１階微分して得た速度と、２階微分して得た加速度とを、各アーム間の関節の機械的剛性を考慮したロボットアームの運動方程式に代入することにより各関節に加わるトルクを算出し、求めたトルクを定数あるいは関数あるいは制御装置内のテーブルとして与えられた各関節の機械的ばね剛性で除することによって各関節の機械的剛性に因るたわみ角を求め、求めたたわみ角を各関節のたわみを打ち消すように位置指令値と和して新たな位置指令値とすることを特徴とする。 Japanese Patent Application Laid-Open No. 61-201304 (Patent Document 1) discloses a method for controlling the position of a robot arm with high accuracy with respect to a position command value even when the mechanical rigidity of a joint group such as a speed reducer is low. . In this position control method, the position command value of each arm constituting the robot, the speed obtained by first-order differentiation of the position command, and the acceleration obtained by second-order differentiation are used as the mechanical rigidity of the joint between the arms. The torque applied to each joint is calculated by substituting it into the equation of motion of the robot arm that takes into account, and the calculated torque is divided by a constant or function or the mechanical spring stiffness of each joint given as a table in the controller Thus, a deflection angle due to the mechanical rigidity of each joint is obtained, and the obtained deflection angle is added to a position command value so as to cancel out the deflection of each joint to obtain a new position command value.

また、特開２００５−１８６２３５号公報（特許文献２）は、互いに干渉する複数軸で構成されるロボットを、干渉力が作用しても各軸が指令どおり動作するロボットの制御装置を開示する。この制御装置は、互いに干渉する複数軸から構成されるロボットであって、モータと、モータに減速機等を介して結合されたアームと、モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置であって、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉位置信号作成部とを備えたことを特徴とする。 Japanese Patent Laying-Open No. 2005-186235 (Patent Document 2) discloses a robot control apparatus in which each axis operates according to a command even when an interference force is applied to a robot composed of a plurality of axes that interfere with each other. This control device is a robot composed of a plurality of axes that interfere with each other, and is composed of a motor, an arm coupled to the motor via a speed reducer, and a motor position detector that detects the position of the motor. In addition, a robot control device having a position control unit and a speed control unit for operating each axis according to a command for each axis, and calculating an interference force acting on another axis from the command of the own axis The motor torque command signal that causes the own axis to operate according to the command even when there is an interference force acting from the other axis from the interference force calculation unit is obtained from the command of the own axis and the calculated value of the interference force acting from the other axis. When the interference torque signal generator and the interference force acting from the other axis are present, the motor position signal that causes the own axis to operate as commanded is calculated from the command of the own axis and the calculated value of the interference force acting from the other axis. Interference position signal generator Characterized by comprising.

特開昭６１−２０１３０４号公報JP 61-201304 A 特開２００５−１８６２３５号公報JP 2005-186235 A

上述した特許文献１では、減速器等の剛性不足から生じる撓み（弾性変形）を関節角目標値等から算出し，その弾性変形を補償するようにモータへの角度指令値に弾性変形量を加算することによって位置精度の改善を図っている。しかしながら、上述のように良好な
フィードフォワード制御などが行われていないのでモータは指令値どおりに動作せず、弾性変形補償は十分に機能していない。 In Patent Document 1 described above, the bending (elastic deformation) resulting from insufficient rigidity of the reducer or the like is calculated from the joint angle target value and the like, and the elastic deformation amount is added to the angle command value to the motor so as to compensate for the elastic deformation. By doing so, position accuracy is improved. However, since good feedforward control or the like is not performed as described above, the motor does not operate according to the command value, and elastic deformation compensation does not function sufficiently.

また、特許文献２では、各軸間の干渉を含めた弾性変形補償補償について記載されている。しかしながら、アーム加速度の１階微分値、２階微分値が必要であり、非常にノイズに弱く、また少し急峻な動作をさせただけで、アーム加速度の２階微分値は天文学的な値を示すなど、実現する上で非常に制約が大きい。
すなわち、従来技術では、以下のような問題を解決できていない。 Patent Document 2 describes elastic deformation compensation compensation including interference between axes. However, the first-order differential value and second-order differential value of the arm acceleration are required, and the second-order differential value of the arm acceleration shows an astronomical value even if it is very sensitive to noise and made a little steeper. For example, the restrictions are very large.
That is, the following problems cannot be solved by the conventional technology.

（１）ロボットの固有振動が低い状態で、弾性変形補償および軸力トルク補償を有効に作用させることができないため、弾性変形の影響を補償できず、精度劣化につながっている。
（２）溶接ロボットのウィービング動作では、ウィービング周期での位相遅れおよびゲイン特性を各軸そろえることが非常に重要であるが、減速器による弾性変形によるサーボ特性変化や軸毎での特性の差異によって、位相・ゲイン特性を高周波ウィービング動作でそろえることが非常に困難である。さらに、外乱として溶接ロボットのアームが障害物に衝突したりして加振力が加わったときに、速やかにその外乱に基づく振動を早期に収束できず、高精度なウィービング動作を実現できていない。 (1) Since the elastic deformation compensation and the axial force torque compensation cannot be effectively applied in a state where the natural vibration of the robot is low, the influence of the elastic deformation cannot be compensated, leading to deterioration in accuracy.
(2) In the weaving operation of a welding robot, it is very important to align the phase lag and gain characteristics in the weaving cycle for each axis. However, depending on the servo characteristic change due to elastic deformation by the reducer and the difference in the characteristics of each axis. It is very difficult to align the phase / gain characteristics with the high frequency weaving operation. Furthermore, when the arm of the welding robot collides with an obstacle as a disturbance and an excitation force is applied, the vibration based on the disturbance cannot be quickly converged, and a high-precision weaving operation cannot be realized. .

本発明は、上述の問題に鑑みてなされたものであり、複数軸を備えた多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる、多関節ロボットの弾性変形補償制御装置および制御方法を提供することを目的とする。 The present invention has been made in view of the above-described problems. In an articulated robot having a plurality of axes, it is possible to perform an operation such as weaving with high trajectory accuracy by compensating for the elastic deformation of each axis. It is an object of the present invention to provide an elastic deformation compensation control device and control method for an articulated robot.

上記課題を解決するため、本発明に係る多関節ロボットの弾性変形補償制御装置は、以下の技術的手段を講じている。
即ち、本発明に係る多関節ロボットの弾性変形補償制御装置は、多関節ロボットの関節軸を駆動するモータとアームとが弾性変形する減速器を介して結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を駆動させる多関節ロボットの弾性変形補償制御装置において、所望のツール動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算部と、前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算部と、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算部と、ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算部と、前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御部と、前記第１の動特性演算部よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、関節角度指令値θｌｃまたは関節角速度指令値θｌｃ’をフィルタリング処理して、処理後の関節角度目標値θｌｄまたは関節角速度目標値θｌｄ’を出力する第２の動特性演算部と、検出された関節角度または関節角速度に基づいて、関節角度の状態量θｌまたは関節角速度の状態量θｌ’を推定して出力する観測器と、前記第２の動特性演算部から出力される関節角度目標値θｌｄまたは関節角速度目標値θｌｄ’と、前記観測器から出力される関節角度の状態量θｌまたは関節角速度の状態量θｌ’とに基づいて、制御量を算出する制御器と、前記モータ角度制御部から出力されるモータトルク指令値に、前記制御器から出力される制御量が加算された値が目標値として入力されるモータ電流制御部と、を含んで構成されていて、前記第１の動特性演算部よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算部への入力および前記軸力トルク計算部からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第３の動特性演算部をさらに含んで構成され、前記モータ電流制御部には、前記目標値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるように構成されていることを特徴とする。 In order to solve the above problems, the elastic deformation compensation control apparatus for an articulated robot according to the present invention employs the following technical means.
That is, the elastic deformation compensation control device for an articulated robot according to the present invention is a tool attached to an articulated robot in which a motor that drives a joint axis of the articulated robot and an arm are coupled via a speed reducer that is elastically deformed. Calculates and outputs a joint angle command value θlc of each joint axis for realizing a desired tool operation in an elastic deformation compensation control apparatus for an articulated robot that drives a plurality of joint axes so as to perform a desired motion. A joint angle command value calculating unit that calculates the axial force torque fc acting on each joint axis that is generated when operating according to the joint angle command value θlc, based on the joint angle command value θlc. A motor angle command value θmc is calculated from the joint angle command value θlc and the axial force torque fc based on the output axial force torque calculation unit and parameters including the stiffness parameter of the joint shaft. A motor angle command value calculation unit that outputs the motor angle and a high-frequency cutoff characteristic having a cut-off frequency lower than the natural vibration frequency of the robot. The motor angle command value θmc is subjected to filtering processing, and the processed motor angle target value is processed. A first dynamic characteristic calculation unit that outputs θmd, a motor angle control unit that receives the motor angle target value θmd as a target value for the motor, and a cut that is lower or equivalent to the first dynamic characteristic calculation unit A second motion that has a high-frequency cutoff characteristic having an off-frequency, filters the joint angle command value θlc or the joint angular velocity command value θlc ′, and outputs the processed joint angle target value θld or the joint angular velocity target value θld ′. a characteristic calculating section, based on the detected joint angle or joint angular velocity, a state quantity .theta.l or joint angular velocity of the joint angle state quantity .theta.l 'estimated And an observation device for outputting said the joint angle target value Shitald or joint angular velocity target value Shitald 'output from the second dynamic characteristic calculation unit, the state quantity θl or joint angular velocity of the joint angle output from the observer Based on the state quantity θl ′, the controller calculates the control amount, and the value obtained by adding the control amount output from the controller to the motor torque command value output from the motor angle control unit is the target. A motor current control unit that is input as a value, and includes a high-frequency cutoff characteristic that has a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation unit, and the axial force torque calculation unit And a third dynamic characteristic calculation unit that outputs a processed axial force torque compensation value fd by filtering at least one of the input to the output and the output from the axial force torque calculation unit. The motor current control unit to the target value, the value where the axial force torque compensation value fd is added is characterized by being configured so as to be input as a desired value.

さらに好ましくは、前記第２の動特性演算部は、前記第３の動特性演算部と同じように構成することができる。
さらに好ましくは、前記観測器は、軸力トルク補償値ｆを推定して出力するように構成され、前記モータ電流制御部には、前記目標値に、前記軸力トルク補償値ｆが加算された値が目標値として入力されるように構成することができる。 More preferably, the second dynamic characteristic calculation unit can be configured in the same manner as the third dynamic characteristic calculation unit.
More preferably, the observer is configured to estimate and output an axial force torque compensation value f, and in the motor current control unit, the axial force torque compensation value f is added to the target value. A value can be configured to be input as a target value.

さらに好ましくは、前記制御器は、ＰＩＤ制御器であるように構成することができる。
また、本発明の別の形態に係る多関節ロボットの弾性変形補償制御方法は、多関節ロボットの関節軸を駆動するモータとアームとが弾性変形する減速器を介して結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を駆動させる多関節ロボットの弾性変形補償制御方法において、所望のツール動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算ステップと、
前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算ステップと、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算ステップと、ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算ステップと、前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御ステップと、前記第１の動特性演算ステップよりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、関節角度指令値θｌｃまたは関節角速度指令値θｌｃ’をフィルタリング処理して、処理後の関節角度目標値θｌｄまたは関節角速度目標値θｌｄ’を出力する第２の動特性演算ステップと、観測器を用いて、検出された関節角度または関節角速度に基づいて、関節角度の状態量θｌまたは関節角速度の状態量θｌ’を推定して出力する観測ステップと、前記第２の動特性演算ステップから出力される関節角度目標値θｌｄまたは関節角速度目標値θｌｄ’と、前記観測ステップから出力される関節角度の状態量θｌまたは関節角速度の状態量θｌ’とに基づいて、制御量を算出する制御ステップと、前記モータ角度制御ステップから出力されるモータトルク指令値に、前記制御ステップから出力される制御量が加算された値が目標値として入力されるモータ電流制御ステップと、を含んで構成されていて、前記第１の動特性演算ステップよりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算ステップへの入力および前記軸力トルク計算ステップからの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第３の動特性演算ステップをさらに含んで構成され、前記モータ電流制御ステップでは、前記目標値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されることを特徴とする。 More preferably, the controller can be configured to be a PID controller.
An elastic deformation compensation control method for an articulated robot according to another aspect of the present invention is an articulated robot in which a motor that drives a joint axis of an articulated robot and an arm are coupled via a speed reducer that is elastically deformed. In an elastic deformation compensation control method for an articulated robot that drives a plurality of joint axes so that an attached tool performs a desired motion, a joint angle command value θlc for each joint shaft for realizing a desired tool motion is obtained. Calculating and outputting a joint angle command value calculating step;
An axial force torque calculating step of calculating and outputting an axial force torque fc acting on each joint axis generated when operating according to the joint angle command value θlc from the joint angle command value θlc based on a dynamic model; A motor angle command value calculation step for calculating and outputting a motor angle command value θmc from the joint angle command value θlc and the axial force torque fc based on parameters including the stiffness parameter of the joint axis, and a natural vibration frequency of the robot A first dynamic characteristic calculation step having a high frequency cutoff characteristic having a low cut-off frequency, filtering the motor angle command value θmc, and outputting a processed motor angle target value θmd; and the motor angle target value θmd Is lower than the motor angle control step that is input as a target value for the motor and the first dynamic characteristic calculation step. Or a high-frequency cutoff characteristic having an equivalent cut-off frequency, filtering the joint angle command value θlc or the joint angular velocity command value θlc ′, and outputting the processed joint angle target value θld or the joint angular velocity target value θld ′ A second dynamic characteristic calculating step, and an observation step of estimating and outputting a joint angle state quantity θl or a joint angular velocity state quantity θl ′ based on the detected joint angle or joint angular velocity using an observer. If, 'and, the state quantity θl state quantity θl or joint angular velocity of the joint angle output from the observation step' the second dynamic characteristic joint angle target value is output from the computing step θld or joint angular velocity target value θld and From the control step, a control step for calculating a control amount and a motor torque command value output from the motor angle control step The value of the control amount is added to the force is configured to include a motor current control step is input as a target value, and have a low or equivalent cutoff frequency than said first dynamic characteristic calculation step A third high-frequency cutoff characteristic is provided, and at least one of the input to the axial force torque calculation step and the output from the axial force torque calculation step is subjected to filtering processing, and the processed axial force torque compensation value fd is output. A dynamic characteristic calculation step is further included, and in the motor current control step, a value obtained by adding the axial force torque compensation value fd to the target value is input as a target value .

本発明に係る弾性変形補償制御装置または制御方法を用いることにより、複数軸を備えた多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる。 By using the elastic deformation compensation control apparatus or control method according to the present invention, in an articulated robot having a plurality of axes, it is possible to compensate for the effects of elastic deformation of each axis and perform operations such as weaving with high trajectory accuracy. be able to.

本発明の実施の形態に係る弾性変形補償制御装置が適用される多関節ロボットの全体構成を示す概略図である。It is the schematic which shows the whole structure of the articulated robot to which the elastic deformation compensation control apparatus which concerns on embodiment of this invention is applied. 本発明の第１の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control device according to the first embodiment of the present invention. 図２に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of the articulated robot controlled by the control block diagram shown in FIG. 従来技術（その１）に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control device according to the prior art (part 1). 図４に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of the articulated robot controlled by the control block diagram shown in FIG. 従来技術（その２）に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control apparatus according to the prior art (part 2). 図６に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of the articulated robot controlled by the control block diagram shown in FIG. 本発明の第２の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control apparatus according to the second embodiment of the present invention. 図８に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of the articulated robot when the excitation force is added controlled by the control block diagram shown in FIG. 本発明の第３の実施の形態に係る弾性変形補償制御装置に関係する装置の制御ブロック図である。It is a control block diagram of the apparatus related to the elastic deformation compensation control apparatus which concerns on the 3rd Embodiment of this invention. 図１０に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of an articulated robot when the excitation force is added controlled by the control block diagram shown in FIG. 本発明の第３の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control apparatus according to the third embodiment of the present invention. 図１２に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。It is a figure which shows the weaving locus | trajectory of the articulated robot when the excitation force is added controlled by the control block diagram shown in FIG. 本発明の第３の実施の形態の第１の変形例に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control apparatus which concerns on the 1st modification of the 3rd Embodiment of this invention. 本発明の第３の実施の形態の第２の変形例に係る弾性変形補償制御装置の制御ブロック図である。It is a control block diagram of the elastic deformation compensation control apparatus which concerns on the 2nd modification of the 3rd Embodiment of this invention.

以下、本発明の実施の形態に係る多関節ロボットの弾性変形補償制御装置および制御方法を、図面に基づき詳しく説明する。なお、以下の説明では、同一の部品には同一の符号を付してある。それらの名称および機能も同じである。したがって、それらについての詳細な説明は繰り返さない。また、以下においては、制御対象として溶接トーチを傾動動作（ウィービング動作）させる多関節ロボットを説明するがこれは一例に過ぎない。本発明に係る弾性変形補償制御装置は、多関節ロボットの関節軸を駆動するモータとアームとが弾性変形する減速器を介して結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を駆動させるための制御に広く適用が可能である。 Hereinafter, an elastic deformation compensation control device and control method for an articulated robot according to an embodiment of the present invention will be described in detail with reference to the drawings. In the following description, the same parts are denoted by the same reference numerals. Their names and functions are also the same. Therefore, detailed description thereof will not be repeated. In the following, an articulated robot that tilts a welding torch (weaving operation) as a control target will be described, but this is only an example. An elastic deformation compensation control device according to the present invention performs a desired operation on a tool attached to an articulated robot in which a motor and an arm that drive a joint axis of the articulated robot are coupled via a speed reducer that is elastically deformed. Therefore, it can be widely applied to control for driving a plurality of joint axes.

＜第１の実施の形態＞
［全体構成］
まず、本実施の形態に係る弾性変形補償制御装置が適用される垂直多関節ロボット（以下、単に多関節ロボットと記載する場合がある）の概要について説明する。
図１は、溶接トーチを傾動動作（ウィービング動作）させるロボットの一例であって、本実施の形態に係る弾性変形補償制御装置が適用される多関節ロボット１の概要を示す図である。この多関節ロボット１は、垂直多関節型であってＪ１〜Ｊ６の６関節を備え、Ｊ６軸の先端に溶接トーチが設けられ、溶接トーチから送りだされる溶接ワイヤによりアーク溶接が行われる。この多関節ロボット１は、予め定められた溶接開始点と溶接終了点との間が溶接作業区間であって、溶接開始点と溶接終了点とを結ぶ溶接線方向に移動しつつ、溶接ワイヤを予め定められた振幅および周波数で傾動する動作（ウィービング動作）を行うようにセットされている。 <First Embodiment>
[overall structure]
First, an outline of a vertical articulated robot (hereinafter simply referred to as an articulated robot) to which the elastic deformation compensation control device according to the present embodiment is applied will be described.
FIG. 1 is an example of a robot that tilts (weaves) a welding torch, and is a diagram showing an outline of an articulated robot 1 to which an elastic deformation compensation control device according to the present embodiment is applied. This articulated robot 1 is a vertical articulated type and includes six joints J1 to J6. A welding torch is provided at the tip of the J6 axis, and arc welding is performed with a welding wire fed from the welding torch. The articulated robot 1 is a welding work section between a predetermined welding start point and a welding end point, and moves the welding wire in the direction of the welding line connecting the welding start point and the welding end point. It is set so as to perform an operation (weaving operation) tilting at a predetermined amplitude and frequency.

このような多関節ロボット１は、図示した多関節ロボット１の本体に加えて、教示ペンダントを備え各軸をサーボ制御する制御装置（サーボ制御部）と、上位コンピュータ（上位ＣＰＵ）とを含む。これらの制御装置および上位コンピュータにより、本実施の形態に係る弾性変形補償制御装置が実現されている。
制御装置（サーボ制御部）は、多関節ロボット１に設けられた溶接トーチを、予め教示したプログラムに従って、上述した溶接線に倣ってウィービング動作して移動するように制御する。教示プログラムは、制御装置に接続された教示ペンダントを使用して作成する場合や、上位コンピュータを利用したオフライン教示システムを使用して作成する場合がある。いずれの場合であっても、教示プログラムは、実際の動作の前に予め作成される。上位コンピュータでは、溶接パスが生成されたり、それに基づくウィービング動作指令が生成されたりする。 Such an articulated robot 1 includes, in addition to the main body of the articulated robot 1 shown in the figure, a control device (servo control unit) that includes a teaching pendant and servo-controls each axis, and an upper computer (upper CPU). The elastic deformation compensation control device according to the present embodiment is realized by these control devices and the host computer.
The control device (servo control unit) controls the welding torch provided in the articulated robot 1 so as to move in accordance with the above-described welding line and to move following the welding line. The teaching program may be created using a teaching pendant connected to the control device, or may be created using an offline teaching system using a host computer. In any case, the teaching program is created in advance before the actual operation. In the host computer, a welding pass is generated or a weaving operation command based on the welding pass is generated.

［制御ブロック］
図２に、図１の多関節ロボット１を制御する弾性変形補償制御装置１０の制御ブロック図を示す。上述したように、この弾性変形補償制御装置１０は、上位ＣＰＵで実現される部分とサーボ制御部で実現される部分とを含んで構成されている。
図２に示すように、この弾性変形補償制御装置１０は、多関節ロボット１に取り付けら
れたツール（ここでは溶接トーチ）に所望の動作（ここではウィービング動作）を行わせるように複数の関節軸を駆動させる。 [Control block]
FIG. 2 is a control block diagram of the elastic deformation compensation control apparatus 10 that controls the articulated robot 1 of FIG. As described above, the elastic deformation compensation control apparatus 10 includes a part realized by the host CPU and a part realized by the servo control unit.
As shown in FIG. 2, the elastic deformation compensation control apparatus 10 has a plurality of joint axes so that a tool (here, a welding torch) attached to the multi-joint robot 1 performs a desired operation (here, a weaving operation). Drive.

この弾性変形補償制御装置１０は、上位ＣＰＵで実現される、関節角度指令値計算部１００、軸力トルク計算部（「軸力ＦＦ」として図示）２００およびモータ角度指令値計算部（「弾性変形補償」として図示）６００と、サーボ制御部で実現される、第１の動特性演算部（「動特性（１）」として図示）３００、第２の動特性演算部（「動特性（２）」として図示）４００およびフィードバック制御部（「サーボ制御ＦＢ特性」として図示）５００とで構成される。フィードバック制御部５００は、モータ角度制御部５１０およびモータ電流制御部（「電流制御」として図示）５２０を含んで構成されている。なお、以下において、制御ブロックにおける要素の特性は（動特性との記載がなくても）全て動特性である。また、ＦＢの記載はフィードバックを意味し、ＦＦの記載はフィードフォワードを意味する。 The elastic deformation compensation control device 10 is realized by a host CPU, and includes a joint angle command value calculation unit 100, an axial force torque calculation unit (shown as “axial force FF”) 200, and a motor angle command value calculation unit (“elastic deformation”). Compensation "600), a first dynamic characteristic calculation unit (illustrated as" dynamic characteristic (1) ") 300, a second dynamic characteristic calculation unit (" dynamic characteristic (2) "realized by the servo control unit ”400 and a feedback control unit (illustrated as“ servo control FB characteristics ”) 500. The feedback control unit 500 includes a motor angle control unit 510 and a motor current control unit (shown as “current control”) 520. In the following, the characteristics of the elements in the control block are all dynamic characteristics (even if there is no description of dynamic characteristics). The description of FB means feedback, and the description of FF means feedforward.

関節角度指令値計算部１００は、溶接トーチのウィービング動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する。
軸力トルク計算部２００は、関節角度指令値計算部１００から出力された関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する。 The joint angle command value calculation unit 100 calculates and outputs a joint angle command value θlc of each joint axis for realizing the weaving operation of the welding torch.
The axial force torque calculator 200 calculates the axial force torque fc acting on each joint axis generated when operating according to the joint angle command value θlc output from the joint angle command value calculator 100 based on the dynamic model. Calculate and output from the joint angle command value θlc.

モータ角度指令値計算部６００は、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力する。
より詳しくは、軸力トルク計算部２００が、関節角度指令値θｌｃに基づいて指令値どおりに動作した際に各軸に作用する軸力トルクｆｃを算出し、モータ角度指令値計算部６００が、軸力トルクｆｃから軸剛性Ｋや粘性Ｂなどに基づいて（粘性は小さいため省略可能）弾性変形量ｅｃを算出し、関節角度指令値θｌｃと弾性変形量ｅｃとからモータ角度指令値θｍｃを算出する。 The motor angle command value calculation unit 600 calculates and outputs a motor angle command value θmc from the joint angle command value θlc and the axial force torque fc based on parameters including the stiffness parameter of the joint shaft.
More specifically, the axial force torque calculation unit 200 calculates the axial force torque fc acting on each axis when operating according to the command value based on the joint angle command value θlc, and the motor angle command value calculation unit 600 The elastic deformation amount ec is calculated from the axial force torque fc based on the shaft rigidity K and viscosity B (can be omitted because the viscosity is small), and the motor angle command value θmc is calculated from the joint angle command value θlc and the elastic deformation amount ec. To do.

第１の動特性演算部３００は、モータ角度指令値計算部６００から出力されたモータ角度指令値θｍｃをフィルタリング処理して処理後のモータ角度目標値θｍｄを出力する。この第１の動特性演算部３００は、多関節ロボット１の固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備える。
第２の動特性演算部４００は、軸力トルク計算部２００への入力および軸力トルク計算部２００からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する。なお、図２において、第２の動特性演算部４００は、軸力トルク計算部２００からの出力をフィルタリング処理している。この第２の動特性演算部４００は、第１の動特性演算部３００よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備える。 The first dynamic characteristic calculation unit 300 performs a filtering process on the motor angle command value θmc output from the motor angle command value calculation unit 600 and outputs the processed motor angle target value θmd. The first dynamic characteristic calculation unit 300 includes a high frequency cutoff characteristic having a cutoff frequency lower than the natural vibration frequency of the articulated robot 1.
The second dynamic characteristic calculation unit 400 performs filtering processing on at least one of the input to the axial force torque calculation unit 200 and the output from the axial force torque calculation unit 200, and outputs the processed axial force torque compensation value fd. To do. In FIG. 2, the second dynamic characteristic calculation unit 400 filters the output from the axial force torque calculation unit 200. The second dynamic characteristic calculation unit 400 includes a high-frequency cutoff characteristic having a cut-off frequency lower than or equivalent to that of the first dynamic characteristic calculation unit 300.

モータ角度制御部５１０には、モータ角度目標値θｍｄがモータに対する目標値として入力される。
モータ電流制御部５２０には、モータ角度制御部５１０から出力されるモータトルク指令値に、第２の動特性演算部４００から出力された軸力トルク補償値ｆｄが加算された値が目標値として入力される。 The motor angle control unit 510 receives the motor angle target value θmd as a target value for the motor.
The motor current control unit 520 has a value obtained by adding the axial force torque compensation value fd output from the second dynamic characteristic calculation unit 400 to the motor torque command value output from the motor angle control unit 510 as a target value. Entered.

図２に示すブロック図で示される弾性変形補償制御装置１０は、以下のような特徴を備える。
非線形項である軸力トルク計算部２００の前および／または後（ここでは後のみ）に第２の動特性演算部４００を配置し、この第２の動特性演算部４００を第１の動特性演算部３００の高周波遮断特性以上の高周波帯域を遮断する特性を与えている（第２の動特性演算部４００のカットオフ周波数は、第１の動特性演算部３００のカットオフ周波数よりも低いか同等）。 The elastic deformation compensation control device 10 shown in the block diagram shown in FIG. 2 has the following features.
A second dynamic characteristic calculation unit 400 is arranged before and / or after (here, only) the axial force torque calculation unit 200 which is a nonlinear term, and the second dynamic characteristic calculation unit 400 is used as the first dynamic characteristic. A characteristic that cuts off a high-frequency band equal to or higher than the high-frequency cutoff characteristic of the calculation unit 300 is given (is the cutoff frequency of the second dynamic characteristic calculation unit 400 lower than the cutoff frequency of the first dynamic characteristic calculation unit 300? Equivalent).

このように構成することにより、第１の動特性演算部３００により、関節角度指令値θｌｃに含まれる固有振動成分を含む高周波を抑制することができることに加えて、第２の動特性演算部４００により、軸力トルクｆｃに含まれる固有振動成分を含む高周波を抑制
することができる。これにより、多関節ロボット１に発生する高周波振動を抑制することができる。 With this configuration, the first dynamic characteristic calculation unit 300 can suppress high frequencies including the natural vibration component included in the joint angle command value θlc, and in addition, the second dynamic characteristic calculation unit 400 Thus, a high frequency including a natural vibration component included in the axial force torque fc can be suppressed. Thereby, the high frequency vibration which generate | occur | produces in the articulated robot 1 can be suppressed.

また、多関節ロボット１では、ＸＹＺ空間で低周波動作をさせても特異点近傍などのヤコビアンが急峻に変化するところでは、関節角度に変化すると２倍や３倍成分の高周波が発生する。さらに、関節角度空間にて低周波動作をさせても、非線形項は速度の２乗項などを有しているため、関節角度の２倍や３倍成分の高周波が発生する。このため、非線形項である軸力トルク計算部２００からの出力である軸力トルクｆｃを、第１の動特性演算部３００の高周波遮断特性以上の高周波遮断特性を備えた第２の動特性演算部４００で処理して軸力トルク補償値ｆｄとすることにより、多関節ロボット１に発生する高周波振動をさらに抑制している。 Further, in the articulated robot 1, when the Jacobian near the singular point changes suddenly even when the low frequency operation is performed in the XYZ space, the high frequency of the double or triple component is generated when the joint angle is changed. Further, even when low-frequency operation is performed in the joint angle space, the nonlinear term has a square term of velocity and the like, and therefore, a high frequency component having twice or three times the joint angle is generated. For this reason, the second dynamic characteristic calculation having a high frequency cutoff characteristic equal to or higher than the high frequency cutoff characteristic of the first dynamic characteristic calculation unit 300 is used for the axial force torque fc output from the axial force torque calculation unit 200 which is a nonlinear term. The high-frequency vibration generated in the articulated robot 1 is further suppressed by processing the unit 400 to obtain the axial force torque compensation value fd.

［制御特性（ウィービング軌跡）］
以上のような構成を備えた弾性変形補償制御装置１０を用いて多関節ロボット１を制御した場合の制御特性（ウィービング軌跡）について説明する。
図３に、第２の動特性演算部４００の高周波遮断特性として、第１の動特性演算部３００と同等の高周波遮断特性を与えた場合のウィービング軌跡を示す。 [Control characteristics (weaving trajectory)]
Control characteristics (weaving trajectory) when the articulated robot 1 is controlled using the elastic deformation compensation control apparatus 10 having the above-described configuration will be described.
FIG. 3 shows a weaving locus when a high frequency cutoff characteristic equivalent to that of the first dynamic characteristic calculation unit 300 is given as the high frequency cutoff characteristic of the second dynamic characteristic calculation unit 400.

これを評価するにあたり、まず従来技術（制御ブロック図およびウィービング軌跡）について説明する。
図４に、最も一般的な多関節ロボットの制御ブロック図を示す。図４に示すように、この制御ブロックは、位置制御部と速度制御部と電流制御部とから構成され、位置制御部は関節角度をフィードバック制御し、角度偏差を比例制御（Ｐ制御）し、速度指令として速度制御部に指令する。速度制御部は関節角速度をフィードバック制御し、与えられた速度指令との偏差を比例積分制御（ＰＩ制御）し、電流制御指令として電流制御に指令する。電流制御は与えられた電流制御指令に基づきモータ電流を制御する。 In evaluating this, first, the prior art (control block diagram and weaving locus) will be described.
FIG. 4 shows a control block diagram of the most common articulated robot. As shown in FIG. 4, this control block includes a position control unit, a speed control unit, and a current control unit. The position control unit performs feedback control of the joint angle, and proportionally controls (P control) the angle deviation. Command the speed controller as a speed command. The speed control unit feedback-controls the joint angular velocity, performs proportional-integral control (PI control) on the deviation from the given speed command, and commands the current control as a current control command. Current control controls the motor current based on a given current control command.

なお、多関節ロボットはメカの固有振動数が低く、目標値に固有振動を励起する成分が含まれないように、図４に示す第１の動特性演算部（動特性１）などでのフィルタ処理にて、それらの成分を抑制している。
ただし、多関節ロボットでは各軸間で干渉トルクが、重力項などを含めて非線形項として各リンクに作用し、リンクとモータとがバネ要素として作用する減速器を介して結合されており、軸力がリンクとモータとに作用・反作用として作用する。この軸力および弾性変形の影響は甚大である。特に溶接ロボットのウィービング動作では、上下にぶれることなく（上下方向の動きを発生させることなく）所望方向に所望の振幅で溶接トーチを揺動させることが必要であるために、非常に高精度な動的制御が必要とされる。 Note that the multi-joint robot has a low mechanical natural frequency, and the filter in the first dynamic characteristic calculation unit (dynamic characteristic 1) shown in FIG. In processing, those components are suppressed.
However, in an articulated robot, the interference torque between each axis acts on each link as a nonlinear term including the gravity term, etc., and the link and motor are coupled via a speed reducer that acts as a spring element. Force acts on the link and motor as an action / reaction. The influence of this axial force and elastic deformation is enormous. Particularly in the weaving operation of a welding robot, it is necessary to swing the welding torch with a desired amplitude in a desired direction without shaking up and down (without causing vertical movement). Dynamic control is required.

図５には、図４の制御ブロックで示される制御装置で多関節ロボットを制御した場合のウィービング軌跡を示す。この図５に示すように、非線形項、軸力および弾性変形が作用すると上下方向の動きが発生してしまい、ウィービング動作として全くふさわしくない。これを抑制するためには、目標値ベースで軸力を算出し、フィードフォワード補償することが考えられる。 FIG. 5 shows a weaving locus when the articulated robot is controlled by the control device shown by the control block in FIG. As shown in FIG. 5, when a nonlinear term, axial force, and elastic deformation are applied, a vertical movement occurs, which is not suitable as a weaving operation. In order to suppress this, it is conceivable to calculate the axial force on the basis of the target value and perform feedforward compensation.

図６には、このような考えに基づく、目標値ベースの従来技術に係る非線形フィードフォワード補償を実現する制御ブロック図を示す。
図７には、図６の制御ブロックで示される制御装置で多関節ロボットを制御した場合のウィービング軌跡を示す。目標値ベースのフィードフォワード補償であるため、位相遅れの影響によりフィードフォワードタイミングがずれてしまい、却って、上下方向の動きを悪化させる結果となっている。 FIG. 6 shows a control block diagram for realizing nonlinear feedforward compensation according to the target value-based prior art based on such an idea.
FIG. 7 shows a weaving locus when the articulated robot is controlled by the control device shown by the control block in FIG. Since the feedforward compensation is based on the target value, the feedforward timing is shifted due to the influence of the phase delay, and on the contrary, the vertical movement is deteriorated.

一般に、このような従来技術に係る制御では、フィードバック制御部（サーボ制御フィードバック特性）における位相特性やゲイン特性が各軸毎に異なるため、フィードフォワード制御などの位相を合わせることが困難であり、上記のような軸力トルク補償や弾性変形補償はほとんど実用化されておらず、弾性変形などの影響を抑制することは困難となっていた。 In general, in the control according to such a conventional technique, since the phase characteristic and gain characteristic in the feedback control unit (servo control feedback characteristic) are different for each axis, it is difficult to match the phase of the feedforward control or the like. Such axial force torque compensation and elastic deformation compensation as described above are hardly put into practical use, and it has been difficult to suppress the influence of elastic deformation and the like.

なお、図６では非線形フィードフォワード計算を上位ＣＰＵで行っている。非線形項の計算が非常に複雑で計算量が多いため、サーボ制御部で計算することが難しく、上位ＣＰ
Ｕにて目標値ベースでおこなっているのが一般的である。
このような従来技術に係る制御による結果（ウィービング軌跡）を示す図５および図７に対して、本実施の形態に係る弾性変形補償制御装置１０による結果（ウィービング軌跡）は、図３に示すように、上下動成分は発生しているものの従来技術に係る制御と比較して格段に上下動成分が抑制されていることがわかる。なお、図３は、第２の動特性演算部４００における高周波遮断特性を、第１の動特性演算部３００における高周波遮断特性と同等とした場合の結果である。 In FIG. 6, the non-linear feedforward calculation is performed by the host CPU. Since the calculation of the nonlinear term is very complicated and has a large amount of calculation, it is difficult to calculate by the servo control unit.
In general, this is performed on a target value basis at U.
FIG. 5 and FIG. 7 showing the results (weaving trajectory) by the control according to the conventional technology, the results (weaving trajectory) by the elastic deformation compensation control device 10 according to the present embodiment are as shown in FIG. In addition, although the vertical motion component is generated, it can be seen that the vertical motion component is remarkably suppressed as compared with the control according to the prior art. FIG. 3 shows the results when the high-frequency cutoff characteristic in the second dynamic characteristic calculation unit 400 is made equal to the high-frequency cutoff characteristic in the first dynamic characteristic calculation unit 300.

本実施の形態に係る弾性変形補償制御装置１０においては、第１の動特性演算部３００により、関節角度指令値θｌｃに含まれる固有振動成分を含む高周波を抑制し、かつ、第２の動特性演算部４００により、軸力トルクｆｃに含まれる固有振動成分を含む高周波を抑制することにより、上述した従来技術における非線形項に起因する振動を抑制している。 In the elastic deformation compensation control apparatus 10 according to the present embodiment, the first dynamic characteristic calculation unit 300 suppresses the high frequency including the natural vibration component included in the joint angle command value θlc, and the second dynamic characteristic. The arithmetic unit 400 suppresses the vibration caused by the nonlinear term in the above-described prior art by suppressing the high frequency including the natural vibration component included in the axial force torque fc.

以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる。
＜第２の実施の形態＞
以下、本発明の第２の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０が備えなかった速度フィードフォワード制御および／または加速度フィードフォワード制御が加えられている点が異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。 As described above, according to the elastic deformation compensation control apparatus according to the present embodiment, in an articulated robot, it is possible to compensate for the influence of elastic deformation of each axis and to perform operations such as weaving with high trajectory accuracy. it can.
<Second Embodiment>
Hereinafter, an elastic deformation compensation control apparatus according to the second embodiment of the present invention will be described. The elastic deformation compensation control device according to the present embodiment is added with speed feedforward control and / or acceleration feedforward control that the elastic deformation compensation control device 10 according to the first embodiment described above does not have. Is different. Other than that, the second embodiment is the same as the first embodiment, and therefore, the same parts as those described above are not repeated here.

上述した実施の形態においては、速度フィードフォワード制御および／または加速度フィードフォワード制御を備えていなかったが、本実施の形態に係る弾性変形補償制御装置３０においては、速度フィードフォワード制御および加速度フィードフォワード制御を備える。
図１２は、第２の実施の形態に係る弾性変形補償制御装置３０（速度フィードフォワード制御および加速度フィードフォワード制御を備える）のブロック図を示す。 In the above-described embodiment, the speed feedforward control and / or the acceleration feedforward control is not provided. However, in the elastic deformation compensation control device 30 according to the present embodiment, the speed feedforward control and the acceleration feedforward control. Is provided.
FIG. 12 is a block diagram of an elastic deformation compensation control device 30 (including speed feedforward control and acceleration feedforward control) according to the second embodiment.

図１２に示すように、弾性変形補償制御装置３０は、モータ角度制御部５１０に代えてモータ角度制御部３５１０を備える。
この図１２に示す制御ブロックの場合、各軸サーボフィードバック制御の動特性（サーボＦＢ制御動特性）は、以下の式（１）で与えられる。なお、「動特性１」は、第１の動特性演算部３００における動的特性を、「動特性２」は、第２の動特性演算部４００における動的特性をそれぞれ示している。 As shown in FIG. 12, the elastic deformation compensation control device 30 includes a motor angle control unit 3510 instead of the motor angle control unit 510.
In the case of the control block shown in FIG. 12, the dynamic characteristic (servo FB control dynamic characteristic) of each axis servo feedback control is given by the following equation (1). “Dynamic characteristic 1” indicates the dynamic characteristic in the first dynamic characteristic calculation unit 300, and “Dynamic characteristic 2” indicates the dynamic characteristic in the second dynamic characteristic calculation unit 400.

この式（１）と、動特性１＝動特性２／サーボＦＢ制御動特性×電流制御特性であることに基づいて、「動特性１」を算出すればよい。電流制御特性を無視して「ゲイン＝１」としても、従来技術よりも良い性能が得られる。
なお、ここで、ＧｄａおよびＧｄｖは加速度フィードフォワードおよび速度フィードフォワードゲインであり、０〜１の値をとる。また、ＪｄはＪｍの予測値である。 Based on this equation (1) and dynamic characteristic 1 = dynamic characteristic 2 / servo FB control dynamic characteristic × current control characteristic, “dynamic characteristic 1” may be calculated. Even if the current control characteristic is ignored and “gain = 1” is set, better performance than the conventional technique can be obtained.
Here, Gda and Gdv are acceleration feed forward and velocity feed forward gains, and take values of 0 to 1. Jd is a predicted value of Jm.

また、上述したように、電流制御特性も、電流制御ゲインおよびモータパラメータ（インダクタンスや抵抗）から算出可能である。
以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、速度フィードフォワード制御および加速度フィードフォワード制御を加えて、各軸の弾性変形の影響を補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。 Further, as described above, the current control characteristics can also be calculated from the current control gain and the motor parameters (inductance and resistance).
As described above, according to the elastic deformation compensation control apparatus according to the present embodiment, velocity feedforward control and acceleration feedforward control are added to compensate for the elastic deformation effect of each axis, and weaving with high trajectory accuracy. Etc. can be made possible.

なお、非線形項である軸力トルク計算部２００の前後に、第２の動特性演算部を配置す
るようにしても構わない。より詳しくは、軸力トルク計算部２００の前に動特性（２１）を、軸力トルク計算部２００の後に動特性（２２）を、それぞれ配置した。ここで、動特性（２１）×動特性（２２）が上述した動特性（２）と一致するように与えれば、これまでの実施の形態と同等の効果が得られる。ここで、θｌｅは関節角度指令値θｌｃを動特性（２１）に入力したときの出力値であり、θｌｅに基づいて軸力トルク計算部２００にて算出された軸力トルクｆｃを動特性（２２）に入力したときの出力値が軸力トルク補償値ｆｄである。このような弾性変形補償制御装置によると、多関節ロボットにおいて、各軸の弾性変形の影響を分割して配置された第２の動特性演算部により補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。 Note that the second dynamic characteristic calculation unit may be arranged before and after the axial force torque calculation unit 200 which is a nonlinear term. More specifically, the dynamic characteristic (21) is arranged before the axial force torque calculation unit 200, and the dynamic characteristic (22) is arranged after the axial force torque calculation unit 200, respectively. Here, if the dynamic characteristic (21) × the dynamic characteristic (22) is given so as to coincide with the above-described dynamic characteristic (2), an effect equivalent to that of the previous embodiments can be obtained. Here, θle is an output value when the joint angle command value θlc is input to the dynamic characteristic (21), and the axial force torque fc calculated by the axial force torque calculation unit 200 based on θle is the dynamic characteristic (22). ) Is the axial force torque compensation value fd. According to such an elastic deformation compensation control device, in an articulated robot, the effects of elastic deformation of each axis are compensated by the second dynamic characteristic calculation unit arranged in a divided manner, and operations such as weaving with high trajectory accuracy are performed. Can be made possible.

＜第３の実施の形態＞
以下、本発明の第３の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第２の実施の形態に係る弾性変形補償制御装置１０が備えなかったオブザーバ（観測器）およびＰＩＤ制御器が加えられている点が異なる。これらの加えられた機器により状態フィードバック制御が実現されている。それ以外は、第２の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。 <Third Embodiment>
Hereinafter, an elastic deformation compensation control apparatus according to the third embodiment of the present invention will be described. The elastic deformation compensation control apparatus according to the present embodiment is provided with an observer (observer) and a PID controller that are not provided in the elastic deformation compensation control apparatus 10 according to the second embodiment described above. Is different. State feedback control is realized by these added devices. Other than that, the second embodiment is the same as the second embodiment, and therefore, the same parts as those described above are not repeated here.

上述した第２の実施の形態に係る弾性変形補償制御装置３０においては、速度フィードフォワード制御および加速度フィードフォワード制御を備えている。このような弾性変形補償制御装置３０において、多関節ロボット１のアーム等が障害物に衝突したなど（外乱）により加振力が加わった場合について考える。
図９に、第２の実施の形態に係る弾性変形補償制御装置の制御ブロック図（図８）により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図９に示すように、時間０で加わった加振力は、０．５秒後の上下方向においても減衰しないで、振動を持続している。これは、第２の実施の形態に係る弾性変形補償制御装置３０においては、上述したようにフィードフォワード制御をベースにしているため、関節側に外乱などの加振力が加わった場合、振動を速やかに抑制することができない。このような上下方向の大きな振幅の振動が持続するという現象は、ウィービング動作における高精度な動的精度を確保することができていないことを示す。 The elastic deformation compensation control device 30 according to the second embodiment described above includes speed feedforward control and acceleration feedforward control. In such an elastic deformation compensation control device 30, consider a case where an excitation force is applied due to an arm or the like of the articulated robot 1 colliding with an obstacle (disturbance).
FIG. 9 shows a weaving trajectory of the articulated robot 1 when an excitation force is applied, which is controlled by the control block diagram (FIG. 8) of the elastic deformation compensation control apparatus according to the second embodiment. As shown in FIG. 9, the excitation force applied at time 0 continues to vibrate without being attenuated even in the vertical direction after 0.5 seconds. This is because the elastic deformation compensation control device 30 according to the second embodiment is based on the feedforward control as described above, and therefore, when an excitation force such as a disturbance is applied to the joint side, vibration is generated. It cannot be suppressed quickly. Such a phenomenon that the vibration with a large amplitude in the vertical direction is sustained indicates that high-precision dynamic accuracy in the weaving operation cannot be ensured.

このような場合、発生した振動を早期に減衰させるために、関節側の状態（関節角度、関節角速度）をオブザーバで推定して、状態フィードバック制御が行われる。すなわち、外乱などの加振力によって振動が発生した場合、それを素早く抑制するには、関節側の情報（関節角度、関節角速度、または軸捩れ量＝関節角度−モータ角度）を状態フィードバックする必要がある。関節側の情報（ここでは関節角度）をオブザーバにて推定し、関節角度指令値θｌｃとの偏差に基づいて、状態フィードバックする場合の制御ブロック図を図１０に示す。 In such a case, in order to attenuate the generated vibration at an early stage, the state on the joint side (joint angle, joint angular velocity) is estimated by an observer, and state feedback control is performed. That is, when vibration is generated by an excitation force such as a disturbance, it is necessary to state-feedback the information on the joint side (joint angle, joint angular velocity, or axial twist amount = joint angle−motor angle) in order to quickly suppress it. There is. FIG. 10 shows a control block diagram in the case where information on the joint side (here, the joint angle) is estimated by an observer and state feedback is performed based on a deviation from the joint angle command value θlc.

図１０に示すように、オブザーバ５０００で推定されて出力された関節側の状態量である関節角度の状態量が、関節角度θｌｃから減算されて、その演算結果（偏差）がＰＩＤ制御器５０１０へ入力されて、ＰＩＤ制御器５０１０での演算結果が、モータ角度制御部３５１０から出力されるモータトルク指令値に加算される。
図１１に、図１０に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図１１に示すように、オブザーバ５０００で推定されて出力された関節側の状態量と目標値との偏差に基づいて状態フィードバック制御すると、上下方向の振幅が異常に大きな大鋸歯状に悪化し、上下方向の振動がウィービング振幅よりも大きくなってしまう。このように、ウィービング波形ではウィービング振幅の５倍以上の上下動が発生するなど、非常に好ましくないウィービング波形となっている。これは、偏差を演算する場合の目標値が適正に与えられていないことが原因と考えられる。 As shown in FIG. 10, the joint angle state quantity estimated and output by the observer 5000 is subtracted from the joint angle θlc, and the calculation result (deviation) is sent to the PID controller 5010. The calculation result in the PID controller 5010 is input and added to the motor torque command value output from the motor angle control unit 3510.
FIG. 11 shows a weaving locus of the articulated robot 1 when an excitation force is applied, which is controlled by the control block diagram shown in FIG. As shown in FIG. 11, when the state feedback control is performed based on the deviation between the joint state quantity and the target value estimated and output by the observer 5000, the amplitude in the vertical direction deteriorates to an abnormally large sawtooth shape. The vibration in the vertical direction becomes larger than the weaving amplitude. Thus, the weaving waveform has a very unfavorable weaving waveform, for example, a vertical movement more than 5 times the weaving amplitude occurs. This is considered to be because the target value for calculating the deviation is not properly given.

このような観点から、第３の実施の形態に係る弾性変形補償制御装置５０においては、図１２に示すように、関節角度指令値計算部１００からの出力（関節角度指令値θlc）をフィルタリング処理して、処理後の関節角度目標値θｌｄを出力する第３の動特性演算部
４１０を設けた。それ以外の構成は図１０と同じく、関節側の状態量を推定するオブザーバ５０００、オブザーバ５０００で推定されて出力された関節側の状態量と関節角度目標値θｌｄとの偏差に基づいてＰＩＤ制御するＰＩＤ制御器５０１０とを備え、ＰＩＤ制御器５０１０での演算結果が、モータ角度制御部３５１０から出力されるモータトルク指令値に加算される。 From such a viewpoint, in the elastic deformation compensation control apparatus 50 according to the third embodiment, as shown in FIG. 12, the output (joint angle command value θlc) from the joint angle command value calculation unit 100 is subjected to a filtering process. Thus, a third dynamic characteristic calculation unit 410 that outputs the processed joint angle target value θld is provided. The other configurations are the same as in FIG. 10. The observer 5000 estimates the joint-side state quantity, and PID control is performed based on the deviation between the joint-side state quantity estimated and output by the observer 5000 and the joint angle target value θld. A PID controller 5010 is provided, and a calculation result in the PID controller 5010 is added to a motor torque command value output from the motor angle control unit 3510.

第３の動特性演算部４１０は、第１の動特性演算部３００よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備える。以下の説明では、第２の動特性演算部４００と同じ動特性を備えるものとして説明する。なお、請求項１における「第２の動特性演算部」は第３の動特性演算部４１０に対応し、請求項２における「第３の動特性演算部」は第２の動特性演算部４００に対応する。 The third dynamic characteristic calculation unit 410 includes a high-frequency cutoff characteristic having a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation unit 300. In the following description, it is assumed that the same dynamic characteristic as that of the second dynamic characteristic calculation unit 400 is provided. Note that the “second dynamic characteristic calculation unit” in claim 1 corresponds to the third dynamic characteristic calculation unit 410, and the “third dynamic characteristic calculation unit” in claim 2 corresponds to the second dynamic characteristic calculation unit 400. Corresponding to

図１３に、図１２に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図１３に示すように、オブザーバ５０００で推定されて出力された関節側の状態量とフィルタ処理された後の目標値との偏差に基づいて状態フィードバック制御すると、上下方向の振幅が速やかに収束している。すなわち、図９と比較して、図１３においては、時間０で加わった振動は、０．３秒程度で収束しており、その後の上下方向の振動は減衰して持続していない。このような上下方向の大きな振幅の振動が速やかに収束するという現象は、ウィービング動作における高精度な動的精度を確保することができていることを示す。 FIG. 13 shows a weaving locus of the articulated robot 1 when an excitation force is applied, which is controlled by the control block diagram shown in FIG. As shown in FIG. 13, when state feedback control is performed based on the deviation between the joint-side state quantity estimated and output by the observer 5000 and the filtered target value, the vertical amplitude converges quickly. doing. That is, as compared with FIG. 9, in FIG. 13, the vibration applied at time 0 converges in about 0.3 seconds, and the subsequent vertical vibration is attenuated and is not sustained. Such a phenomenon that large amplitude vibrations in the vertical direction converge quickly indicates that a high dynamic accuracy can be ensured in the weaving operation.

これは、偏差を演算する場合の目標値が適正に与えられていることが理由である。すなわち、目標値から多関節ロボット１の固有振動周期を遮断するローパスフィルタ特性を備えた第３の動特性演算部４１０を設けて、振動成分を目標値（ここでは関節角度）から除去する。このように除去された目標値（ここでは微分値である関節角度）が、関節側状態量の目標値として与えられ偏差が演算されて、ＰＩＤ制御された結果が、状態フィードバック量として処理される。 This is because the target value for calculating the deviation is appropriately given. That is, the third dynamic characteristic calculation unit 410 having a low-pass filter characteristic that cuts off the natural vibration period of the articulated robot 1 from the target value is provided to remove the vibration component from the target value (here, the joint angle). The target value thus removed (here, the joint angle, which is a differential value) is given as the target value of the joint-side state quantity, the deviation is calculated, and the result of PID control is processed as the state feedback quantity. .

このように処理することにより、関節側の状態量の目標値が適正に与えられることになり、振動を抑制しつつ、多関節ロボット１の先端における高精度の動作が実現できる。
以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、関節側の状態量をオブザーバで推定して、その状態量の目標値をローパスフィルタ処理して振動成分を目標値から除外してから与えることにより、外乱として多関節ロボットのアームが障害物に衝突したり加振力が加わったりしても、速やかに振動を収束させることができ、各軸の弾性変形の影響を補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。 By performing processing in this way, the target value of the state quantity on the joint side is appropriately given, and high-precision operation at the tip of the articulated robot 1 can be realized while suppressing vibration.
As described above, according to the elastic deformation compensation control device according to the present embodiment, the state quantity on the joint side is estimated by the observer, and the target value of the state quantity is low-pass filtered to obtain the vibration component from the target value. By giving it after excluding it, even if the arm of the articulated robot collides with an obstacle or an excitation force is applied as a disturbance, the vibration can be quickly converged, and the influence of the elastic deformation of each axis can be reduced. It is possible to compensate and enable operations such as weaving with high trajectory accuracy.

＜第３の実施の形態の変形例＞
次に、第３の実施の形態の２つの変形例に係る弾性変形補償制御装置について説明する。
１つめの変形例である第１の変形例に係る弾性変形補償制御装置６０の制御ブロック図を図１４に示す。図１２に示した制御ブロック図との差は、（１）オブザーバ５０００への入力側に微分演算を備える点、（２）オブザーバ５０００からの出力である関節側の状態量が関節角度ではなく関節角速度である点、（３）これら２点に対応して、第３の動特性演算部４１０の出力後に微分演算を備える点、である。 <Modification of Third Embodiment>
Next, an elastic deformation compensation control apparatus according to two modifications of the third embodiment will be described.
FIG. 14 shows a control block diagram of the elastic deformation compensation control device 60 according to the first modification which is the first modification. The difference from the control block diagram shown in FIG. 12 is that (1) a differential operation is provided on the input side to the observer 5000, and (2) the state quantity on the joint side that is the output from the observer 5000 is not a joint angle but a joint angle. A point that is an angular velocity, and (3) a point that includes a differential operation after the output of the third dynamic characteristic calculation unit 410 corresponding to these two points.

このため、第１の変形例に係る弾性変形補償制御装置６０においては、状態フィードバックされる状態量は関節角速度となり、それが関節角度である第３の実施の形態と異なる。第１の変形例に係る弾性変形補償制御装置６０のように、関節角速度を用いて振動抑制した方が、抑制性能は高く好ましい。
次に、２つめの変形例である第２の変形例に係る弾性変形補償制御装置７０の制御ブロック図を図１５に示す。図１４に示した制御ブロック図との差は、（１）第２の動特性演算部４００を備えずこれによりフィルタリング処理された軸力トルク補償値ｆｄにより軸力補償されていない点、（２）オブザーバ５０００からの出力により軸力補償している点、である。 For this reason, in the elastic deformation compensation control apparatus 60 according to the first modification, the state amount to be fed back is the joint angular velocity, which is different from the third embodiment, which is the joint angle. As in the elastic deformation compensation control device 60 according to the first modification, it is preferable to suppress vibration using the joint angular velocity because the suppression performance is high.
Next, FIG. 15 shows a control block diagram of the elastic deformation compensation control device 70 according to the second modified example which is the second modified example. The difference from the control block diagram shown in FIG. 14 is that (1) the second dynamic characteristic calculation unit 400 is not provided and the axial force compensation is not performed by the filtered axial force torque compensation value fd. ) The axial force is compensated by the output from the observer 5000.

このため、第２の変形例に係る弾性変形補償制御装置７０においては、軸力補償するの
はオブザーバ５０００からの出力となり、それが軸力トルク計算部２００からの出力である第３の実施の形態と異なる。弾性変形補償制御装置５０および弾性変形補償制御装置６０では、軸力をフィードフォワード的に補償しているが、第２の変形例に係る弾性変形補償制御装置７０のようにオブザーバ５０００などで推定しフィードバック補償することも可能である。ただし、フィードバック補償のため、応答遅れがありウィービング精度は若干悪くなるが、従来の補償しない場合に比べて断然良い精度および振動抑制が可能となっている。 For this reason, in the elastic deformation compensation control device 70 according to the second modification, the axial force compensation is an output from the observer 5000, which is an output from the axial force torque calculation unit 200. Different from form. In the elastic deformation compensation control device 50 and the elastic deformation compensation control device 60, the axial force is compensated in a feedforward manner, but is estimated by an observer 5000 or the like as in the elastic deformation compensation control device 70 according to the second modification. It is also possible to compensate for feedback. However, because of feedback compensation, there is a response delay and the weaving accuracy is slightly worse. However, the accuracy and vibration suppression can be significantly better than in the case where the conventional compensation is not performed.

＜実施の形態の作用効果＞
上述した第１〜第３の実施の形態のように、以下のように弾性変形補償制御装置を構成したため、モータとアームとが弾性変形する減速器を介して結合された多関節ロボットにおいて、高い軌跡精度でウィービング等の動作を可能とすることができる。
（１）目標値から固有振動周期を遮断するローパスフィルタ特性を与え、振動成分を目標角度から除去するとともに、軸力トルクについてもローパスフィルタ処理し、目標値と軸力トルクの位相を揃え、振動抑制しつつ弾性変形補償を行う。 <Effects of Embodiment>
Since the elastic deformation compensation control apparatus is configured as follows as in the first to third embodiments described above, the multi-joint robot in which the motor and the arm are coupled via a speed reducer that is elastically deformed is high. Operations such as weaving can be made possible with trajectory accuracy.
(1) A low-pass filter characteristic that cuts off the natural vibration period from the target value is given, the vibration component is removed from the target angle, and the axial force torque is also low-pass filtered to align the phase of the target value and the axial force torque, and vibration Elastic deformation compensation is performed while suppressing.

（２）状態フィードバック制御（オブザーバ、ＰＩＤ制御器）を付加して、その状態量の目標値をフィルタリング処理して振動成分を目標値から除外して、外乱として多関節ロボットのアームが障害物に衝突したりして加振力が加わっても、速やかに振動を収束させる。
なお、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。 (2) A state feedback control (observer, PID controller) is added, the target value of the state quantity is filtered to remove the vibration component from the target value, and the arm of the articulated robot becomes an obstacle as a disturbance. Even if a collision force is applied due to a collision, the vibration is quickly converged.
The embodiment disclosed this time should be considered as illustrative in all points and not restrictive. The scope of the present invention is defined by the terms of the claims, rather than the description above, and is intended to include any modifications within the scope and meaning equivalent to the terms of the claims.

１多関節ロボット
１０、３０、５０、６０、７０弾性変形補償制御装置
１００関節角度指令値計算部
２００軸力トルク計算部（軸力ＦＦ）
３００第１の動特性演算部（動特性（１））
４００第２の動特性演算部（動特性（２））
４１０第３の動特性演算部（動特性（３））
５００、５５００フィードバック制御部（サーボ制御ＦＢ特性）
６００モータ角度指令値計算部（弾性変形補償）
５１０モータ角度制御部
５２０モータ電流制御部（電流制御）
５０００オブザーバ
５０１０ＰＩＤ制御器
DESCRIPTION OF SYMBOLS 1 Articulated robot 10, 30, 50, 60, 70 Elastic deformation compensation control apparatus 100 Joint angle command value calculation part 200 Axial force torque calculation part (axial force FF)
300 First dynamic characteristic calculation unit (dynamic characteristic (1))
400 Second dynamic characteristic calculation unit (dynamic characteristic (2))
410 Third dynamic characteristic calculation unit (dynamic characteristic (3))
500, 5500 Feedback control unit (servo control FB characteristics)
600 Motor angle command value calculator (elastic deformation compensation)
510 Motor angle control unit 520 Motor current control unit (current control)
5000 observer 5010 PID controller

Claims

A multi-joint robot that drives a plurality of joint axes so that a tool attached to the multi-joint robot coupled with a motor that drives the joint axes of the multi-joint robot via a speed reducer that is elastically deformed performs a desired operation. In the elastic deformation compensation control device for the joint robot,
A joint angle command value calculator for calculating and outputting a joint angle command value θlc of each joint axis for realizing a desired tool operation;
An axial force torque calculator that calculates and outputs the axial force torque fc acting on each joint axis generated when operating according to the joint angle command value θlc from the joint angle command value θlc based on a dynamic model;
A motor angle command value calculation unit that calculates and outputs a motor angle command value θmc from the joint angle command value θlc and the axial force torque fc based on parameters including the stiffness parameter of the joint shaft;
A first dynamic characteristic calculator having a high-frequency cutoff characteristic having a cutoff frequency lower than a natural vibration frequency of the robot, filtering the motor angle command value θmc, and outputting a processed motor angle target value θmd; ,
A motor angle control unit in which the motor angle target value θmd is input as a target value for the motor;
It has a high-frequency cutoff characteristic having a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation unit, and performs a filtering process on the joint angle command value θlc or the joint angular velocity command value θlc ′ to obtain a joint angle target value after processing. a second dynamic characteristic calculator that outputs θld or a joint angular velocity target value θld ′;
An observer that estimates and outputs the state angle θl of the joint angle or the state quantity θl ′ of the joint angular velocity based on the detected joint angle or joint angular velocity;
'And, the state quantity θl state quantity θl or joint angular velocity of the joint angle output from the observer' said second dynamic characteristic calculation joint angle target value is output from the unit θld or joint angular velocity target value θld based on the A controller for calculating the control amount;
A motor current control unit that receives a value obtained by adding a control amount output from the controller to a motor torque command value output from the motor angle control unit;
The comprise be composed,
A high-frequency cutoff characteristic having a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation unit, and filtering at least one of an input to the axial force torque calculation unit and an output from the axial force torque calculation unit A third dynamic characteristic calculator that processes and outputs the processed axial force torque compensation value fd;
The motor current control unit is configured so that a value obtained by adding the axial force torque compensation value fd to the target value is input as a target value. Control device.

The elastic deformation compensation control device for an articulated robot according to claim 1 , wherein the second dynamic characteristic calculation unit has the same configuration as the third dynamic characteristic calculation unit.

The observer is configured to estimate and output an axial force torque compensation value f;
The said motor current control part is comprised so that the value which added the said axial force torque compensation value f to the said target value may be input as a target value, It is characterized by the above-mentioned. Elastic deformation compensation controller for articulated robots.

The elastic deformation compensation control device for an articulated robot according to any one of claims 1 to 3 , wherein the controller is configured to be a PID controller.

A multi-joint robot that drives a plurality of joint axes so that a tool attached to the multi-joint robot coupled with a motor that drives the joint axes of the multi-joint robot via a speed reducer that is elastically deformed performs a desired operation. In the elastic deformation compensation control method of the joint robot,
A joint angle command value calculation step of calculating and outputting a joint angle command value θlc of each joint axis for realizing a desired tool operation;
An axial force torque calculating step of calculating and outputting an axial force torque fc acting on each joint axis generated when operating according to the joint angle command value θlc from the joint angle command value θlc based on a dynamic model;
A motor angle command value calculating step for calculating and outputting a motor angle command value θmc from the joint angle command value θlc and the axial force torque fc based on parameters including a stiffness parameter of the joint shaft;
A first dynamic characteristic calculation step having a high frequency cutoff characteristic having a cutoff frequency lower than the natural vibration frequency of the robot, filtering the motor angle command value θmc, and outputting a processed motor angle target value θmd; ,
A motor angle control step in which the motor angle target value θmd is input as a target value for the motor;
A high-frequency cutoff characteristic having a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation step, the joint angle command value θlc or the joint angular velocity command value θlc ′ is filtered, and the joint angle target value after processing is processed a second dynamic characteristic calculation step for outputting θld or a joint angular velocity target value θld ′;
An observation step of estimating and outputting a joint angle state quantity θl or a joint angular speed state quantity θl ′ based on the detected joint angle or joint angular velocity using an observer;
'And, the state quantity θl state quantity θl or joint angular velocity of the joint angle output from the observation step' the second dynamic characteristic calculation joint angle target value is output from the step θld or joint angular velocity target value θld based on the A control step for calculating a control amount;
A motor current control step in which a value obtained by adding a control amount output from the control step to a motor torque command value output from the motor angle control step is input as a target value;
The comprise be composed,
A high-frequency cutoff characteristic having a cutoff frequency lower than or equivalent to that of the first dynamic characteristic calculation step, and filtering at least one of an input to the axial force torque calculation step and an output from the axial force torque calculation step And further including a third dynamic characteristic calculation step of processing and outputting the processed axial force torque compensation value fd,
In the motor current control step, a value obtained by adding the axial force torque compensation value fd to the target value is input as a target value .