JP5299255B2

JP5299255B2 - Robot controller

Info

Publication number: JP5299255B2
Application number: JP2009282813A
Authority: JP
Inventors: 健司長松; 哲也佐藤
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2009-03-18
Filing date: 2009-12-14
Publication date: 2013-09-25
Anticipated expiration: 2029-12-14
Also published as: JP2010240827A

Abstract

<P>PROBLEM TO BE SOLVED: To provide a robot control device capable of making a real operation locus of a robot become closer to a theory by properly compensating friction. <P>SOLUTION: In the case of driving an output shaft in a target driving direction from a condition wherein a motor shaft side gear and an output shaft side gear are stopped and an elastic deformation quantity is zero, a robot control device 3 gradually increases the friction compensating torque in the target driving direction, the torque being applied to the motor shaft side gear, and after the elastic deformation quantity reaches the maximum elastic deformation quantity, the robot control device 3 applies the friction compensating torque at a time point when the elastic deformation quantity reaches the maximum elastic deformation quantity as a constant value. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

本発明は、モータ軸側ギアと出力軸側ギアとが噛み合っている状態で前記モータ軸側ギアが回転することに連動して前記出力軸側ギアが回転することで前記出力軸が駆動可能に構成されてなる駆動軸の動作を制御するロボット制御装置に関する。 In the present invention, the output shaft can be driven by rotating the output shaft side gear in conjunction with the rotation of the motor shaft side gear while the motor shaft side gear and the output shaft side gear are engaged with each other. The present invention relates to a robot control device that controls the operation of a drive shaft.

モータ軸側ギアが回転することに連動して出力軸側ギアが回転することで出力軸が駆動可能に構成されてなる駆動軸を有するロボットの動作軌跡の精度を高める方法として、送り系のバックラッシに起因するロストモーション量、ギアの弾性変形に起因するロストモーション量及び静摩擦に起因するロストモーション量を予め計算しておき、軸の移動方向が反転する場合に、モータのギアをバックラッシ加速度で送り系のバックラッシに起因するロストモーション量だけ駆動させ、続いて弾性変形加速度でギアの弾性変形に起因するロストモーション量だけ駆動させ、続いて静摩擦加速度で静摩擦に起因するロストモーション量だけ駆動させる方法がある（例えば特許文献１参照）。 As a method for improving the accuracy of the operation trajectory of a robot having a drive shaft configured such that the output shaft can be driven by rotating the output shaft side gear in conjunction with the rotation of the motor shaft side gear, backlash of the feed system If the lost motion amount due to gear loss, the lost motion amount due to elastic deformation of the gear, and the lost motion amount due to static friction are calculated in advance, the motor gear is fed at the backlash acceleration when the shaft movement direction is reversed. The system is driven by the amount of lost motion due to the backlash of the system, then the amount of lost motion due to elastic deformation of the gear is driven by the elastic deformation acceleration, and then the amount of lost motion due to static friction is driven by the static friction acceleration. Yes (see, for example, Patent Document 1).

特開平５−２８２０１９号公報JP-A-5-282019

特許文献１に開示されている方法では理論上ではロボットの動作軌跡の精度を高めることが可能であるが、しかしながら、具体的にどのような動作制御で摩擦補償を行えば良いのかについては開示されておらず、ロボットの動作軌跡の精度を高めることができるという点で不確かな点が多い。 The method disclosed in Patent Document 1 can theoretically increase the accuracy of the motion trajectory of the robot. However, it is disclosed what kind of motion control should be used for friction compensation. However, there are many uncertain points in that the accuracy of the robot motion trajectory can be improved.

本発明は、上記した事情に鑑みてなされたものであり、その目的は、摩擦補償を適切に行うことでロボットの実際の動作軌跡をより理論に近づけることができ、ロボットの動作軌跡を高い精度で再現することができるロボット制御装置を提供することにある。 The present invention has been made in view of the above-described circumstances, and an object of the present invention is to appropriately approximate friction so that the actual motion trajectory of the robot can be made closer to the theory, and the robot motion trajectory can be obtained with high accuracy. An object of the present invention is to provide a robot control device that can be reproduced with

請求項１に記載した発明によれば、最大弾性変形量角度記憶手段は、モータ軸のねじれ、モータ軸とモータ軸側ギアとの接続箇所のねじれ、モータ軸側ギアの変形、出力軸側ギアの変形及び出力軸側ギアと出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する。弾性変形量計算手段は、モータ軸の回転角度を検出するモータ軸側エンコーダが電源投入時に示す角度を弾性変形量ゼロ角度とし、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を超えているか否かを判定し、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を越えていなければ、モータ軸側エンコーダ角度を弾性変形中の角度として判定して弾性変形量を計算し、一方、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を越えていれば、モータ軸側エンコーダ角度を動作開始時の角度として判定する。弾性変形方向検出手段は、弾性変形量ゼロ角度とモータ軸側エンコーダ角度との差分が示す正負を判定してモータ軸側ギアの弾性変形方向を検出する。 According to the first aspect of the present invention, the maximum elastic deformation amount angle storage means includes the twist of the motor shaft, the twist of the connection portion between the motor shaft and the motor shaft side gear, the deformation of the motor shaft side gear, and the output shaft side gear. And the deformation caused by the twist of the connection portion between the output shaft side gear and the output shaft are stored as the elastic deformation, and the maximum value of the elastic deformation amount, which is the sum of these deformations, is stored as the maximum elastic deformation amount angle. The elastic deformation amount calculation means uses the angle that the motor shaft side encoder that detects the rotation angle of the motor shaft shows when the power is turned on as the elastic deformation amount zero angle, and the difference between the motor shaft side encoder angle and the elastic deformation amount zero angle is the maximum elasticity. It is determined whether or not the deformation amount angle is exceeded, and if the difference between the motor shaft side encoder angle and the elastic deformation amount zero angle does not exceed the maximum elastic deformation amount angle, the motor shaft side encoder angle is the angle during elastic deformation. If the difference between the motor shaft side encoder angle and the zero elastic deformation amount angle exceeds the maximum elastic deformation amount angle, the motor shaft side encoder angle is calculated as the angle at the start of operation. Judge as. The elastic deformation direction detecting means detects the elastic deformation direction of the motor shaft side gear by determining whether the difference between the zero elastic deformation amount angle and the motor shaft side encoder angle is positive or negative.

モータ制御手段は、弾性変形量を更新する弾性変形更新処理を実行することに続いて摩擦補償用トルクを決定する摩擦補償用トルク決定処理を実行する処理を周期的に行う。弾性変形更新処理においては、弾性変形量計算手段により計算された弾性変形量と正方向の最大弾性変形量及び負方向の最大弾性変形量とを比較し、弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていなく且つ負方向の最大弾性変形量を超えていないと判定したときには弾性変形中であると判定して現在の弾性変形量を維持し、弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその正方向の最大弾性変形量を弾性変形量として設定し、弾性変形量計算手段により計算された弾性変形量が負方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその負方向の最大弾性変形量を弾性変形量として設定する。 The motor control means periodically performs a process of executing a friction compensation torque determination process for determining a friction compensation torque subsequent to executing the elastic deformation update process of updating the elastic deformation amount. In the elastic deformation update process, the elastic deformation amount calculated by the elastic deformation amount calculating means is compared with the maximum elastic deformation amount in the positive direction and the maximum elastic deformation amount in the negative direction, and the elasticity calculated by the elastic deformation amount calculating means is compared. When it is determined that the deformation amount does not exceed the maximum elastic deformation amount in the positive direction and does not exceed the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is in progress and the current elastic deformation amount is maintained. When it is determined that the amount of elastic deformation calculated by the amount calculation means exceeds the maximum amount of elastic deformation in the positive direction, it is determined that the elastic deformation is not in progress and the maximum amount of elastic deformation in the positive direction is set as the amount of elastic deformation, When it is determined that the elastic deformation amount calculated by the elastic deformation amount calculation means exceeds the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is not in progress, and the maximum elastic deformation amount in the negative direction is defined as the elastic deformation amount. Set Te.

又、摩擦補償用トルク決定処理においては、目標方向と弾性変形方向検出手段により検出された弾性変形方向とを比較し、目標方向と弾性変形方向検出手段により検出された弾性変形方向とが逆方向であると判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に減少させ、目標方向と弾性変形方向検出手段により検出された弾性変形方向とが同方向であると判定したときには弾性変形量が最大弾性変形量であるか否かを判定し、弾性変形量が最大弾性変形量でないと判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に増加させ、弾性変形量が最大弾性変形量であると判定したときには予め設定されている最大補償トルクを摩擦補償用トルクとして設定する。 In the friction compensation torque determination process, the target direction and the elastic deformation direction detected by the elastic deformation direction detecting means are compared, and the target direction and the elastic deformation direction detected by the elastic deformation direction detecting means are opposite. When it is determined that the friction compensation torque is decreased continuously or stepwise for each sampling time unit, and when it is determined that the target direction and the elastic deformation direction detected by the elastic deformation direction detecting means are the same direction It is determined whether or not the elastic deformation amount is the maximum elastic deformation amount. When it is determined that the elastic deformation amount is not the maximum elastic deformation amount, the friction compensation torque is increased continuously or stepwise for each sampling time unit to When it is determined that the deformation amount is the maximum elastic deformation amount, a preset maximum compensation torque is set as the friction compensation torque.

これにより、例えばロボットの駆動を開始する場合などであり弾性変形していない停止状態から出力軸を目標駆動方向に駆動させる場合では、モータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加し、出力軸を目標駆動方向に駆動させることになる。 Thus, for example, when the drive of the robot is started and the output shaft is driven in the target drive direction from a stopped state that is not elastically deformed, the friction compensation torque applied to the motor shaft side gear in the target drive direction. After the elastic deformation amount reaches the maximum elastic deformation amount, the friction compensation torque when the elastic deformation amount reaches the maximum elastic deformation amount is applied as a constant value, and the output shaft is set in the target drive direction. Will be driven.

又、例えばロボットの動作途中で正転を一旦停止して正転を再開する場合などであり弾性変形している停止状態から出力軸を弾性変形方向と同一方向である目標駆動方向に駆動させる場合では、モータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量計算手段により計算された弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加し、出力軸を目標駆動方向に駆動させることになる。 Also, for example, when normal rotation is temporarily stopped and the normal rotation is restarted during the operation of the robot, and when the output shaft is driven in the target drive direction that is the same direction as the elastic deformation direction from the elastically deformed stop state. Then, after gradually increasing the friction compensation torque applied to the motor shaft side gear in the target drive direction and the elastic deformation amount reaches the maximum elastic deformation amount, the elastic deformation amount calculated by the elastic deformation amount calculating means is When the maximum elastic deformation amount is reached, the friction compensation torque is applied as a constant value, and the output shaft is driven in the target drive direction.

又、例えばロボットの動作途中で正転を一旦停止して反転を開始する場合などであり弾性変形している停止状態から出力軸を弾性変形方向と反対方向である目標駆動方向に駆動させる場合では、モータ軸側ギアに印加する目標駆動方向とは反対方向への摩擦補償用トルクを徐々に減少させた後にモータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加し、出力軸を目標駆動方向に駆動させることになる。 Also, for example, when the forward rotation is temporarily stopped and the reversal is started during the operation of the robot, for example, when the output shaft is driven in the target drive direction opposite to the elastic deformation direction from the elastically deformed stop state. Then, after gradually decreasing the friction compensation torque in the direction opposite to the target drive direction applied to the motor shaft side gear, gradually increase the friction compensation torque in the target drive direction applied to the motor shaft side gear, After the elastic deformation amount reaches the maximum elastic deformation amount, the friction compensation torque when the elastic deformation amount reaches the maximum elastic deformation amount is applied as a constant value, and the output shaft is driven in the target drive direction. .

このように弾性変形することを考慮して摩擦補償用トルクを印加する制御を行い、摩擦補償を行うことで、ロボットの如何に拘らず実際の動作軌跡をより理論に近づけることができ、ロボットの動作軌跡を高い精度で再現することができる。 By controlling the application of the friction compensation torque in consideration of the elastic deformation in this way and performing the friction compensation, the actual motion trajectory can be made closer to the theory regardless of the robot. The motion trajectory can be reproduced with high accuracy.

以下、上記した請求項１に記載した発明の技術的特徴について、図１７乃至図１９を参照して説明する。上記した請求項１に記載した発明は、モータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させる点及び摩擦補償用トルクと加速トルクとを切分けて制御している点を特徴としている。 The technical features of the invention described in claim 1 will be described below with reference to FIGS. In the first aspect of the present invention, the friction compensation torque applied to the motor shaft side gear in the target drive direction is gradually increased and the friction compensation torque and the acceleration torque are controlled separately. Characterized by dots.

理論上、弾性変形を考慮することなく摩擦補償だけを考慮してロボットの動作軌跡の精度を高めるようとする場合には、図１７に示すように制御すれば良い。即ち、最大静止摩擦を超える摩擦補償用トルクを一気に（急激に）印加したとしても、ロボットの動作軌跡は線形となるので、静止摩擦分のトルクを加速トルクに加算すれば良い。 Theoretically, when it is intended to improve the accuracy of the robot motion trajectory by considering only friction compensation without considering elastic deformation, the control may be performed as shown in FIG. That is, even if a friction compensation torque exceeding the maximum static friction is applied at once (abruptly), the robot's motion trajectory becomes linear, and the torque for static friction may be added to the acceleration torque.

しかしながら、摩擦補償だけでなく弾性変形をも考慮してロボットの動作軌跡の精度を高めようとする場合、上記した図１７に示す技術思想にしたがって最大静止摩擦を超える摩擦補償用トルクを一気に印加すると、ロボットの動作軌跡は理論上では上記した図１７に示したものと同様に線形となるが、現実的には制御対象となる物体は理論のように絶対的な硬度を持たず弾性変形する性質であるので、図１８に示すように、弾性変形分だけの位置移動が先行し、物体の動作開始時に弾性変形分が戻ることで逆方向への位置移動が発生し、その後に加速トルクにより物体の位置移動が開始するという現象が発生する。即ち、物体の形状の揺り戻しによる動作軌跡のぐらつきが発生し、理論にしたがった動作軌跡とはならず、動作軌跡の精度を高めることは困難である。これは上記した特許文献１の技術に相当する。 However, when trying to improve the accuracy of the robot motion trajectory in consideration of not only friction compensation but also elastic deformation, if friction compensation torque exceeding the maximum static friction is applied at once according to the technical idea shown in FIG. The robot's motion trajectory is theoretically linear like that shown in FIG. 17 described above, but in reality, the object to be controlled does not have an absolute hardness as in theory and elastically deforms. Therefore, as shown in FIG. 18, the position movement by the amount corresponding to the elastic deformation precedes, and the position movement in the reverse direction occurs when the elastic deformation amount returns at the start of the operation of the object. The phenomenon that the movement of the position starts is generated. That is, the motion trajectory fluctuates due to the swinging back of the shape of the object and does not become the motion trajectory according to the theory, and it is difficult to increase the accuracy of the motion trajectory. This corresponds to the technique of Patent Document 1 described above.

このような事情から、請求項１に記載した発明では、図１９に示すように、摩擦補償用トルクと加速トルクとを切分けて制御し、摩擦補償用トルクを徐々に増加させることで、一旦弾性変形した物体を弾性変形させたままで加速トルクにより位置移動させることが可能となり、動作軌跡の精度を高めることが可能となる。 Under such circumstances, in the invention described in claim 1, as shown in FIG. 19, the friction compensation torque and the acceleration torque are controlled separately, and the friction compensation torque is gradually increased. The elastically deformed object can be moved by acceleration torque while being elastically deformed, and the accuracy of the motion trajectory can be improved.

尚、以上に説明した上記した請求項１に記載した発明の技術的特徴は後述する請求項２に記載した発明の技術的特徴と同様であり、請求項２に記載した発明は請求項１に記載した発明の技術的特徴と同じ理論に基づく発明である。 The technical features of the invention described in claim 1 described above are the same as the technical features of the invention described in claim 2 described later, and the invention described in claim 2 is described in claim 1. The invention is based on the same theory as the technical features of the described invention.

請求項２に記載した発明によれば、最大弾性変形量角度記憶手段は、モータ軸のねじれ、モータ軸とモータ軸側ギアとの接続箇所のねじれ、モータ軸側ギアの変形、出力軸側ギアの変形及び出力軸側ギアと出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する。弾性変形量計算手段は、モータ軸の回転角度を検出するモータ軸側エンコーダ角度と出力軸の回転角度を検出する出力軸側エンコーダ角度との差分が最大弾性変形量角度を超えているか否かを判定し、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が最大弾性変形量角度を越えていなければ、モータ軸側エンコーダ角度を弾性変形中の角度として判定して弾性変形量を計算し、一方、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が最大弾性変形量角度を越えていれば、モータ軸側エンコーダ角度を動作開始時の角度として判定する。弾性変形方向検出手段は、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が示す正負を判定してモータ軸側ギアの弾性変形方向を検出する。 According to the second aspect of the present invention, the maximum elastic deformation amount angle storage means includes the twist of the motor shaft, the twist of the connection portion between the motor shaft and the motor shaft side gear, the deformation of the motor shaft side gear, and the output shaft side gear. And the deformation caused by the twist of the connection portion between the output shaft side gear and the output shaft are stored as the elastic deformation, and the maximum value of the elastic deformation amount, which is the sum of these deformations, is stored as the maximum elastic deformation amount angle. The elastic deformation amount calculation means determines whether or not the difference between the motor shaft side encoder angle for detecting the rotation angle of the motor shaft and the output shaft side encoder angle for detecting the rotation angle of the output shaft exceeds the maximum elastic deformation amount angle. If the difference between the motor shaft side encoder angle and the output shaft side encoder angle does not exceed the maximum elastic deformation amount angle, the motor shaft side encoder angle is determined as the elastic deformation angle and the elastic deformation amount is calculated. On the other hand, if the difference between the motor shaft side encoder angle and the output shaft side encoder angle exceeds the maximum elastic deformation amount angle, the motor shaft side encoder angle is determined as the angle at the start of operation. The elastic deformation direction detection means detects the elastic deformation direction of the motor shaft side gear by determining whether the difference between the motor shaft side encoder angle and the output shaft side encoder angle is positive or negative.

これにより、上記した請求項１に記載したものと弾性変形量を計算する手順が異なるだけであり、上記した請求項１に記載したもの同様の作用効果を得ることができる。 Thus, only the procedure for calculating the amount of elastic deformation is different from that described in claim 1, and the same effects as those described in claim 1 can be obtained.

請求項３に記載した発明によれば、モータ制御手段は、弾性変形更新処理及び摩擦補償用トルク決定処理を実行することに続いて停止時のチャタリングを防止する停止時チャタリング防止処理を実行する処理を周期的に行い、停止時チャタリング防止処理においては、ロボットの動作停止命令を判定したときには、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算する。 According to the third aspect of the present invention, the motor control means performs the stop chattering prevention process for preventing the chattering at the stop following the execution of the elastic deformation update process and the friction compensation torque determination process. In the stop chattering prevention process, when the robot operation stop command is determined, the friction compensation torque immediately before the robot operation is stopped is maintained, and when the robot operation start command is determined. Then, the state quantity of the integral compensation at the start of the operation of the robot for compensating for the disturbance force is calculated according to the following calculation formula.

ロボットの動作開始時の積分補償の状態量
＝ロボットの動作開始直前の積分補償の状態量−（ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量）
これにより、ロボットの動作停止時から動作開始時までの停止期間では、外乱力や摩擦が変化すると、その外乱力や摩擦が変化したことに追従して変化分を積分補償することで、外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で均衡を保つことができ、ロボットの動作開始時においても外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で両者の均衡を保つことができ、ロボットの動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。又、このように積分補償の状態量を変化させるだけで良いので、制御系の安定性に悪影響を与えることなく、単純な演算だけで実装可能である。 Integral compensation state quantity at the start of robot operation = Integral compensation state quantity immediately before the robot operation starts-(Friction compensation state quantity at the start of robot operation-Friction compensation state quantity immediately before the robot operation starts)
As a result, during the stop period from when the robot stops moving to when it starts moving, if the disturbance force or friction changes, the disturbance force or friction is tracked as the disturbance force or friction changes to compensate for the disturbance force. And the sum of the friction and the sum of the integral compensation and the friction compensation can be maintained, and both the disturbance force and the sum of the friction and the sum of the integral compensation and the friction compensation are The balance can be maintained, and the accuracy of the entire motion trajectory from the start to the stop of the operation of the robot can be further increased. Further, since it is only necessary to change the state quantity of the integral compensation in this way, it can be implemented by simple calculation without adversely affecting the stability of the control system.

請求項４に記載した発明によれば、モータ制御手段は、弾性変形更新処理及び摩擦補償用トルク決定処理を実行することに続いて停止時の摩擦補償を行う停止時摩擦補償処理を実行する処理を周期的に行い、停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、目標角度とモータ軸側エンコーダ角度との差分の絶対値が最大弾性変形量角度よりも大きな角度である設定角度を超えているか否かを判定し、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えている場合に、弾性変形量を考慮した摩擦補償を行う。 According to the fourth aspect of the present invention, the motor control means performs the stop friction compensation process for performing the stop friction compensation following the execution of the elastic deformation update process and the friction compensation torque determination process. In the friction compensation process at stop, when the robot operation stop command is determined, the absolute value of the difference between the target angle and the motor shaft side encoder angle is larger than the maximum elastic deformation angle. It is determined whether or not a certain set angle is exceeded, and if the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle, friction compensation is performed in consideration of the amount of elastic deformation.

ロボットの動作停止時では、停止指令がロボット制御装置から出力された後に速度をゼロとするので、停止指令が出力されてから速度をゼロとするまでの僅かな期間がタイムラグとして発生する。又、物体には物理現象としての慣性が存在する。よって、停止指令が出力された後に上記したタイムラグ分だけ遅れて実際の速度がゼロとなる。ロボットの動作としては、このタイムラグが発生している期間中の動作により、目標角度を僅かに越えてしまい（オーバーシュートしてしまい）、その後に目標角度に対して僅かに越えてしまったオーバーシュート分を戻す動作を行うことで、目標角度に近づけるようにしている。 When the operation of the robot is stopped, the speed is set to zero after the stop command is output from the robot control device. Therefore, a short period from when the stop command is output until the speed is set to zero occurs as a time lag. In addition, the object has inertia as a physical phenomenon. Therefore, after the stop command is output, the actual speed becomes zero with a delay by the time lag described above. As the robot operation, the target angle is slightly exceeded (overshoot) due to the operation during this time lag, and then the target angle is slightly exceeded. By performing the operation of returning the minute, it is made closer to the target angle.

一方、ロボットは例えば生産性の効率が高いことが要求されていることから、なるべく短い時間で正確な位置に到達されることが要求されており、よって、上記したようなオーバーシュートが発生した状況でもなるべく短い時間で目標角度に到達することが要求されている。そこで、通常のトルク制御である時間と目標角度までの差で積分を行い、積分値に比例したトルクを印加するものであるが、この方法では、そもそもオーバーシュート分の角度が極めて僅かであり、全体の動作軌跡の精度を高めることを考慮すると、最初は極めて僅かな積分量では極めて僅かなトルクを印加することしかできず、オーバーシュート分を戻る（方向反転）時に発生する静止摩擦を超える（打ち勝つ）ことができない。最初から大きなトルクを印加してしまうと、静止摩擦を超えることができるようになるが、全体の動作軌跡を精度良く形成することができなくなる。 On the other hand, since robots are required to have high productivity efficiency, for example, it is required to reach an accurate position in as short a time as possible. However, it is required to reach the target angle in as short a time as possible. Therefore, the normal torque control is integrated with the difference between the time and the target angle, and the torque proportional to the integral value is applied, but in this method, the angle for the overshoot is very small in the first place, Considering the improvement of the accuracy of the entire motion trajectory, at first, only a very small amount of torque can be applied with a very small amount of integration, exceeding the static friction that occurs when the overshoot is returned (direction reversal) ( Can't win) If a large torque is applied from the beginning, it becomes possible to exceed the static friction, but the entire motion trajectory cannot be accurately formed.

このような事情から、オーバーシュートした後では、オーバーシュート分を直ちに戻すことができず、外見的にオーバーシュートした角度であたかも停止した状態になる。その後、時間経過にしたがって積分が進み、積分値が一定値に達し、印加するトルクが方向反転時の静止摩擦を超えるようになると、そこで初めてオーバーシュート分を戻す制御を開始する。このようにオーバーシュート分を戻す制御を開始するまでに時間がかかり、オーバーシュートを解消するまでに時間がかかるという問題がある。 Under such circumstances, after overshooting, the amount of overshooting cannot be returned immediately, and it appears as if it has stopped at an apparently overshot angle. Thereafter, the integration proceeds with time, and when the integrated value reaches a constant value and the applied torque exceeds the static friction at the time of direction reversal, control for returning the overshoot is started for the first time. Thus, there is a problem that it takes time to start the control for returning the overshoot amount, and it takes time to eliminate the overshoot.

そこで、ロボットの動作停止時においても、目標角度とモータ軸側エンコーダ角度との差分の絶対値が最大弾性変形量角度よりも大きな角度である設定角度を超えていることを条件に、ロボットの動作開始時に弾性変形量を考慮した摩擦補償を行うことと同様に、弾性変形量を考慮した摩擦補償を行うことにより、オーバーシュート分を戻す制御を早く開始し、オーバーシュートを解消するまでにかかる時間を短縮することができる。これにより、ロボットの動作開始時だけでなく動作停止時までも含めて動作軌跡をより理論に近づけることができ、ロボットの動作軌跡を高い精度で再現することができる。 Therefore, even when the robot operation is stopped, the robot operation is performed on the condition that the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle that is larger than the maximum elastic deformation amount angle. Similar to performing friction compensation in consideration of the amount of elastic deformation at the start, it takes time to start the control to return the overshoot quickly by performing friction compensation in consideration of the amount of elastic deformation and eliminate the overshoot. Can be shortened. As a result, the motion trajectory can be made closer to the theory including not only when the robot motion starts but also when the motion is stopped, and the robot motion trajectory can be reproduced with high accuracy.

請求項５に記載した発明によれば、モータ制御手段は、停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていない場合に、弾性変形量を考慮した摩擦補償を行わないと共に、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算する。 According to the fifth aspect of the present invention, when the motor control means determines the robot operation stop command in the stop friction compensation process, the absolute value of the difference between the target angle and the motor shaft side encoder angle is If the set angle is not exceeded, friction compensation that takes into account the amount of elastic deformation is not performed, and the friction compensation torque immediately before the robot operation is stopped is maintained. The state quantity of the integral compensation at the start of the operation of the robot for compensating for the force is calculated according to the following calculation formula.

ロボットの動作開始時の積分補償の状態量
＝ロボットの動作開始直前の積分補償の状態量−（ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量）
これにより、上記した請求項４に記載した弾性変形量を考慮した摩擦補償を行うことで、目標角度とモータ軸側エンコーダ角度との差分が最大弾性変形量角度よりも大きな角度である設定角度を超えなくなった後に（ある程度まで目標角度に収束した後に）、上記した請求項３に記載したものと同様の作用効果を得ることができる。即ち、オーバーシュートを解消するまでにかかる時間を短縮することができ、しかも、ロボットの動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。 Integral compensation state quantity at the start of robot operation = Integral compensation state quantity immediately before the robot operation starts-(Friction compensation state quantity at the start of robot operation-Friction compensation state quantity immediately before the robot operation starts)
Accordingly, by performing friction compensation in consideration of the elastic deformation amount described in claim 4, the set angle at which the difference between the target angle and the motor shaft side encoder angle is larger than the maximum elastic deformation amount angle is set. After not exceeding (after converging to a target angle to some extent), the same effect as that described in the third aspect can be obtained. That is, it is possible to shorten the time taken to eliminate the overshoot, and to further improve the accuracy of the entire motion trajectory from the start to the stop of the operation of the robot.

本発明の第１の実施形態を示す機能ブロック図Functional block diagram showing a first embodiment of the present invention ロボット制御システムの全体構成を示す図Diagram showing the overall configuration of the robot control system 軸の駆動部分を概略的に示す図Diagram showing the drive part of the shaft フローチャートflowchart 図４相当図4 equivalent figure 図４相当図4 equivalent figure 図４相当図4 equivalent figure モータ軸側ギアが動作開始時に弾性変形する態様を示す図及び摩擦補償用トルクの推移を示す図The figure which shows the mode which the motor shaft side gear elastically deforms at the time of an operation | movement start, and the figure which shows transition of the torque for friction compensation 図８相当図Equivalent to FIG. 図８相当図Equivalent to FIG. モータ軸側ギアが動作停止時に弾性変形する態様を示す図及び摩擦補償用トルクの推移を示す図The figure which shows the mode which the motor shaft side gear elastically deforms at the time of operation stop, and the figure which shows transition of the friction compensation torque 図１１相当図11 equivalent figure 外乱力及び摩擦に対する積分補償及び摩擦補償を概略的に示す図Schematic diagram of integral and friction compensation for disturbance forces and friction 図１３相当図13 equivalent diagram 図１３相当図13 equivalent diagram 測定結果を示す図Diagram showing measurement results 本発明の原理を説明する図The figure explaining the principle of this invention 図１７相当図17 equivalent diagram 図１７相当図17 equivalent diagram 本発明の第２の実施形態を示すフローチャートThe flowchart which shows the 2nd Embodiment of this invention. 図２０相当図FIG. 20 equivalent diagram 停止時摩擦補償処理を行わなかった場合の目標角度に近づくまでの挙動を示す図The figure which shows the behavior until it approaches the target angle when the friction compensation processing at the time of stop is not performed 停止時摩擦補償処理を行った場合の目標角度に近づくまでの挙動を示す図The figure which shows the behavior until it approaches the target angle when the friction compensation processing at the time of stop is performed

（第１の実施形態）
以下、本発明の第１の実施形態について、図１乃至図１９を参照して説明する。図２は、ロボット制御システムの全体構成を示している。ロボット制御システム１は、例えば６軸の垂直多関節型ロボットからなるロボット２と、ロボット２の動作を制御するロボット制御装置３と、作業者が操作するティーチングペンダンド４とを備えて構成されている。 (First embodiment)
Hereinafter, a first embodiment of the present invention will be described with reference to FIGS. FIG. 2 shows the overall configuration of the robot control system. The robot control system 1 includes, for example, a robot 2 composed of a 6-axis vertical articulated robot, a robot control device 3 that controls the operation of the robot 2, and a teaching pendant 4 that is operated by an operator. Yes.

ロボット２は、ベース５と、ベース５に水平方向に旋回可能に支持されているショルダ部６と、ショルダ部６に上下方向に旋回可能に支持されている下アーム７と、下アーム７に上下方向に旋回可能に支持されている第１の上アーム８と、第１の上アーム８の先端部に捻り回転可能に支持されている第２の上アーム９と、第２の上アーム９に上下方向に回転可能に支持されている手首１０と、手首１０に回転（捻り動作）可能に支持されているフランジ１１とを備えて構成されている。 The robot 2 includes a base 5, a shoulder portion 6 supported by the base 5 so as to be turnable in the horizontal direction, a lower arm 7 supported by the shoulder portion 6 so as to be turnable in the vertical direction, and a lower arm 7 A first upper arm 8 that is supported so as to be pivotable in a direction, a second upper arm 9 that is supported by a tip portion of the first upper arm 8 in a twistable manner, and a second upper arm 9. The wrist 10 is supported by being rotatable in the vertical direction, and the flange 11 is supported by the wrist 10 so as to be rotatable (twisting).

上記したベース５を含め、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０及びフランジ１１は、ロボット２におけるリンクとして機能し、ベース５を除く各リンクは前段のリンクに対して回転関節により回転可能に連結されている。最先端のリンクであるフランジ１１にはワークを把持するためのハンド（図示せず）が取付け可能になっている。 The shoulder portion 6, the lower arm 7, the first upper arm 8, the second upper arm 9, the wrist 10, and the flange 11 including the base 5 described above function as links in the robot 2, and each link excluding the base 5 Is rotatably connected to the previous link by a rotary joint. A hand (not shown) for gripping a work can be attached to the flange 11 which is the most advanced link.

尚、本実施形態では、第１のリンクであるベース５と第２のリンクであるショルダ部６との間を連結する回転関節の関節軸を１軸、第２のリンクであるショルダ部６と第３のリンクである下アーム７との間を連結する回転関節の関節軸を２軸、第３のリンクである下アーム７と第４のリンクである第１の上アーム８との間を連結する回転関節の関節軸を３軸、第４のリンクである第１の上アーム８と第５のリンクである第２の上アーム９との間を連結する回転関節の関節軸を４軸、第５のリンクである第２の上アーム９と第６のリンクである手首１０との間を連結する回転関節の関節軸を５軸、第６のリンクである手首１０と第７のリンクであるフランジ１１との間を連結する回転関節の関節軸を６軸として図示している。 In the present embodiment, the joint axis of the rotary joint that connects between the base 5 that is the first link and the shoulder portion 6 that is the second link is one axis, and the shoulder portion 6 that is the second link, Two joint axes of the rotary joint connecting between the lower link 7 as the third link and between the lower arm 7 as the third link and the first upper arm 8 as the fourth link Three joint axes of the rotary joint to be connected, and four joint axes of the rotary joint connecting the first upper arm 8 being the fourth link and the second upper arm 9 being the fifth link. In addition, the five joint axes of the rotary joint connecting the second upper arm 9 as the fifth link and the wrist 10 as the sixth link, and the wrist 10 and the seventh link as the sixth link The joint axes of the rotary joint that connects with the flange 11 are shown as six axes.

図１は、ロボット制御装置３及びロボット２の電気的な構成を機能ブロック図により示している。ロボット制御装置３は、マイクロコンピュータからなるＣＰＵ１２（本発明でいう最大弾性変形量角度記憶手段、弾性変形量計算手段、弾性変形方向検出手段、モータ制御手段）と、ＣＰＵ１２からの動作指令に基づいて駆動信号をロボット２に出力する駆動回路１３と、ロボット２の動作プログラムなどを記憶するＲＡＭ１４と、ロボット２のシステムプログラムや動作プログラムを作成するためのロボット言語などを記憶するＲＯＭ１５とを備えて構成されている。ロボット２は、ショルダ部６、各アーム７〜９、手首１０及びフランジ１１を駆動させるモータ１６と、モータ１６の回転角度を検出するエンコーダ１７とを備えている。尚、これらモータ１６及びエンコーダ１７はリンク毎に設けられている。 FIG. 1 is a functional block diagram showing the electrical configuration of the robot control device 3 and the robot 2. The robot control device 3 is based on a CPU 12 (maximum elastic deformation amount angle storage means, elastic deformation amount calculation means, elastic deformation direction detection means, motor control means) according to the present invention, and an operation command from the CPU 12. A drive circuit 13 that outputs a drive signal to the robot 2, a RAM 14 that stores an operation program for the robot 2, and a ROM 15 that stores a robot language for creating a system program and an operation program for the robot 2. Has been. The robot 2 includes a motor 16 that drives the shoulder unit 6, the arms 7 to 9, the wrist 10, and the flange 11, and an encoder 17 that detects the rotation angle of the motor 16. The motor 16 and the encoder 17 are provided for each link.

ＣＰＵ１２は、リンク毎のエンコーダ１７から入力したエンコーダ角度（エンコーダ値）に基づいてベース５に対するショルダ部６の回転角度、ショルダ部６に対する下アーム７の回転角度、下アーム７に対する第１の上アーム８の回転角度、第１の上アーム８に対する第２の上アーム９の回転角度、第２の上アーム９に対する手首１０の回転角度、手首１０に対するフランジ１１の回転角度を検出し、ロボット２の位置（姿勢）を検出すると共に、エンコーダ角度をフィードバック制御して駆動回路１３から駆動信号を出力させ、それらショルダ部６、各アーム７〜９、手首１０及びフランジ１１の動作を制御する。 The CPU 12 determines the rotation angle of the shoulder portion 6 with respect to the base 5 based on the encoder angle (encoder value) input from the encoder 17 for each link, the rotation angle of the lower arm 7 with respect to the shoulder portion 6, and the first upper arm with respect to the lower arm 7. 8, the rotation angle of the second upper arm 9 relative to the first upper arm 8, the rotation angle of the wrist 10 relative to the second upper arm 9, and the rotation angle of the flange 11 relative to the wrist 10 are detected. While detecting the position (posture), the encoder angle is feedback-controlled to output a drive signal from the drive circuit 13, and the operations of the shoulder unit 6, the arms 7 to 9, the wrist 10 and the flange 11 are controlled.

ロボット２における各軸の駆動部分は、図３に示すように、モータ１６のモータ軸１８に取付けられているモータ軸側ハーモニックギア（以下、モータ軸側ギアと称する）１９と出力軸２０に取付けられている出力軸側ハーモニックギア（以下、出力軸側ギアと称する）２１とが噛み合っており、ロボット制御装置３からモータ１６に駆動信号が出力されてトルク指令値が与えられることでモータ１６がモータ軸側ギア１９を回転させ、モータ軸側ギア１９が回転されることに追従して出力軸側ギア２１が回転されるように構成されている。尚、これ以降、モータ軸１８の回転角度を検出するエンコーダ１７が出力するエンコーダ角度をモータ軸側エンコーダ角度として定義し、出力軸２０の回転角度を検出するエンコーダが出力するエンコーダ角度を出力軸側エンコーダ角度として定義する。 As shown in FIG. 3, the drive portion of each axis in the robot 2 is attached to a motor shaft side harmonic gear (hereinafter referred to as a motor shaft side gear) 19 attached to a motor shaft 18 of the motor 16 and an output shaft 20. The output shaft side harmonic gear (hereinafter referred to as output shaft side gear) 21 is engaged with the motor 16, and a drive signal is output from the robot control device 3 to the motor 16 to give a torque command value. The motor shaft side gear 19 is rotated, and the output shaft side gear 21 is rotated following the rotation of the motor shaft side gear 19. Hereinafter, the encoder angle output by the encoder 17 that detects the rotation angle of the motor shaft 18 is defined as the motor shaft side encoder angle, and the encoder angle output by the encoder that detects the rotation angle of the output shaft 20 is the output shaft side. Define as encoder angle.

上記した構成では、モータ軸１８のねじれ、モータ軸１８とモータ軸側ギア１９との接続箇所のねじれ、モータ軸側ギア１９の変形、出力軸側ギア２１の変形及び出力軸側ギア２１と出力軸２０との接続箇所のねじれに起因する弾性変形が発生する。このような事情から、本実施形態ではＣＰＵ１２は以下に示す制御を行う。以下、上記した構成の作用として、ＣＰＵ１２が本発明に関連して行う制御を図４乃至図１６を参照して説明する。ロボット制御装置３において、ＣＰＵ１２は、ＲＡＭ１４に記憶されている動作プログラムを実行して所定周期（サンプリング単位周期）で以下に示すメイン処理を実行する。 In the configuration described above, the motor shaft 18 is twisted, the connection portion of the motor shaft 18 and the motor shaft side gear 19 is twisted, the motor shaft side gear 19 is deformed, the output shaft side gear 21 is deformed, and the output shaft side gear 21 and the output are output. Elastic deformation due to torsion of the connecting portion with the shaft 20 occurs. Under such circumstances, in the present embodiment, the CPU 12 performs the following control. Hereinafter, as the operation of the above-described configuration, the control performed by the CPU 12 in relation to the present invention will be described with reference to FIGS. In the robot controller 3, the CPU 12 executes an operation program stored in the RAM 14 and executes the following main process at a predetermined cycle (sampling unit cycle).

ＣＰＵ１２は、実行中にある動作プログラムから目標角度指令を取得すると（ステップＳ１）、その目標角度指令により指示されている目標角度（目標値）とエンコーダ１７から入力しているモータ軸側エンコーダ角度とに基づいて指令速度を計算し（ステップＳ２）、計算した指令速度とエンコーダ速度とに基づいてトルク指令値を計算する（ステップＳ３）。そして、ＣＰＵ１２は、現在のモータ軸側エンコーダ角度と現在の出力軸側エンコーダ角度とに基づいて弾性変形量を更新する弾性変形量更新処理に移行する（ステップＳ４）。 When the CPU 12 obtains the target angle command from the operation program being executed (step S1), the target angle (target value) instructed by the target angle command and the motor shaft side encoder angle input from the encoder 17 The command speed is calculated based on (step S2), and the torque command value is calculated based on the calculated command speed and encoder speed (step S3). Then, the CPU 12 proceeds to an elastic deformation amount update process for updating the elastic deformation amount based on the current motor shaft side encoder angle and the current output shaft side encoder angle (step S4).

ＣＰＵ１２は、メイン処理から弾性変形量更新処理に移行すると、現在の出力軸側エンコーダ角度を制御視点角度とし、制御視点角度から現在のモータ軸側エンコーダ角度を減算した値を弾性変形量として計算し（ステップＳ１１）、その計算した弾性変形量と予め記憶している正方向の最大弾性変形量及び負方向の弾性変形量との大小関係を比較判定する（ステップＳ１２、Ｓ１３）。ここで、ＣＰＵ１２は、計算した弾性変形量が正方向の最大弾性変形量を超えていなく（同等又は小さく）且つ負方向の最大弾性変形量を超えていない（同等又は大きい）と判定すると（ステップＳ１２にて「ＮＯ」、Ｓ１３にて「ＮＯ」）、モータ軸側ギア１９が弾性変形中であると判定し、現在の弾性変形量を維持し（ステップＳ１４）、弾性変形量更新処理を終了してメイン処理に戻り、弾性変形量から摩擦補償用トルクを決定する摩擦補償用トルク決定処理に移行する（ステップＳ５）。 When the CPU 12 shifts from the main process to the elastic deformation amount update process, the CPU calculates the current output shaft side encoder angle as the control viewpoint angle, and the value obtained by subtracting the current motor shaft side encoder angle from the control viewpoint angle as the elastic deformation amount. (Step S11), the magnitude relationship between the calculated elastic deformation amount and the positive maximum elastic deformation amount and negative elastic deformation amount stored in advance is compared (step S12, S13). When the CPU 12 determines that the calculated elastic deformation amount does not exceed the maximum elastic deformation amount in the positive direction (equivalent or small) and does not exceed the maximum elastic deformation amount in the negative direction (equivalent or large) (step) "NO" in S12, "NO" in S13), it is determined that the motor shaft side gear 19 is undergoing elastic deformation, the current elastic deformation amount is maintained (step S14), and the elastic deformation amount updating process is terminated. Then, the process returns to the main process, and shifts to a friction compensation torque determination process for determining the friction compensation torque from the elastic deformation amount (step S5).

一方、ＣＰＵ１２は、計算した弾性変形量が正方向の最大弾性変形量を超えていると判定すると（ステップＳ１２にて「ＹＥＳ」）、モータ軸側ギア１９が弾性変形中でないと判定し、その正方向の最大弾性変形量を弾性変形量として設定し（ステップＳ１５）、正方向の最大弾性変形量と現在のモータ軸側エンコーダ角度とを加算した値を制御視点角度として計算し（ステップＳ１６）、弾性変形量更新処理を終了してメイン処理に戻り、摩擦補償用トルク決定処理に移行する（ステップＳ５）。 On the other hand, when determining that the calculated elastic deformation amount exceeds the maximum elastic deformation amount in the positive direction (“YES” in step S12), the CPU 12 determines that the motor shaft side gear 19 is not elastically deforming, and The maximum elastic deformation amount in the positive direction is set as the elastic deformation amount (step S15), and the value obtained by adding the maximum elastic deformation amount in the positive direction and the current motor shaft side encoder angle is calculated as the control viewpoint angle (step S16). Then, the elastic deformation amount update process is terminated, the process returns to the main process, and the process proceeds to the friction compensation torque determination process (step S5).

又、ＣＰＵ１２は、計算した弾性変形量が負方向の最大弾性変形量を超えていると判定すると（ステップＳ１３にて「ＹＥＳ」）、モータ軸側ギア１９が弾性変形中でないと判定し、その負方向の最大弾性変形量を弾性変形量として設定し（ステップＳ１７）、負方向の最大弾性変形量と現在のモータ軸側エンコーダ角度とを加算した値を制御視点角度として計算し（ステップＳ１８）、弾性変形量更新処理を終了してメイン処理に戻り、摩擦補償用トルク決定処理に移行する（ステップＳ５）。 If the CPU 12 determines that the calculated elastic deformation amount exceeds the negative maximum elastic deformation amount ("YES" in step S13), the CPU 12 determines that the motor shaft side gear 19 is not elastically deforming, and The maximum elastic deformation amount in the negative direction is set as the elastic deformation amount (step S17), and a value obtained by adding the maximum elastic deformation amount in the negative direction and the current motor shaft side encoder angle is calculated as the control viewpoint angle (step S18). Then, the elastic deformation amount update process is terminated, the process returns to the main process, and the process proceeds to the friction compensation torque determination process (step S5).

次いで、ＣＰＵ１２は、メイン処理から摩擦補償用トルク決定処理に移行すると、目標方向と弾性変形方向とを比較し、目標方向と弾性変形方向とが逆方向であるか否かを判定する（ステップＳ２１）。ここで、ＣＰＵ１２は、目標方向と弾性変形方向とが逆方向であると判定すると（ステップＳ２１にて「ＹＥＳ」）、摩擦補償用トルクを減少させ（ステップＳ２２）、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時のチャタリングを防止する停止時チャタリング防止処理に移行する（ステップＳ６）。 Next, when shifting from the main process to the friction compensation torque determination process, the CPU 12 compares the target direction with the elastic deformation direction and determines whether the target direction and the elastic deformation direction are opposite directions (step S21). ). If the CPU 12 determines that the target direction and the elastic deformation direction are opposite (“YES” in step S21), the CPU 12 decreases the friction compensation torque (step S22), and performs the friction compensation torque determination process. The process returns to the main process and proceeds to a stop chattering prevention process for preventing chattering during a stop (step S6).

又、ＣＰＵ１２は、目標方向と弾性変形方向とが逆方向でないと判定すると（ステップＳ２１にて「ＮＯ」）、弾性変形量が最大弾性変形量であるか否かを判定し（ステップＳ２３）、弾性変形量が最大弾性変形量でないと判定すると（ステップＳ２３にて「ＮＯ」）、摩擦補償用トルクを増加させ（ステップＳ２４）、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時チャタリング防止処理に移行する（ステップＳ６）。一方、ＣＰＵ１２は、弾性変形量が最大弾性変形量であると判定すると（ステップＳ２３にて「ＹＥＳ」）、予め設定されている最大補償トルクを摩擦補償用トルクとして設定し（ステップＳ２５）、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時チャタリング防止処理に移行する（ステップＳ６）。 If the CPU 12 determines that the target direction and the elastic deformation direction are not opposite directions ("NO" in step S21), the CPU 12 determines whether the elastic deformation amount is the maximum elastic deformation amount (step S23). If it is determined that the amount of elastic deformation is not the maximum amount of elastic deformation (“NO” in step S23), the friction compensation torque is increased (step S24), the friction compensation torque determination process is terminated, the process returns to the main process, and is stopped. The process proceeds to the hour chattering prevention process (step S6). On the other hand, when the CPU 12 determines that the elastic deformation amount is the maximum elastic deformation amount ("YES" in step S23), the CPU 12 sets the preset maximum compensation torque as the friction compensation torque (step S25), and the friction The compensation torque determination process is terminated, the process returns to the main process, and the process proceeds to the stop chattering prevention process (step S6).

次いで、ＣＰＵ１２は、メイン処理から停止時チャタリング防止処理に移行すると、計算した指令速度がゼロであるか否か（ロボット２の動作停止命令であるか否か）を判定する（ステップＳ３１）。ＣＰＵ１２は、指令速度がゼロでないと判定すると（ステップＳ３１にて「ＮＯ」）、指令速度ゼロフラグをオンに設定しているか否かを判定し（ステップＳ３２）、指令速度ゼロフラグをオンに設定していないと判定すると（ステップＳ３２にて「ＮＯ」）、現在の摩擦補償用トルクを保存補償トルクとして設定し（ステップＳ３３）、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、ＣＰＵ１２は、摩擦補償用トルクをトルク指令値に加算し（ステップＳ７）、メイン処理を終了する。 Next, when the CPU 12 shifts from the main process to the stop chattering prevention process, the CPU 12 determines whether or not the calculated command speed is zero (whether or not it is an operation stop command for the robot 2) (step S31). If the CPU 12 determines that the command speed is not zero (“NO” in step S31), the CPU 12 determines whether or not the command speed zero flag is set to on (step S32), and the command speed zero flag is set to on. If it is determined that there is not (“NO” in step S32), the current friction compensation torque is set as the storage compensation torque (step S33), the stop chattering prevention process is terminated, and the process returns to the main process. Then, the CPU 12 adds the friction compensation torque to the torque command value (step S7) and ends the main process.

ここで、ＣＰＵ１２は、指令速度がゼロであると判定すると（ステップＳ３１にて「ＹＥＳ」）、指令速度ゼロフラグをオフに設定しているか否かを判定し（ステップＳ３４）、指令速度ゼロフラグをオフに設定していると判定すると（ステップＳ３４にて「ＹＥＳ」）、指令速度ゼロフラグをオンに設定し（ステップＳ３５）、現在の保存補償トルクを摩擦補償用トルクとして設定し（ステップＳ３６）、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、ＣＰＵ１２は、摩擦補償用トルクをトルク指令値に加算し（ステップＳ７）、メイン処理を終了する。即ち、ＣＰＵ１２は、ロボット２の動作停止時には、ロボット２の動作を停止させる直前の摩擦補償用トルクを維持する。 If the CPU 12 determines that the command speed is zero (“YES” in step S31), the CPU 12 determines whether the command speed zero flag is set to off (step S34), and turns off the command speed zero flag. Is set (step S34: YES), the command speed zero flag is set to ON (step S35), the current storage compensation torque is set as the friction compensation torque (step S36), and the operation is stopped. End the hour chattering prevention process and return to the main process. Then, the CPU 12 adds the friction compensation torque to the torque command value (step S7) and ends the main process. That is, when the operation of the robot 2 is stopped, the CPU 12 maintains the friction compensation torque immediately before the operation of the robot 2 is stopped.

次いで、ＣＰＵ１２は、指令速度がゼロでないと判定し（ステップＳ３１にて「ＮＯ」）、指令速度ゼロフラグをオンに設定していると判定すると（ステップＳ３２にて「ＹＥＳ」）、指令速度ゼロフラグをオフに設定し（ステップＳ３７）、外乱力に対する補償を行う積分補償の状態量を次に示す計算式にしたがって計算する（ステップＳ３８）。 Next, when the CPU 12 determines that the command speed is not zero (“NO” in step S31) and determines that the command speed zero flag is set to ON (“YES” in step S32), the command speed zero flag is set. It is set to off (step S37), and an integral compensation state quantity for compensating for disturbance force is calculated according to the following calculation formula (step S38).

積分補償の状態量←積分補償の状態量−（保存補償トルク−摩擦補償用トルク）／積分補償ゲイン
そして、ＣＰＵ１２は、現在の摩擦補償用トルクを保存補償トルクとして設定し（ステップＳ３３）、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、ＣＰＵ１２は、摩擦補償用トルクをトルク指令値に加算し（ステップＳ７）、メイン処理を終了する。 Integral compensation state quantity ← integral compensation state quantity-(storage compensation torque-friction compensation torque) / integration compensation gain Then, the CPU 12 sets the current friction compensation torque as the storage compensation torque (step S33) and stops. End the hour chattering prevention process and return to the main process. Then, the CPU 12 adds the friction compensation torque to the torque command value (step S7) and ends the main process.

尚、上記した処理では、現在の出力軸側エンコーダ角度を制御視点角度とし、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分に基づいて弾性変形量を計算する方法を説明したが、エンコーダ１７が電源投入時に示す角度を弾性変形量ゼロ角度とし、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分に基づいて弾性変形量を計算するようにしても良い。 In the above processing, the method of calculating the elastic deformation amount based on the difference between the motor shaft side encoder angle and the output shaft side encoder angle with the current output shaft side encoder angle as the control viewpoint angle has been described. The angle indicated by 17 when the power is turned on may be the elastic deformation amount zero angle, and the elastic deformation amount may be calculated based on the difference between the motor shaft side encoder angle and the elastic deformation amount zero angle.

又、このようにモータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させる点及び摩擦補償用トルクと加速トルクとを切分けて制御している点を特徴とするのは、前述した「課題を解決するための手段」で説明した通りである。 Further, it is characterized in that the friction compensation torque applied to the motor shaft side gear in the target drive direction is gradually increased and the friction compensation torque and the acceleration torque are controlled separately. Is as described in the above-mentioned “Means for Solving the Problems”.

図８乃至図１０は、モータ軸側ギア１９が動作開始時に弾性変形する態様及び摩擦補償用トルクの推移を示しており、図１１及び図１２は、モータ軸側ギア１９が動作停止時に弾性変形する態様及び摩擦補償用トルクの推移を示している。尚、図８乃至図１２中では、「ａ１」は制御視点角度を示しており、「ａ２」はモータ軸側エンコーダ角度を示している。 8 to 10 show how the motor shaft side gear 19 is elastically deformed at the start of operation and the transition of the friction compensation torque. FIGS. 11 and 12 show the elastic deformation of the motor shaft side gear 19 when the operation is stopped. And the transition of the friction compensation torque. 8 to 12, “a1” indicates the control viewpoint angle, and “a2” indicates the motor shaft side encoder angle.

図８は、モータ軸側ギア１９の弾性変形量がゼロである状態から出力軸２０を目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクがゼロの状態から摩擦補償用トルクをモータ軸側ギア１９の弾性変形量が最大弾性変形量に達するまで正比例的に増加させ、モータ軸側ギア１９の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えない領域では弾性変形し、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して出力軸２０が動作開始する。 FIG. 8 shows a case where the output shaft 20 is driven in the target drive direction from a state where the elastic deformation amount of the motor shaft side gear 19 is zero. In this case, the friction compensation is performed from a state where the friction compensation torque is zero. The torque for use is increased in direct proportion until the amount of elastic deformation of the motor shaft side gear 19 reaches the maximum amount of elastic deformation, and is constant after the amount of elastic deformation of the motor shaft side gear 19 reaches the maximum amount of elastic deformation. That is, the output shaft 20 starts elastically deforming in a region where the friction compensation torque does not exceed the maximum compensation torque, and is deformed up to the maximum elastic deformation amount in a region where the friction compensation torque exceeds the maximum compensation torque.

図９は、モータ軸側ギア１９の弾性変形量がゼロでない状態から出力軸２０を目標駆動方向に駆動させる場合であり、出力軸を弾性変形方向と同一方向である目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクが所定値の状態から摩擦補償用トルクをモータ軸側ギア１９の弾性変形量が最大弾性変形量に達するまで正比例的に増加させ、モータ軸側ギア１９の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して最初の弾性変形方向と同一方向に出力軸２０が動作開始する。 FIG. 9 shows a case where the output shaft 20 is driven in the target drive direction from a state where the amount of elastic deformation of the motor shaft side gear 19 is not zero, and a case where the output shaft is driven in the target drive direction which is the same direction as the elastic deformation direction. In this case, from the state where the friction compensation torque is a predetermined value, the friction compensation torque is increased in direct proportion until the elastic deformation amount of the motor shaft side gear 19 reaches the maximum elastic deformation amount. After the elastic deformation amount of the gear 19 reaches the maximum elastic deformation amount, a constant value is set. That is, in the region where the friction compensation torque exceeds the maximum compensation torque, the output shaft 20 starts to operate in the same direction as the first elastic deformation direction by deformation up to the maximum elastic deformation amount.

図１０は、モータ軸側ギア１９の弾性変形量がゼロでない状態から出力軸２０を目標駆動方向に駆動させる場合であり、出力軸を弾性変形方向と反対方向である目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクが所定値の状態から摩擦補償用トルクをモータ軸側ギア１９の弾性変形量が最大弾性変形量に達するまで正比例的に減少させ、モータ軸側ギア１９の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して最初の弾性変形方向と反対方向に出力軸２０が動作開始する。 FIG. 10 shows a case where the output shaft 20 is driven in the target drive direction from a state where the elastic deformation amount of the motor shaft side gear 19 is not zero, and a case where the output shaft is driven in the target drive direction opposite to the elastic deformation direction. In this case, the friction compensation torque is decreased in a directly proportional manner from the state where the friction compensation torque is a predetermined value until the elastic deformation amount of the motor shaft side gear 19 reaches the maximum elastic deformation amount. After the elastic deformation amount of the gear 19 reaches the maximum elastic deformation amount, a constant value is set. That is, in the region where the friction compensation torque exceeds the maximum compensation torque, the output shaft 20 is deformed up to the maximum elastic deformation amount and the operation of the output shaft 20 starts in the direction opposite to the first elastic deformation direction.

図１１は、出力軸側ギア２１が目標位置を通過することなく停止した場合を示しており、この場合は、出力軸側ギア２１が目標位置で停止した状態の摩擦補償用トルクを維持する。図１２は、出力軸側ギア２１が目標位置を通過して停止した場合を示しており、この場合は、出力軸側ギア２１を目標位置まで戻すために摩擦補償用トルクを減少させ、出力軸側ギア２１が目標位置で停止した状態の摩擦補償用トルクを維持する。 FIG. 11 shows a case where the output shaft side gear 21 is stopped without passing through the target position. In this case, the friction compensation torque in a state where the output shaft side gear 21 is stopped at the target position is maintained. FIG. 12 shows a case where the output shaft side gear 21 has stopped after passing through the target position. In this case, the friction compensation torque is decreased to return the output shaft side gear 21 to the target position, and the output shaft The friction compensation torque is maintained while the side gear 21 is stopped at the target position.

ところで、上記したように摩擦補償を実施した場合では、理論上では振動することなくロボット２が動作停止すると考えられるか、実際には僅かな補償誤差や外乱力（主には重力）に起因して振動が発生する。そのため、ロボット２の動作停止時には摩擦補償を停止する必要があるが、単純に摩擦補償をオンオフする制御では不連続な部分が発生して新たな振動が発生する要因となる。即ち、図１３に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況から摩擦が変化すると、摩擦が変化したことに追従して摩擦補償が変化することで、両者の均衡が継続して保たれることになる。 By the way, when friction compensation is performed as described above, the robot 2 is theoretically considered to stop operating without vibrating, or actually caused by a slight compensation error or disturbance force (mainly gravity). Cause vibration. Therefore, it is necessary to stop the friction compensation when the operation of the robot 2 is stopped. However, in the control for simply turning on / off the friction compensation, a discontinuous portion is generated and a new vibration is generated. That is, as shown in FIG. 13, when the friction changes from the situation where the sum of disturbance force and friction is the same as the sum of integral compensation and friction compensation, and the balance between the two is maintained, the fact that the friction has changed is followed. As the friction compensation is changed, the balance between the two is continuously maintained.

一方、図１４に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況で摩擦補償をオフすると、摩擦補償をオフした分だけ両者の均衡が崩れる。そして、両者の均衡が崩れると、オフした分の摩擦補償を時間が経過するにしたがって積分補償が補うことになり、その結果、両者の均衡が保たれるようになる。しかしながら、オフした分の摩擦補償を積分補償が補ったままの状況でロボット２の動作開始時（動作開始直後）に摩擦補償をオンすると、摩擦補償をオンした分だけ両者の均衡が崩れることになる。 On the other hand, as shown in FIG. 14, when the friction compensation is turned off in the situation where the sum of the disturbance force and the friction is the same as the sum of the integral compensation and the friction compensation and the balance between them is maintained, the amount of the friction compensation turned off. Only the balance of the two is broken. When the balance between the two is lost, the integral compensation compensates for the friction compensation corresponding to the off time as time passes, and as a result, the balance between the two is maintained. However, if the friction compensation is turned on at the start of the operation of the robot 2 (immediately after the start of the operation) in a situation where the integral compensation compensates for the friction compensation for the amount that has been turned off, the balance between the two will be lost as much as the friction compensation is turned on. Become.

そこで、本実施形態では、図１５に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況では摩擦補償をオフすることなく更新を停止する。即ち、ロボット２の動作停止直後では両者の均衡が保たれるようになる。その状況から時間経過にしたがって摩擦及び外乱力が変化すると、摩擦補償の更新を停止しているので、それら摩擦及び外乱力の変化分を積分補償が補うことで、両者の均衡が継続して保たれる。そして、ロボット２の動作開始時に摩擦補償の更新を再開し、更新を停止した時点から摩擦補償が変化することで摩擦と摩擦補償との均衡は保たれるが、その分、それまでに摩擦の変化分を補っていた積分補償に相当する分の補償量だけ均衡が崩れることになる。したがって、均衡を保つために、ロボット２の動作開始時の積分補償用トルク（状態量）を次式にしたがって計算して変更する。 Therefore, in the present embodiment, as shown in FIG. 15, in a situation where the sum of disturbance force and friction is the same as the sum of integral compensation and friction compensation and the balance between the two is maintained, friction compensation is not turned off. Stop the update. That is, the balance between the two is maintained immediately after the operation of the robot 2 stops. If the friction and disturbance force change over time from the situation, the update of the friction compensation is stopped. Therefore, the integral compensation compensates for the change of the friction and disturbance force, so that the balance between the two continues. Be drunk. Then, the friction compensation is restarted at the start of the operation of the robot 2, and the friction compensation is changed from the point when the update is stopped, so that the balance between the friction and the friction compensation is maintained. The balance is broken by the amount of compensation corresponding to the integral compensation that compensates for the change. Therefore, in order to maintain the balance, the integral compensation torque (state quantity) at the start of the operation of the robot 2 is calculated and changed according to the following equation.

ロボットの動作開始時の積分補償の状態量（図１５中（１）参照）
＝ロボットの動作開始直前の積分補償の状態量（図１５中（２）参照）−（ロボットの動作開始時の摩擦補償の状態量（図１５中（３）参照）−ロボットの動作開始直前の摩擦補償の状態量（図１５中（４）参照））
上記した計算式における（ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量）は補正量に相当する。 State quantity of integral compensation at the start of robot operation (see (1) in Fig. 15)
= Integral compensation state quantity immediately before the start of robot operation (see (2) in FIG. 15)-(Friction compensation state quantity at the start of robot operation (see (3) in FIG. 15))- State quantity of friction compensation (see (4) in Fig. 15))
In the above calculation formula, (the amount of state of friction compensation at the start of operation of the robot−the amount of state of friction compensation immediately before the start of operation of the robot) corresponds to the correction amount.

尚、上記した図１５では、ロボット２の動作停止中に摩擦が増加したことで積分補償を加えたので、その加えた分を結果的に減じる場合を説明したが、これとは反対に、ロボット２の動作停止中に摩擦が低下したことで積分補償を減じた場合には、その減じた分を結果的に加えることになり、その場合も、不連続となることはなく、両者の均衡が継続して保たれることになる。 Note that in FIG. 15 described above, the integral compensation was added because the friction increased while the operation of the robot 2 was stopped, so the case where the added amount was reduced as a result was explained. If the integral compensation is reduced due to the decrease in friction during the operation stop of 2, the reduced amount will be added as a result. In this case, the balance between the two will not be discontinuous. It will be maintained continuously.

図１６は摩擦補償及び停止時チャタリング防止処理を行ったか否かによる動作中及び動作停止後の振動の発生状況を示す図であり、（ａ）は摩擦補償を行わなかった場合での振動の発生状況を示しており、（ｂ）は摩擦補償を行わったが停止時チャタリング防止処理を行わなかった場合での振動の発生状況を示しており、（ｃ）は摩擦補償を行って停止時チャタリング防止処理を行った場合での振動の発生状況を示している。図１６から明らかなように、摩擦補償を行った場合には、摩擦補償を行わなかった場合に比べて、動作中には振動の発生が抑制されるが、その一方で、動作停止後には逆に振動が発生している。しかしながら、摩擦補償を行った後に上記した積分補償の状態量を変化させることで、摩擦補償を行わなかった場合と同程度まで振動の発生が抑制される。 FIG. 16 is a diagram showing the occurrence of vibration during operation and after operation stop depending on whether or not friction compensation and chattering prevention processing at the time of stop are performed. FIG. 16A shows the occurrence of vibration when friction compensation is not performed. (B) shows the occurrence of vibrations when the friction compensation is performed but the stop chattering prevention process is not performed, and (c) shows the chattering at the stop with the friction compensation. It shows the occurrence of vibrations when the prevention process is performed. As is clear from FIG. 16, when friction compensation is performed, the occurrence of vibration is suppressed during operation compared to when friction compensation is not performed. Vibration has occurred in However, by changing the state quantity of the integral compensation described above after performing the friction compensation, the occurrence of vibration is suppressed to the same extent as when the friction compensation is not performed.

以上に説明したように第１の実施形態によれば、ロボット制御装置３において、モータ軸側ギア１９と出力軸側ギア２１とが停止しており且つ弾性変形量がゼロである状態から出力軸２０を目標駆動方向に駆動させる場合に、モータ軸側ギア１９に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット２の駆動を開始する場合などであり、弾性変形していない停止状態から出力軸２０を目標駆動方向に駆動させる場合に、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことでロボット２の実際の動作軌跡をより理論に近づけることができ、ロボットの動作軌跡を高い精度で再現することができる。 As described above, according to the first embodiment, in the robot control device 3, the output shaft from the state where the motor shaft side gear 19 and the output shaft side gear 21 are stopped and the elastic deformation amount is zero. When the motor 20 is driven in the target drive direction, the friction compensation torque applied to the motor shaft side gear 19 in the target drive direction is gradually increased, and the elastic deformation amount is reached after the elastic deformation amount reaches the maximum elastic deformation amount. Since the friction compensation torque at the time when the maximum elastic deformation amount is reached is applied as a constant value, for example, when driving of the robot 2 is started, the output shaft is stopped from the stop state where the elastic deformation is not caused. When the robot 20 is driven in the target drive direction, the actual motion trajectory of the robot 2 can be obtained by appropriately performing friction compensation by applying friction compensation torque in consideration of such elastic deformation. Can be brought close to more theory, it is possible to reproduce the operation path of the robot with high accuracy.

又、モータ軸側ギア１９と出力軸側ギア２１とが停止しており且つ弾性変形量がゼロでない状態から出力軸２０を弾性変形方向と同一方向である目標駆動方向に駆動させる場合に、モータ軸側ギア１９に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット２の動作途中で正転を一旦停止して正転を再開する場合などであり、弾性変形している停止状態から出力軸２０を弾性変形方向と同一方向である目標駆動方向に駆動させる場合にも、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことができ、ロボットの動作軌跡の精度を適切に高めることができる。 In addition, when the motor shaft side gear 19 and the output shaft side gear 21 are stopped and the amount of elastic deformation is not zero, the output shaft 20 is driven in a target drive direction that is the same direction as the elastic deformation direction. The friction compensation torque in the target drive direction applied to the shaft side gear 19 is gradually increased, and after the elastic deformation reaches the maximum elastic deformation, the friction at the time when the elastic deformation reaches the maximum elastic deformation. Since the compensation torque is applied as a constant value, for example, when the forward rotation is temporarily stopped during the operation of the robot 2 and the normal rotation is resumed, the output shaft 20 is stopped from the elastically deformed stopped state. Even when driving in a target drive direction that is the same direction as the elastic deformation direction, friction compensation can be appropriately performed by applying the friction compensation torque in consideration of such elastic deformation, The accuracy of Tsu City of operation path can be appropriately enhanced.

又、モータ軸側ギア１９と出力軸側ギア２１とが停止しており且つ弾性変形量がゼロでない状態から出力軸２０を弾性変形方向と反対方向である目標駆動方向に駆動させる場合に、モータ軸側ギア１９に印加する目標駆動方向とは反対方向への摩擦補償用トルクを徐々に減少させた後にモータ軸側ギア１９に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット２の動作途中で正転を一旦停止して正転を再開する場合などであり、弾性変形している停止状態から出力軸２０を弾性変形方向と反対方向である目標駆動方向に駆動させる場合にも、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことができ、ロボットの動作軌跡の精度を適切に高めることができる。 When the motor shaft side gear 19 and the output shaft side gear 21 are stopped and the elastic deformation amount is not zero, the output shaft 20 is driven in the target drive direction opposite to the elastic deformation direction. After gradually reducing the friction compensation torque in the direction opposite to the target drive direction applied to the shaft side gear 19, gradually increase the friction compensation torque in the target drive direction applied to the motor shaft side gear 19, After the elastic deformation amount reaches the maximum elastic deformation amount, the configuration is such that the friction compensation torque when the elastic deformation amount reaches the maximum elastic deformation amount is applied as a constant value. This is also the case when the forward rotation is temporarily stopped and the normal rotation is restarted, and when the output shaft 20 is driven in the target drive direction opposite to the elastic deformation direction from the stopped state where the elastic deformation is performed. Elasticity it can be appropriately performed friction compensation by applying a friction compensation torque taking into account the deforming, can be appropriately increase the accuracy of the motion track of the robot.

更に、ロボット２の動作停止時では摩擦補償をオフすることなく更新を停止し、ロボット２の動作再開時では、ロボット２の動作再開時の摩擦補償とロボット２の動作再開直前の摩擦補償との差分を補正量として計算し、ロボット２の動作再開時の積分補償をロボット２の動作開始直前の積分補償から補正量を差引くことで計算するように構成したので、ロボット２の動作再開直後においても外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で両者の均衡を保つことができ、ロボット２の動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。又、このように積分補償の状態量を変化させるだけで良いので、制御系の安定性に悪影響を与えることなく、単純な演算だけで実装可能である。 Further, when the operation of the robot 2 is stopped, the update is stopped without turning off the friction compensation, and when the operation of the robot 2 is resumed, the friction compensation when the operation of the robot 2 is resumed and the friction compensation immediately before the operation of the robot 2 is resumed. The difference is calculated as a correction amount, and the integral compensation when the robot 2 resumes operation is calculated by subtracting the compensation amount from the integral compensation immediately before the robot 2 motion starts. Furthermore, the balance between the disturbance force and the sum of the friction and the sum of the integral compensation and the friction compensation can be maintained, and the accuracy of the entire motion trajectory from the start to the stop of the operation of the robot 2 can be further improved. . Further, since it is only necessary to change the state quantity of the integral compensation in this way, it can be implemented by simple calculation without adversely affecting the stability of the control system.

（第２の実施形態）
次に、本発明の第２の実施形態について、図２０乃至図２３を参照して説明する。尚、上記した第１の実施形態と同一部分については説明を省略し、異なる部分について説明する。第２の実施形態は、ロボット２の動作停止時にオーバーシュートの現象が発生することを考慮し、第１の実施形態で説明した動作開始時に行う摩擦補償と同様の制御を動作停止時にも行うことで、ロボット２の動作軌跡をより高い精度で再現するものである。 (Second Embodiment)
Next, a second embodiment of the present invention will be described with reference to FIGS. In addition, description is abbreviate | omitted about the same part as above-mentioned 1st Embodiment, and a different part is demonstrated. In the second embodiment, in consideration of the occurrence of an overshoot phenomenon when the operation of the robot 2 is stopped, the same control as the friction compensation performed at the start of the operation described in the first embodiment is performed even when the operation is stopped. Thus, the motion trajectory of the robot 2 is reproduced with higher accuracy.

即ち、ロボット２の動作停止時では、停止指令がロボット制御装置３から出力された後に速度をゼロとするので、停止指令が出力されてから速度をゼロとするまでの僅かな期間がタイムラグとして発生する。又、物体には物理現象としての慣性が存在する。よって、停止指令が出力された後に上記したタイムラグ分だけ遅れて実際の速度がゼロとなる。ロボット２の動作としては、このタイムラグが発生している期間中の動作により、目標角度を僅かに越えてしまい（オーバーシュートしてしまい）、その後に目標角度に対して僅かに越えてしまったオーバーシュート分を戻す動作を行うことで、目標角度に近づけるようにしている。 That is, when the operation of the robot 2 is stopped, the speed is set to zero after the stop command is output from the robot controller 3, so that a short period from the output of the stop command until the speed is set to zero is generated as a time lag. To do. In addition, the object has inertia as a physical phenomenon. Therefore, after the stop command is output, the actual speed becomes zero with a delay by the time lag described above. As the operation of the robot 2, the target angle is slightly exceeded (overshoot) due to the operation during the time lag occurs, and then the target angle is slightly exceeded. By performing the operation to return the shot, it is made closer to the target angle.

一方、ロボット２は例えば生産性の効率が高いことが要求されていることから、なるべく短い時間で正確な位置に到達されることが要求されており、よって、上記したようなオーバーシュートが発生した状況でもなるべく短い時間で目標角度に到達することが要求されている。そこで、通常のトルク制御である時間と目標角度までの差で積分を行い、積分値に比例したトルクを印加するものであるが、この方法では、そもそもオーバーシュート分の角度が極めて僅かであり、全体の動作軌跡の精度を高めることを考慮すると、最初は極めて僅かな積分量では極めて僅かなトルクを印加することしかできず、オーバーシュート分を戻る（方向反転）時に発生する静止摩擦を超える（打ち勝つ）ことができない。最初から大きなトルクを印加してしまうと、静止摩擦を超えることができるようになるが、全体の動作軌跡を精度良く形成することができなくなる。 On the other hand, since the robot 2 is required to have high productivity efficiency, for example, the robot 2 is required to reach an accurate position in as short a time as possible, and thus the above-described overshoot has occurred. Even under circumstances, it is required to reach the target angle in as short a time as possible. Therefore, the normal torque control is integrated with the difference between the time and the target angle, and the torque proportional to the integral value is applied, but in this method, the angle for the overshoot is very small in the first place, Considering the improvement of the accuracy of the entire motion trajectory, at first, only a very small amount of torque can be applied with a very small amount of integration, exceeding the static friction that occurs when the overshoot is returned (direction reversal) ( Can't win) If a large torque is applied from the beginning, it becomes possible to exceed the static friction, but the entire motion trajectory cannot be accurately formed.

そこで、第２の実施形態では、ロボット２の動作停止時においても、目標角度とモータ軸側エンコーダ角度との差分の絶対値が最大弾性変形量角度よりも大きな角度である設定角度を超えていることを条件に、ロボット２の動作開始時に弾性変形量を考慮した摩擦補償を行うことと同様に、弾性変形量を考慮した摩擦補償を行うことにより、オーバーシュート分を戻す制御を早く開始し、オーバーシュートを解消するまでにかかる時間を短縮し、ロボット２の動作軌跡をより高い精度で再現するものである。 Therefore, in the second embodiment, even when the operation of the robot 2 is stopped, the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle which is an angle larger than the maximum elastic deformation amount angle. In the same manner as the friction compensation in consideration of the elastic deformation amount at the start of the operation of the robot 2, the control for returning the overshoot is started early by performing the friction compensation in consideration of the elastic deformation amount, The time required to eliminate the overshoot is shortened, and the motion trajectory of the robot 2 is reproduced with higher accuracy.

第２の実施形態では、ＣＰＵ１２は、弾性変形量更新処理を行い（ステップＳ４）、摩擦補償用トルク決定処理を行った後に（ステップＳ５）、停止時摩擦補償処理を行う（ステップＳ４１）。この停止時摩擦補償処理は第１の実施形態で説明した停止時チャタリング防止処理を含む処理である。ＣＰＵ１２は、メイン処理から停止時摩擦補償処理に移行すると、指令速度がゼロであるか否か（ロボット２の動作停止命令であるか否か）を判定する（ステップＳ３１）。 In the second embodiment, the CPU 12 performs an elastic deformation amount update process (step S4), performs a friction compensation torque determination process (step S5), and then performs a stop time friction compensation process (step S41). This stop-time friction compensation process is a process including the stop-time chattering prevention process described in the first embodiment. When the CPU 12 shifts from the main process to the friction compensation process at stop, the CPU 12 determines whether or not the command speed is zero (whether or not it is an operation stop command for the robot 2) (step S31).

ここで、ＣＰＵ１２は、指令速度がゼロであると判定すると（ステップＳ３１にて「ＹＥＳ」）、第１の実施形態とは異なり、目標角度とモータ軸側エンコーダ角度との差分の絶対値を計算し、その計算した目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えているか否かを判定する（ステップＳ５１）。設定角度とは予め最大弾性変形量角度よりも大きな角度として設定された角度である。そして、ＣＰＵ１２は、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていると判定すると（ステップＳ５１にて「ＹＥＳ」）、第１の実施形態で説明したステップＳ３２以降の処理を行い、即ち、弾性変形量を考慮した摩擦補償を行う。これに対して、ＣＰＵ１２は、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていないと判定すると（ステップＳ５１にて「ＮＯ」）、第１の実施形態で説明したステップＳ３４以降の処理を行い、即ち、弾性変形量を考慮した摩擦補償を行うことはなく、続けて、現在の保存補償トルクを摩擦補償用トルクとして設定し、摩擦補償値を固定した停止時チャタリング防止の摩擦補償を行う。 If the CPU 12 determines that the command speed is zero (“YES” in step S31), the CPU 12 calculates the absolute value of the difference between the target angle and the motor shaft side encoder angle, unlike the first embodiment. Then, it is determined whether or not the absolute value of the difference between the calculated target angle and the motor shaft side encoder angle exceeds the set angle (step S51). The set angle is an angle set in advance as an angle larger than the maximum elastic deformation amount angle. If the CPU 12 determines that the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle (“YES” in step S51), the CPU 12 and subsequent steps described in the first embodiment are executed. That is, the friction compensation is performed in consideration of the elastic deformation amount. On the other hand, if the CPU 12 determines that the absolute value of the difference between the target angle and the motor shaft side encoder angle does not exceed the set angle (“NO” in step S51), the CPU 12 has been described in the first embodiment. Processing after step S34 is performed, that is, friction compensation is not performed in consideration of the amount of elastic deformation. Subsequently, the current storage compensation torque is set as the friction compensation torque, and the chattering at the stop with the friction compensation value fixed Prevent friction compensation.

尚、第１の実施形態で説明したように、ＣＰＵ１２は、メイン処理を所定周期（サンプリング単位周期）で行うことから、停止時摩擦補償処理を所定周期で行うことになり、即ち、指令速度がゼロであると判定し続けており、且つ目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていると判定し続けている期間で、弾性変形量を考慮した摩擦補償を行う。 As described in the first embodiment, since the CPU 12 performs the main process at a predetermined period (sampling unit period), the friction compensation process at the time of stop is performed at a predetermined period. Friction compensation taking into account the amount of elastic deformation during the period when it is determined that the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle. Do.

図２２は停止時摩擦補償処理を行わなかった場合の目標角度に近づくまでの挙動を示しており、図２３は停止時摩擦補償処理を行った場合の目標角度に近づくまでの挙動を示している。図２２及び図２３から明らかなように、停止時摩擦補償処理を行わなかった制御と停止時摩擦補償処理を行った制御とを比較すると、後者は前者よりも目標角度に近づくまでの戻り始め動作を開始するまでの時間が短いので、戻り始め動作を早く開始した分、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度の範囲内に収束するまでの時間が短くなっている。尚、停止時摩擦補償処理を行わなかった場合でのロボットの動作が停止される直前の摩擦補償用トルクは、停止指令が出力されたと同時に摩擦補償を行わなくなるので、図２２中「Ａ」にて示す位置であるが、一方、停止時摩擦補償処理を行った場合でのロボットの動作が停止される直前の摩擦補償用トルクは、停止指令が出力された以降でも実際の角度が設定角度の範囲内となるまで摩擦補償を継続するので、図２３中「Ｂ」にて示す位置である。 FIG. 22 shows the behavior until the target angle is approached when the stop-time friction compensation processing is not performed, and FIG. 23 shows the behavior until the target angle is approached when the stop-time friction compensation processing is performed. . As is apparent from FIGS. 22 and 23, when the control without the stop friction compensation process is compared with the control with the stop friction compensation process, the latter starts to return until it approaches the target angle than the former. Since the time until starting the operation is short, the time until the absolute value of the difference between the target angle and the motor shaft side encoder angle converges within the range of the set angle is shortened as soon as the operation to start returning is started earlier . Note that the friction compensation torque immediately before the operation of the robot is stopped when the friction compensation process at the time of stop is not performed is not performed at the same time as the stop command is output. On the other hand, the friction compensation torque immediately before the robot operation is stopped in the case where the friction compensation process at the time of stopping is performed, the actual angle is the same as the set angle even after the stop command is output. Since the friction compensation is continued until it falls within the range, the position is indicated by “B” in FIG.

以上に説明したように第２の実施形態によれば、ロボット２の動作停止時においても、目標角度とモータ軸側エンコーダ角度との差分が最大弾性変形量角度よりも大きな角度である設定角度を超えていることを条件に、ロボット２の動作開始時に弾性変形量を考慮した摩擦補償を行うことと同様に、弾性変形量を考慮した摩擦補償を行うように構成したので、戻り始め動作を開始するまでにかかる時間を短縮することができる。これにより、ロボット２の動作開始時だけでなく動作停止時までも含めて動作軌跡をより理論に近づけることができ、ロボット２の動作軌跡を高い精度で再現することができる。 As described above, according to the second embodiment, even when the operation of the robot 2 is stopped, the set angle at which the difference between the target angle and the motor shaft side encoder angle is larger than the maximum elastic deformation amount angle is set. Since it is configured to perform friction compensation in consideration of the amount of elastic deformation, in the same manner as performing friction compensation in consideration of the amount of elastic deformation at the start of operation of the robot 2 on the condition that it has exceeded, the operation to start returning starts The time taken to do so can be shortened. As a result, the motion trajectory can be made closer to the theory not only when the robot 2 starts operating but also when the motion stops, and the motion trajectory of the robot 2 can be reproduced with high accuracy.

又、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えなくなると（ある程度まで目標角度に収束すると）、第１の実施形態で説明した停止時チャタリング防止処理と同様の処理を行うように構成したので、ロボット２の動作停止時から動作開始時までの停止期間で外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で均衡を保つことができ、ロボット２の動作開始時においても外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で両者の均衡を保つことができ、ロボット２の動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。 When the absolute value of the difference between the target angle and the motor shaft side encoder angle does not exceed the set angle (when it converges to the target angle to some extent), the same processing as the stop chattering prevention processing described in the first embodiment Thus, the balance between the disturbance force and the friction and the sum of the integral compensation and the friction compensation can be maintained in the stop period from the stop of the operation of the robot 2 to the start of the operation. The balance between the disturbance force and friction sum, and the integral compensation and friction compensation sum can be maintained even at the start of the operation of the robot 2, and the accuracy of the entire motion trajectory from the start of the operation of the robot 2 to the stop of the operation can be improved. Can be increased.

（その他の実施形態）
本発明は、上記した実施形態にのみ限定されるものではなく、以下のように変形又は拡張することができる。
対象とするロボットは６軸の垂直多関節型ロボットに限らず他のロボットであっても良い。 (Other embodiments)
The present invention is not limited to the above-described embodiment, and can be modified or expanded as follows.
The target robot is not limited to a six-axis vertical articulated robot, and may be another robot.

図面中、２はロボット、３はロボット制御装置、１２はＣＰＵ（最大弾性変形量角度記憶手段、弾性変形量計算手段、弾性変形方向検出手段、モータ制御手段）、１６はモータ、１８はモータ軸、１９モータ軸側ギア、２０は出力軸、２１は出力軸側ギアである。 In the drawings, 2 is a robot, 3 is a robot controller, 12 is a CPU (maximum elastic deformation amount angle storage means, elastic deformation amount calculation means, elastic deformation direction detection means, motor control means), 16 is a motor, and 18 is a motor shaft. , 19 is a motor shaft side gear, 20 is an output shaft, and 21 is an output shaft side gear.

Claims

モータ軸側ギアと出力軸側ギアとが噛み合っている状態で前記モータ軸側ギアが回転することに連動して前記出力軸側ギアが回転することで前記出力軸が駆動可能に構成されてなる駆動軸の動作を制御するロボット制御装置において、
前記モータ軸のねじれ、前記モータ軸と前記モータ軸側ギアとの接続箇所のねじれ、前記モータ軸側ギアの変形、前記出力軸側ギアの変形及び前記出力軸側ギアと前記出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する最大弾性変形量角度記憶手段と、
前記モータ軸の回転角度を検出するモータ軸側エンコーダが電源投入時に示す角度を弾性変形量ゼロ角度とし、前記モータ軸側エンコーダが検出した角度であるモータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分を弾性変形量として計算し、前記モータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えているか否かを判定し、前記モータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えていない場合に前記モータ軸側エンコーダ角度を弾性変形中の角度として判定し、一方、前記モータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えている場合に前記モータ軸側エンコーダ角度を動作開始時の角度として判定する弾性変形量計算手段と、
前記弾性変形量ゼロ角度と前記モータ軸側エンコーダ角度との差分が示す正負を判定して前記モータ軸側ギアの弾性変形方向を検出する弾性変形方向検出手段と、
前記モータ軸側ギアに摩擦補償用トルクを印加するモータ制御手段とを備え、
前記モータ制御手段は、弾性変形量を更新する弾性変形更新処理を実行することに続いて摩擦補償用トルクを決定する摩擦補償用トルク決定処理を実行する処理を周期的に行い、前記弾性変形更新処理においては、前記弾性変形量計算手段により計算された弾性変形量と正方向の最大弾性変形量及び負方向の最大弾性変形量とを比較し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていなく且つ負方向の最大弾性変形量を超えていないと判定したときには弾性変形中であると判定して現在の弾性変形量を維持し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその正方向の最大弾性変形量を弾性変形量として設定し、前記弾性変形量計算手段により計算された弾性変形量が負方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその負方向の最大弾性変形量を弾性変形量として設定し、前記摩擦補償用トルク決定処理においては、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とを比較し、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが逆方向であると判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に減少させ、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが同方向であると判定したときには弾性変形量が最大弾性変形量であるか否かを判定し、弾性変形量が最大弾性変形量でないと判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に増加させ、弾性変形量が最大弾性変形量であると判定したときには予め設定されている最大補償トルクを摩擦補償用トルクとして設定することを特徴とするロボット制御装置。 The output shaft is configured to be driven by the output shaft side gear rotating in conjunction with the rotation of the motor shaft side gear while the motor shaft side gear and the output shaft side gear are engaged with each other. In the robot control device that controls the operation of the drive shaft,
Twisting of the motor shaft, twisting of a connection portion between the motor shaft and the motor shaft side gear, deformation of the motor shaft side gear, deformation of the output shaft side gear, and connection between the output shaft side gear and the output shaft A maximum elastic deformation amount angle storage means for storing, as an elastic deformation, a deformation caused by a twist of a portion, and storing a maximum value of an elastic deformation amount as a total value thereof as a maximum elastic deformation amount angle;
The angle that the motor shaft side encoder that detects the rotation angle of the motor shaft shows when the power is turned on is defined as the zero elastic deformation amount angle, and the motor shaft side encoder angle that is detected by the motor shaft side encoder and the zero elastic deformation amount angle The difference between the motor shaft side encoder angle and the zero elastic deformation amount angle is determined as to whether or not the difference between the motor shaft side encoder angle and the elastic deformation zero angle exceeds the maximum elastic deformation amount angle. And the zero elastic deformation amount angle do not exceed the maximum elastic deformation amount angle, the motor shaft side encoder angle is determined as an elastic deformation angle, while the motor shaft side encoder angle and the elastic deformation amount are determined. When the difference from the zero deformation amount angle exceeds the maximum elastic deformation amount angle, the motor shaft side encoder angle is determined as the angle at the start of operation. And the elastic deformation amount calculation means that,
Elastic deformation direction detecting means for detecting the elastic deformation direction of the motor shaft side gear by determining whether the difference between the elastic deformation amount zero angle and the motor shaft side encoder angle is positive or negative;
Motor control means for applying a friction compensation torque to the motor shaft side gear,
The motor control means periodically performs a process of executing a friction compensation torque determination process of determining a friction compensation torque subsequent to executing an elastic deformation update process of updating an elastic deformation amount, and the elastic deformation update. In the processing, the elastic deformation amount calculated by the elastic deformation amount calculating means is compared with the maximum elastic deformation amount in the positive direction and the maximum elastic deformation amount in the negative direction, and the elastic deformation calculated by the elastic deformation amount calculating means. When it is determined that the amount does not exceed the maximum elastic deformation amount in the positive direction and does not exceed the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is in progress, and the current elastic deformation amount is maintained. When it is determined that the amount of elastic deformation calculated by the amount calculation means exceeds the maximum amount of elastic deformation in the positive direction, it is determined that elastic deformation is not in progress, and the maximum amount of elastic deformation in the positive direction is determined as the amount of elastic deformation. And when it is determined that the elastic deformation amount calculated by the elastic deformation amount calculating means exceeds the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is not in progress and the maximum elastic deformation amount in the negative direction is determined. The elastic deformation amount is set, and in the friction compensation torque determination process, the target direction is compared with the elastic deformation direction detected by the elastic deformation direction detection means, and the target direction and the elastic deformation direction detection means are detected. When it is determined that the elastic deformation direction is the reverse direction, the friction compensation torque is decreased continuously or stepwise for each sampling time unit, and the target direction and the elastic deformation direction detected by the elastic deformation direction detecting means are Are determined to be in the same direction, it is determined whether the amount of elastic deformation is the maximum amount of elastic deformation, and when it is determined that the amount of elastic deformation is not the maximum amount of elastic deformation The friction compensation torque is increased continuously or stepwise for each sampling time unit, and when it is determined that the elastic deformation amount is the maximum elastic deformation amount, the preset maximum compensation torque is set as the friction compensation torque. A robot controller characterized by the above.

モータ軸側ギアと出力軸側ギアとが噛み合っている状態で前記モータ軸側ギアが回転することに連動して前記出力軸側ギアが回転することで前記出力軸が駆動可能に構成されてなる駆動軸の動作を制御するロボット制御装置において、
前記モータ軸のねじれ、前記モータ軸と前記モータ軸側ギアとの接続箇所のねじれ、前記モータ軸側ギアの変形、前記出力軸側ギアの変形及び前記出力軸側ギアと前記出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する最大弾性変形量角度記憶手段と、
前記モータ軸の回転角度を検出するモータ軸側エンコーダが検出した角度であるモータ軸側エンコーダ角度と前記出力軸の回転角度を検出する出力軸側エンコーダが検出した角度である出力軸側エンコーダ角度との差分を弾性変形量として計算し、前記モータ軸側エンコーダ角度と前記出力軸側エンコーダ角度との差分が前記最大弾性変形量角度を超えているか否かを判定し、前記モータ軸側エンコーダ角度と前記出力軸側エンコーダ角度との差分が前記最大弾性変形量角度を超えていない場合に前記モータ軸側エンコーダ角度を弾性変形中の角度として判定し、一方、前記モータ軸側エンコーダ角度と前記出力軸側エンコーダ角度との差分が前記最大弾性変形量角度を超えている場合に前記モータ軸側エンコーダ角度を動作開始時の角度として判定する弾性変形量計算手段と、
前記モータ軸側エンコーダ角度と前記出力軸側エンコーダ角度との差分が示す正負を判定して前記モータ軸側ギアの弾性変形方向を検出する弾性変形方向検出手段と、
前記モータ軸側ギアに摩擦補償用トルクを印加するモータ制御手段とを備え、
前記モータ制御手段は、弾性変形量を更新する弾性変形更新処理を実行することに続いて摩擦補償用トルクを決定する摩擦補償用トルク決定処理を実行する処理を周期的に行い、前記弾性変形更新処理においては、前記弾性変形量計算手段により計算された弾性変形量と正方向の最大弾性変形量及び負方向の最大弾性変形量とを比較し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていなく且つ負方向の最大弾性変形量を超えていないと判定したときには弾性変形中であると判定して現在の弾性変形量を維持し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその正方向の最大弾性変形量を弾性変形量として設定し、前記弾性変形量計算手段により計算された弾性変形量が負方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその負方向の最大弾性変形量を弾性変形量として設定し、前記摩擦補償用トルク決定処理においては、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とを比較し、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが逆方向であると判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に減少させ、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが同方向であると判定したときには弾性変形量が最大弾性変形量であるか否かを判定し、弾性変形量が最大弾性変形量でないと判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に増加させ、弾性変形量が最大弾性変形量であると判定したときには予め設定されている最大補償トルクを摩擦補償用トルクとして設定することを特徴とするロボット制御装置。
The output shaft is configured to be driven by the output shaft side gear rotating in conjunction with the rotation of the motor shaft side gear while the motor shaft side gear and the output shaft side gear are engaged with each other. In the robot control device that controls the operation of the drive shaft,
Twisting of the motor shaft, twisting of a connection portion between the motor shaft and the motor shaft side gear, deformation of the motor shaft side gear, deformation of the output shaft side gear, and connection between the output shaft side gear and the output shaft A maximum elastic deformation amount angle storage means for storing, as an elastic deformation, a deformation caused by a twist of a portion, and storing a maximum value of an elastic deformation amount as a total value thereof as a maximum elastic deformation amount angle;
A motor shaft side encoder angle that is an angle detected by a motor shaft side encoder that detects a rotation angle of the motor shaft, and an output shaft side encoder angle that is an angle detected by an output shaft side encoder that detects the rotation angle of the output shaft; The difference between the motor shaft side encoder angle and the output shaft side encoder angle is determined as to whether or not the difference between the motor shaft side encoder angle and the output shaft side encoder angle exceeds the maximum elastic deformation amount angle. When the difference from the output shaft side encoder angle does not exceed the maximum elastic deformation amount angle, the motor shaft side encoder angle is determined as an angle during elastic deformation, while the motor shaft side encoder angle and the output shaft When the difference from the side encoder angle exceeds the maximum elastic deformation amount angle, the motor shaft side encoder angle is And the elastic deformation amount calculating means for determining Te,
Elastic deformation direction detecting means for detecting the elastic deformation direction of the motor shaft side gear by determining the positive and negative indicated by the difference between the motor shaft side encoder angle and the output shaft side encoder angle;
Motor control means for applying a friction compensation torque to the motor shaft side gear,
The motor control means periodically performs a process of executing a friction compensation torque determination process of determining a friction compensation torque subsequent to executing an elastic deformation update process of updating an elastic deformation amount, and the elastic deformation update. In the processing, the elastic deformation amount calculated by the elastic deformation amount calculating means is compared with the maximum elastic deformation amount in the positive direction and the maximum elastic deformation amount in the negative direction, and the elastic deformation calculated by the elastic deformation amount calculating means. When it is determined that the amount does not exceed the maximum elastic deformation amount in the positive direction and does not exceed the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is in progress, and the current elastic deformation amount is maintained. When it is determined that the amount of elastic deformation calculated by the amount calculation means exceeds the maximum amount of elastic deformation in the positive direction, it is determined that elastic deformation is not in progress, and the maximum amount of elastic deformation in the positive direction is determined as the amount of elastic deformation. And when it is determined that the elastic deformation amount calculated by the elastic deformation amount calculating means exceeds the maximum elastic deformation amount in the negative direction, it is determined that the elastic deformation is not in progress and the maximum elastic deformation amount in the negative direction is determined. The elastic deformation amount is set, and in the friction compensation torque determination process, the target direction is compared with the elastic deformation direction detected by the elastic deformation direction detection means, and the target direction and the elastic deformation direction detection means are detected. When it is determined that the elastic deformation direction is the reverse direction, the friction compensation torque is decreased continuously or stepwise for each sampling time unit, and the target direction and the elastic deformation direction detected by the elastic deformation direction detecting means are Are determined to be in the same direction, it is determined whether the amount of elastic deformation is the maximum amount of elastic deformation, and when it is determined that the amount of elastic deformation is not the maximum amount of elastic deformation The friction compensation torque is increased continuously or stepwise for each sampling time unit, and when it is determined that the elastic deformation amount is the maximum elastic deformation amount, the preset maximum compensation torque is set as the friction compensation torque. A robot controller characterized by the above.

請求項１又は２に記載したロボット制御装置において、
前記モータ制御手段は、前記弾性変形更新処理及び前記摩擦補償用トルク決定処理を実行することに続いて停止時のチャタリングを防止する停止時チャタリング防止処理を実行する処理を周期的に行い、前記停止時チャタリング防止処理においては、ロボットの動作停止命令を判定したときには、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算することを特徴とするロボット制御装置。
ロボットの動作開始時の積分補償の状態量
＝ロボットの動作開始直前の積分補償の状態量−（ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量） In the robot control device according to claim 1 or 2,
The motor control means periodically performs a process of executing an anti-chattering prevention process for preventing chattering at the time of stopping following the execution of the elastic deformation update process and the friction compensation torque determination process, and the stop In the chattering prevention process, when the robot operation stop command is determined, the friction compensation torque immediately before the robot operation is stopped is maintained, and when the robot operation start command is determined, the disturbance force is compensated. A robot controller which calculates a state quantity of integral compensation at the start of robot operation according to the following formula.
Integral compensation state quantity at the start of robot operation = Integral compensation state quantity immediately before the robot operation starts-(Friction compensation state quantity at the start of robot operation-Friction compensation state quantity immediately before the robot operation starts)

請求項１又は２に記載したロボット制御装置において、
前記モータ制御手段は、前記弾性変形更新処理及び前記摩擦補償用トルク決定処理を実行することに続いて停止時の摩擦補償を行う停止時摩擦補償処理を実行する処理を周期的に行い、前記停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、目標角度と前記モータ軸側エンコーダ角度との差分の絶対値が前記最大弾性変形量角度よりも大きな角度である設定角度を超えているか否かを判定し、前記目標角度と前記モータ軸側エンコーダ角度との差分の絶対値が前記設定角度を超えている場合に、弾性変形量を考慮した摩擦補償を行うことを特徴とするロボット制御装置。 In the robot control device according to claim 1 or 2,
The motor control means periodically performs a process of executing a stop-time friction compensation process for performing a stop-time friction compensation process following the execution of the elastic deformation update process and the friction compensation torque determination process, and the stop In the friction compensation process, the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds a set angle that is larger than the maximum elastic deformation amount angle when a robot operation stop command is determined. Friction compensation is performed in consideration of the amount of elastic deformation when the absolute value of the difference between the target angle and the motor shaft side encoder angle exceeds the set angle. Robot control device.

請求項４に記載したロボット制御装置において、
前記モータ制御手段は、前記停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、前記目標角度と前記モータ軸側エンコーダ角度との差分の絶対値が前記設定角度を超えていない場合に、弾性変形量を考慮した摩擦補償を行わないと共に、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算することを特徴とするロボット制御装置。
ロボットの動作開始時の積分補償の状態量
＝ロボットの動作開始直前の積分補償の状態量−（ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量） The robot control device according to claim 4, wherein
In the stop friction compensation process, the motor control means determines that the absolute value of the difference between the target angle and the motor shaft side encoder angle does not exceed the set angle when determining a robot operation stop command. In this case, the robot does not perform the friction compensation considering the elastic deformation amount, maintains the friction compensation torque immediately before the operation of the robot is stopped, and compensates for the disturbance force when the robot operation start command is determined. A robot controller characterized by calculating a state quantity of integral compensation at the start of operation according to the following formula.
Integral compensation state quantity at the start of robot operation = Integral compensation state quantity immediately before the robot operation starts-(Friction compensation state quantity at the start of robot operation-Friction compensation state quantity immediately before the robot operation starts)