JP2010240827A - Robot control device - Google Patents
Robot control device Download PDFInfo
- Publication number
- JP2010240827A JP2010240827A JP2009282813A JP2009282813A JP2010240827A JP 2010240827 A JP2010240827 A JP 2010240827A JP 2009282813 A JP2009282813 A JP 2009282813A JP 2009282813 A JP2009282813 A JP 2009282813A JP 2010240827 A JP2010240827 A JP 2010240827A
- Authority
- JP
- Japan
- Prior art keywords
- elastic deformation
- angle
- shaft side
- amount
- deformation amount
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000005489 elastic deformation Effects 0.000 claims abstract description 343
- 238000000034 method Methods 0.000 claims description 108
- 230000008569 process Effects 0.000 claims description 101
- 244000145845 chattering Species 0.000 claims description 19
- 230000002265 prevention Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 10
- 230000003247 decreasing effect Effects 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 7
- 230000002441 reversible effect Effects 0.000 claims description 3
- 230000003068 static effect Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 11
- 210000000707 wrist Anatomy 0.000 description 9
- 230000001133 acceleration Effects 0.000 description 8
- 230000008859 change Effects 0.000 description 5
- 230000010354 integration Effects 0.000 description 5
- 230000007704 transition Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000002829 reductive effect Effects 0.000 description 3
- 230000002411 adverse Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Abstract
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.
モータ軸側ギアが回転することに連動して出力軸側ギアが回転することで出力軸が駆動可能に構成されてなる駆動軸を有するロボットの動作軌跡の精度を高める方法として、送り系のバックラッシに起因するロストモーション量、ギアの弾性変形に起因するロストモーション量及び静摩擦に起因するロストモーション量を予め計算しておき、軸の移動方向が反転する場合に、モータのギアをバックラッシ加速度で送り系のバックラッシに起因するロストモーション量だけ駆動させ、続いて弾性変形加速度でギアの弾性変形に起因するロストモーション量だけ駆動させ、続いて静摩擦加速度で静摩擦に起因するロストモーション量だけ駆動させる方法がある(例えば特許文献1参照)。 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).
特許文献1に開示されている方法では理論上ではロボットの動作軌跡の精度を高めることが可能であるが、しかしながら、具体的にどのような動作制御で摩擦補償を行えば良いのかについては開示されておらず、ロボットの動作軌跡の精度を高めることができるという点で不確かな点が多い。 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
請求項1に記載した発明によれば、最大弾性変形量角度記憶手段は、モータ軸のねじれ、モータ軸とモータ軸側ギアとの接続箇所のねじれ、モータ軸側ギアの変形、出力軸側ギアの変形及び出力軸側ギアと出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する。弾性変形量計算手段は、モータ軸の回転角度を検出するモータ軸側エンコーダが電源投入時に示す角度を弾性変形量ゼロ角度とし、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を超えているか否かを判定し、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を越えていなければ、モータ軸側エンコーダ角度を弾性変形中の角度として判定して弾性変形量を計算し、一方、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分が最大弾性変形量角度を越えていれば、モータ軸側エンコーダ角度を動作開始時の角度として判定する。弾性変形方向検出手段は、弾性変形量ゼロ角度とモータ軸側エンコーダ角度との差分が示す正負を判定してモータ軸側ギアの弾性変形方向を検出する。 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.
以下、上記した請求項1に記載した発明の技術的特徴について、図17乃至図19を参照して説明する。上記した請求項1に記載した発明は、モータ軸側ギアに印加する目標駆動方向への摩擦補償用トルクを徐々に増加させる点及び摩擦補償用トルクと加速トルクとを切分けて制御している点を特徴としている。 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.
理論上、弾性変形を考慮することなく摩擦補償だけを考慮してロボットの動作軌跡の精度を高めるようとする場合には、図17に示すように制御すれば良い。即ち、最大静止摩擦を超える摩擦補償用トルクを一気に(急激に)印加したとしても、ロボットの動作軌跡は線形となるので、静止摩擦分のトルクを加速トルクに加算すれば良い。 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.
しかしながら、摩擦補償だけでなく弾性変形をも考慮してロボットの動作軌跡の精度を高めようとする場合、上記した図17に示す技術思想にしたがって最大静止摩擦を超える摩擦補償用トルクを一気に印加すると、ロボットの動作軌跡は理論上では上記した図17に示したものと同様に線形となるが、現実的には制御対象となる物体は理論のように絶対的な硬度を持たず弾性変形する性質であるので、図18に示すように、弾性変形分だけの位置移動が先行し、物体の動作開始時に弾性変形分が戻ることで逆方向への位置移動が発生し、その後に加速トルクにより物体の位置移動が開始するという現象が発生する。即ち、物体の形状の揺り戻しによる動作軌跡のぐらつきが発生し、理論にしたがった動作軌跡とはならず、動作軌跡の精度を高めることは困難である。これは上記した特許文献1の技術に相当する。 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.
このような事情から、請求項1に記載した発明では、図19に示すように、摩擦補償用トルクと加速トルクとを切分けて制御し、摩擦補償用トルクを徐々に増加させることで、一旦弾性変形した物体を弾性変形させたままで加速トルクにより位置移動させることが可能となり、動作軌跡の精度を高めることが可能となる。 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.
尚、以上に説明した上記した請求項1に記載した発明の技術的特徴は後述する請求項2に記載した発明の技術的特徴と同様であり、請求項2に記載した発明は請求項1に記載した発明の技術的特徴と同じ理論に基づく発明である。
The technical features of the invention described in claim 1 described above are the same as the technical features of the invention described in
請求項2に記載した発明によれば、最大弾性変形量角度記憶手段は、モータ軸のねじれ、モータ軸とモータ軸側ギアとの接続箇所のねじれ、モータ軸側ギアの変形、出力軸側ギアの変形及び出力軸側ギアと出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する。弾性変形量計算手段は、モータ軸の回転角度を検出するモータ軸側エンコーダ角度と出力軸の回転角度を検出する出力軸側エンコーダ角度との差分が最大弾性変形量角度を超えているか否かを判定し、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が最大弾性変形量角度を越えていなければ、モータ軸側エンコーダ角度を弾性変形中の角度として判定して弾性変形量を計算し、一方、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が最大弾性変形量角度を越えていれば、モータ軸側エンコーダ角度を動作開始時の角度として判定する。弾性変形方向検出手段は、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分が示す正負を判定してモータ軸側ギアの弾性変形方向を検出する。 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.
モータ制御手段は、弾性変形量を更新する弾性変形更新処理を実行することに続いて摩擦補償用トルクを決定する摩擦補償用トルク決定処理を実行する処理を周期的に行う。弾性変形更新処理においては、弾性変形量計算手段により計算された弾性変形量と正方向の最大弾性変形量及び負方向の最大弾性変形量とを比較し、弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていなく且つ負方向の最大弾性変形量を超えていないと判定したときには弾性変形中であると判定して現在の弾性変形量を維持し、弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその正方向の最大弾性変形量を弾性変形量として設定し、弾性変形量計算手段により計算された弾性変形量が負方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその負方向の最大弾性変形量を弾性変形量として設定する。 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.
これにより、上記した請求項1に記載したものと弾性変形量を計算する手順が異なるだけであり、上記した請求項1に記載したもの同様の作用効果を得ることができる。 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.
請求項3に記載した発明によれば、モータ制御手段は、弾性変形更新処理及び摩擦補償用トルク決定処理を実行することに続いて停止時のチャタリングを防止する停止時チャタリング防止処理を実行する処理を周期的に行い、停止時チャタリング防止処理においては、ロボットの動作停止命令を判定したときには、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算する。 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.
請求項4に記載した発明によれば、モータ制御手段は、弾性変形更新処理及び摩擦補償用トルク決定処理を実行することに続いて停止時の摩擦補償を行う停止時摩擦補償処理を実行する処理を周期的に行い、停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、目標角度とモータ軸側エンコーダ角度との差分の絶対値が最大弾性変形量角度よりも大きな角度である設定角度を超えているか否かを判定し、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えている場合に、弾性変形量を考慮した摩擦補償を行う。 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.
請求項5に記載した発明によれば、モータ制御手段は、停止時摩擦補償処理においては、ロボットの動作停止命令を判定したときに、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていない場合に、弾性変形量を考慮した摩擦補償を行わないと共に、ロボットの動作が停止される直前の摩擦補償用トルクを維持し、ロボットの動作開始命令を判定したときには、外乱力に対する補償を行うロボットの動作開始時の積分補償の状態量を次に示す計算式にしたがって計算する。 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.
ロボットの動作開始時の積分補償の状態量
=ロボットの動作開始直前の積分補償の状態量−(ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量)
これにより、上記した請求項4に記載した弾性変形量を考慮した摩擦補償を行うことで、目標角度とモータ軸側エンコーダ角度との差分が最大弾性変形量角度よりも大きな角度である設定角度を超えなくなった後に(ある程度まで目標角度に収束した後に)、上記した請求項3に記載したものと同様の作用効果を得ることができる。即ち、オーバーシュートを解消するまでにかかる時間を短縮することができ、しかも、ロボットの動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。
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
(第1の実施形態)
以下、本発明の第1の実施形態について、図1乃至図19を参照して説明する。図2は、ロボット制御システムの全体構成を示している。ロボット制御システム1は、例えば6軸の垂直多関節型ロボットからなるロボット2と、ロボット2の動作を制御するロボット制御装置3と、作業者が操作するティーチングペンダンド4とを備えて構成されている。
(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
ロボット2は、ベース5と、ベース5に水平方向に旋回可能に支持されているショルダ部6と、ショルダ部6に上下方向に旋回可能に支持されている下アーム7と、下アーム7に上下方向に旋回可能に支持されている第1の上アーム8と、第1の上アーム8の先端部に捻り回転可能に支持されている第2の上アーム9と、第2の上アーム9に上下方向に回転可能に支持されている手首10と、手首10に回転(捻り動作)可能に支持されているフランジ11とを備えて構成されている。
The
上記したベース5を含め、ショルダ部6、下アーム7、第1の上アーム8、第2の上アーム9、手首10及びフランジ11は、ロボット2におけるリンクとして機能し、ベース5を除く各リンクは前段のリンクに対して回転関節により回転可能に連結されている。最先端のリンクであるフランジ11にはワークを把持するためのハンド(図示せず)が取付け可能になっている。
The
尚、本実施形態では、第1のリンクであるベース5と第2のリンクであるショルダ部6との間を連結する回転関節の関節軸を1軸、第2のリンクであるショルダ部6と第3のリンクである下アーム7との間を連結する回転関節の関節軸を2軸、第3のリンクである下アーム7と第4のリンクである第1の上アーム8との間を連結する回転関節の関節軸を3軸、第4のリンクである第1の上アーム8と第5のリンクである第2の上アーム9との間を連結する回転関節の関節軸を4軸、第5のリンクである第2の上アーム9と第6のリンクである手首10との間を連結する回転関節の関節軸を5軸、第6のリンクである手首10と第7のリンクであるフランジ11との間を連結する回転関節の関節軸を6軸として図示している。
In the present embodiment, the joint axis of the rotary joint that connects between the
図1は、ロボット制御装置3及びロボット2の電気的な構成を機能ブロック図により示している。ロボット制御装置3は、マイクロコンピュータからなるCPU12(本発明でいう最大弾性変形量角度記憶手段、弾性変形量計算手段、弾性変形方向検出手段、モータ制御手段)と、CPU12からの動作指令に基づいて駆動信号をロボット2に出力する駆動回路13と、ロボット2の動作プログラムなどを記憶するRAM14と、ロボット2のシステムプログラムや動作プログラムを作成するためのロボット言語などを記憶するROM15とを備えて構成されている。ロボット2は、ショルダ部6、各アーム7〜9、手首10及びフランジ11を駆動させるモータ16と、モータ16の回転角度を検出するエンコーダ17とを備えている。尚、これらモータ16及びエンコーダ17はリンク毎に設けられている。
FIG. 1 is a functional block diagram showing the electrical configuration of the
CPU12は、リンク毎のエンコーダ17から入力したエンコーダ角度(エンコーダ値)に基づいてベース5に対するショルダ部6の回転角度、ショルダ部6に対する下アーム7の回転角度、下アーム7に対する第1の上アーム8の回転角度、第1の上アーム8に対する第2の上アーム9の回転角度、第2の上アーム9に対する手首10の回転角度、手首10に対するフランジ11の回転角度を検出し、ロボット2の位置(姿勢)を検出すると共に、エンコーダ角度をフィードバック制御して駆動回路13から駆動信号を出力させ、それらショルダ部6、各アーム7〜9、手首10及びフランジ11の動作を制御する。
The
ロボット2における各軸の駆動部分は、図3に示すように、モータ16のモータ軸18に取付けられているモータ軸側ハーモニックギア(以下、モータ軸側ギアと称する)19と出力軸20に取付けられている出力軸側ハーモニックギア(以下、出力軸側ギアと称する)21とが噛み合っており、ロボット制御装置3からモータ16に駆動信号が出力されてトルク指令値が与えられることでモータ16がモータ軸側ギア19を回転させ、モータ軸側ギア19が回転されることに追従して出力軸側ギア21が回転されるように構成されている。尚、これ以降、モータ軸18の回転角度を検出するエンコーダ17が出力するエンコーダ角度をモータ軸側エンコーダ角度として定義し、出力軸20の回転角度を検出するエンコーダが出力するエンコーダ角度を出力軸側エンコーダ角度として定義する。
As shown in FIG. 3, the drive portion of each axis in the
上記した構成では、モータ軸18のねじれ、モータ軸18とモータ軸側ギア19との接続箇所のねじれ、モータ軸側ギア19の変形、出力軸側ギア21の変形及び出力軸側ギア21と出力軸20との接続箇所のねじれに起因する弾性変形が発生する。このような事情から、本実施形態ではCPU12は以下に示す制御を行う。以下、上記した構成の作用として、CPU12が本発明に関連して行う制御を図4乃至図16を参照して説明する。ロボット制御装置3において、CPU12は、RAM14に記憶されている動作プログラムを実行して所定周期(サンプリング単位周期)で以下に示すメイン処理を実行する。
In the configuration described above, the
CPU12は、実行中にある動作プログラムから目標角度指令を取得すると(ステップS1)、その目標角度指令により指示されている目標角度(目標値)とエンコーダ17から入力しているモータ軸側エンコーダ角度とに基づいて指令速度を計算し(ステップS2)、計算した指令速度とエンコーダ速度とに基づいてトルク指令値を計算する(ステップS3)。そして、CPU12は、現在のモータ軸側エンコーダ角度と現在の出力軸側エンコーダ角度とに基づいて弾性変形量を更新する弾性変形量更新処理に移行する(ステップS4)。
When the
CPU12は、メイン処理から弾性変形量更新処理に移行すると、現在の出力軸側エンコーダ角度を制御視点角度とし、制御視点角度から現在のモータ軸側エンコーダ角度を減算した値を弾性変形量として計算し(ステップS11)、その計算した弾性変形量と予め記憶している正方向の最大弾性変形量及び負方向の弾性変形量との大小関係を比較判定する(ステップS12、S13)。ここで、CPU12は、計算した弾性変形量が正方向の最大弾性変形量を超えていなく(同等又は小さく)且つ負方向の最大弾性変形量を超えていない(同等又は大きい)と判定すると(ステップS12にて「NO」、S13にて「NO」)、モータ軸側ギア19が弾性変形中であると判定し、現在の弾性変形量を維持し(ステップS14)、弾性変形量更新処理を終了してメイン処理に戻り、弾性変形量から摩擦補償用トルクを決定する摩擦補償用トルク決定処理に移行する(ステップS5)。
When the
一方、CPU12は、計算した弾性変形量が正方向の最大弾性変形量を超えていると判定すると(ステップS12にて「YES」)、モータ軸側ギア19が弾性変形中でないと判定し、その正方向の最大弾性変形量を弾性変形量として設定し(ステップS15)、正方向の最大弾性変形量と現在のモータ軸側エンコーダ角度とを加算した値を制御視点角度として計算し(ステップS16)、弾性変形量更新処理を終了してメイン処理に戻り、摩擦補償用トルク決定処理に移行する(ステップ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
又、CPU12は、計算した弾性変形量が負方向の最大弾性変形量を超えていると判定すると(ステップS13にて「YES」)、モータ軸側ギア19が弾性変形中でないと判定し、その負方向の最大弾性変形量を弾性変形量として設定し(ステップS17)、負方向の最大弾性変形量と現在のモータ軸側エンコーダ角度とを加算した値を制御視点角度として計算し(ステップS18)、弾性変形量更新処理を終了してメイン処理に戻り、摩擦補償用トルク決定処理に移行する(ステップS5)。
If the
次いで、CPU12は、メイン処理から摩擦補償用トルク決定処理に移行すると、目標方向と弾性変形方向とを比較し、目標方向と弾性変形方向とが逆方向であるか否かを判定する(ステップS21)。ここで、CPU12は、目標方向と弾性変形方向とが逆方向であると判定すると(ステップS21にて「YES」)、摩擦補償用トルクを減少させ(ステップS22)、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時のチャタリングを防止する停止時チャタリング防止処理に移行する(ステップS6)。
Next, when shifting from the main process to the friction compensation torque determination process, the
又、CPU12は、目標方向と弾性変形方向とが逆方向でないと判定すると(ステップS21にて「NO」)、弾性変形量が最大弾性変形量であるか否かを判定し(ステップS23)、弾性変形量が最大弾性変形量でないと判定すると(ステップS23にて「NO」)、摩擦補償用トルクを増加させ(ステップS24)、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時チャタリング防止処理に移行する(ステップS6)。一方、CPU12は、弾性変形量が最大弾性変形量であると判定すると(ステップS23にて「YES」)、予め設定されている最大補償トルクを摩擦補償用トルクとして設定し(ステップS25)、摩擦補償用トルク決定処理を終了してメイン処理に戻り、停止時チャタリング防止処理に移行する(ステップS6)。
If the
次いで、CPU12は、メイン処理から停止時チャタリング防止処理に移行すると、計算した指令速度がゼロであるか否か(ロボット2の動作停止命令であるか否か)を判定する(ステップS31)。CPU12は、指令速度がゼロでないと判定すると(ステップS31にて「NO」)、指令速度ゼロフラグをオンに設定しているか否かを判定し(ステップS32)、指令速度ゼロフラグをオンに設定していないと判定すると(ステップS32にて「NO」)、現在の摩擦補償用トルクを保存補償トルクとして設定し(ステップS33)、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、CPU12は、摩擦補償用トルクをトルク指令値に加算し(ステップS7)、メイン処理を終了する。
Next, when the
ここで、CPU12は、指令速度がゼロであると判定すると(ステップS31にて「YES」)、指令速度ゼロフラグをオフに設定しているか否かを判定し(ステップS34)、指令速度ゼロフラグをオフに設定していると判定すると(ステップS34にて「YES」)、指令速度ゼロフラグをオンに設定し(ステップS35)、現在の保存補償トルクを摩擦補償用トルクとして設定し(ステップS36)、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、CPU12は、摩擦補償用トルクをトルク指令値に加算し(ステップS7)、メイン処理を終了する。即ち、CPU12は、ロボット2の動作停止時には、ロボット2の動作を停止させる直前の摩擦補償用トルクを維持する。
If the
次いで、CPU12は、指令速度がゼロでないと判定し(ステップS31にて「NO」)、指令速度ゼロフラグをオンに設定していると判定すると(ステップS32にて「YES」)、指令速度ゼロフラグをオフに設定し(ステップS37)、外乱力に対する補償を行う積分補償の状態量を次に示す計算式にしたがって計算する(ステップS38)。
Next, when the
積分補償の状態量←積分補償の状態量−(保存補償トルク−摩擦補償用トルク)/積分補償ゲイン
そして、CPU12は、現在の摩擦補償用トルクを保存補償トルクとして設定し(ステップS33)、停止時チャタリング防止処理を終了してメイン処理に戻る。そして、CPU12は、摩擦補償用トルクをトルク指令値に加算し(ステップS7)、メイン処理を終了する。
Integral compensation state quantity ← integral compensation state quantity-(storage compensation torque-friction compensation torque) / integration compensation gain Then, the
尚、上記した処理では、現在の出力軸側エンコーダ角度を制御視点角度とし、モータ軸側エンコーダ角度と出力軸側エンコーダ角度との差分に基づいて弾性変形量を計算する方法を説明したが、エンコーダ17が電源投入時に示す角度を弾性変形量ゼロ角度とし、モータ軸側エンコーダ角度と弾性変形量ゼロ角度との差分に基づいて弾性変形量を計算するようにしても良い。 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乃至図10は、モータ軸側ギア19が動作開始時に弾性変形する態様及び摩擦補償用トルクの推移を示しており、図11及び図12は、モータ軸側ギア19が動作停止時に弾性変形する態様及び摩擦補償用トルクの推移を示している。尚、図8乃至図12中では、「a1」は制御視点角度を示しており、「a2」はモータ軸側エンコーダ角度を示している。
8 to 10 show how the motor
図8は、モータ軸側ギア19の弾性変形量がゼロである状態から出力軸20を目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクがゼロの状態から摩擦補償用トルクをモータ軸側ギア19の弾性変形量が最大弾性変形量に達するまで正比例的に増加させ、モータ軸側ギア19の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えない領域では弾性変形し、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して出力軸20が動作開始する。
FIG. 8 shows a case where the
図9は、モータ軸側ギア19の弾性変形量がゼロでない状態から出力軸20を目標駆動方向に駆動させる場合であり、出力軸を弾性変形方向と同一方向である目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクが所定値の状態から摩擦補償用トルクをモータ軸側ギア19の弾性変形量が最大弾性変形量に達するまで正比例的に増加させ、モータ軸側ギア19の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して最初の弾性変形方向と同一方向に出力軸20が動作開始する。
FIG. 9 shows a case where the
図10は、モータ軸側ギア19の弾性変形量がゼロでない状態から出力軸20を目標駆動方向に駆動させる場合であり、出力軸を弾性変形方向と反対方向である目標駆動方向に駆動させる場合を示しており、この場合は、摩擦補償用トルクが所定値の状態から摩擦補償用トルクをモータ軸側ギア19の弾性変形量が最大弾性変形量に達するまで正比例的に減少させ、モータ軸側ギア19の弾性変形量が最大弾性変形量に達した以後は一定値とする。即ち、摩擦補償用トルクが最大補償トルクを超えた領域では最大弾性変形量まで変形して最初の弾性変形方向と反対方向に出力軸20が動作開始する。
FIG. 10 shows a case where the
図11は、出力軸側ギア21が目標位置を通過することなく停止した場合を示しており、この場合は、出力軸側ギア21が目標位置で停止した状態の摩擦補償用トルクを維持する。図12は、出力軸側ギア21が目標位置を通過して停止した場合を示しており、この場合は、出力軸側ギア21を目標位置まで戻すために摩擦補償用トルクを減少させ、出力軸側ギア21が目標位置で停止した状態の摩擦補償用トルクを維持する。
FIG. 11 shows a case where the output
ところで、上記したように摩擦補償を実施した場合では、理論上では振動することなくロボット2が動作停止すると考えられるか、実際には僅かな補償誤差や外乱力(主には重力)に起因して振動が発生する。そのため、ロボット2の動作停止時には摩擦補償を停止する必要があるが、単純に摩擦補償をオンオフする制御では不連続な部分が発生して新たな振動が発生する要因となる。即ち、図13に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況から摩擦が変化すると、摩擦が変化したことに追従して摩擦補償が変化することで、両者の均衡が継続して保たれることになる。
By the way, when friction compensation is performed as described above, the
一方、図14に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況で摩擦補償をオフすると、摩擦補償をオフした分だけ両者の均衡が崩れる。そして、両者の均衡が崩れると、オフした分の摩擦補償を時間が経過するにしたがって積分補償が補うことになり、その結果、両者の均衡が保たれるようになる。しかしながら、オフした分の摩擦補償を積分補償が補ったままの状況でロボット2の動作開始時(動作開始直後)に摩擦補償をオンすると、摩擦補償をオンした分だけ両者の均衡が崩れることになる。 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.
そこで、本実施形態では、図15に示すように、外乱力及び摩擦の和と積分補償及び摩擦補償の和とが同じであり両者の均衡が保たれている状況では摩擦補償をオフすることなく更新を停止する。即ち、ロボット2の動作停止直後では両者の均衡が保たれるようになる。その状況から時間経過にしたがって摩擦及び外乱力が変化すると、摩擦補償の更新を停止しているので、それら摩擦及び外乱力の変化分を積分補償が補うことで、両者の均衡が継続して保たれる。そして、ロボット2の動作開始時に摩擦補償の更新を再開し、更新を停止した時点から摩擦補償が変化することで摩擦と摩擦補償との均衡は保たれるが、その分、それまでに摩擦の変化分を補っていた積分補償に相当する分の補償量だけ均衡が崩れることになる。したがって、均衡を保つために、ロボット2の動作開始時の積分補償用トルク(状態量)を次式にしたがって計算して変更する。
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
ロボットの動作開始時の積分補償の状態量(図15中(1)参照)
=ロボットの動作開始直前の積分補償の状態量(図15中(2)参照)−(ロボットの動作開始時の摩擦補償の状態量(図15中(3)参照)−ロボットの動作開始直前の摩擦補償の状態量(図15中(4)参照))
上記した計算式における(ロボットの動作開始時の摩擦補償の状態量−ロボットの動作開始直前の摩擦補償の状態量)は補正量に相当する。
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.
尚、上記した図15では、ロボット2の動作停止中に摩擦が増加したことで積分補償を加えたので、その加えた分を結果的に減じる場合を説明したが、これとは反対に、ロボット2の動作停止中に摩擦が低下したことで積分補償を減じた場合には、その減じた分を結果的に加えることになり、その場合も、不連続となることはなく、両者の均衡が継続して保たれることになる。
Note that in FIG. 15 described above, the integral compensation was added because the friction increased while the operation of the
図16は摩擦補償及び停止時チャタリング防止処理を行ったか否かによる動作中及び動作停止後の振動の発生状況を示す図であり、(a)は摩擦補償を行わなかった場合での振動の発生状況を示しており、(b)は摩擦補償を行わったが停止時チャタリング防止処理を行わなかった場合での振動の発生状況を示しており、(c)は摩擦補償を行って停止時チャタリング防止処理を行った場合での振動の発生状況を示している。図16から明らかなように、摩擦補償を行った場合には、摩擦補償を行わなかった場合に比べて、動作中には振動の発生が抑制されるが、その一方で、動作停止後には逆に振動が発生している。しかしながら、摩擦補償を行った後に上記した積分補償の状態量を変化させることで、摩擦補償を行わなかった場合と同程度まで振動の発生が抑制される。 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.
以上に説明したように第1の実施形態によれば、ロボット制御装置3において、モータ軸側ギア19と出力軸側ギア21とが停止しており且つ弾性変形量がゼロである状態から出力軸20を目標駆動方向に駆動させる場合に、モータ軸側ギア19に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット2の駆動を開始する場合などであり、弾性変形していない停止状態から出力軸20を目標駆動方向に駆動させる場合に、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことでロボット2の実際の動作軌跡をより理論に近づけることができ、ロボットの動作軌跡を高い精度で再現することができる。
As described above, according to the first embodiment, in the
又、モータ軸側ギア19と出力軸側ギア21とが停止しており且つ弾性変形量がゼロでない状態から出力軸20を弾性変形方向と同一方向である目標駆動方向に駆動させる場合に、モータ軸側ギア19に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット2の動作途中で正転を一旦停止して正転を再開する場合などであり、弾性変形している停止状態から出力軸20を弾性変形方向と同一方向である目標駆動方向に駆動させる場合にも、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことができ、ロボットの動作軌跡の精度を適切に高めることができる。
In addition, when the motor
又、モータ軸側ギア19と出力軸側ギア21とが停止しており且つ弾性変形量がゼロでない状態から出力軸20を弾性変形方向と反対方向である目標駆動方向に駆動させる場合に、モータ軸側ギア19に印加する目標駆動方向とは反対方向への摩擦補償用トルクを徐々に減少させた後にモータ軸側ギア19に印加する目標駆動方向への摩擦補償用トルクを徐々に増加させ、弾性変形量が最大弾性変形量に到達した以後では弾性変形量が最大弾性変形量に到達した時点での摩擦補償用トルクを一定値として印加するように構成したので、例えばロボット2の動作途中で正転を一旦停止して正転を再開する場合などであり、弾性変形している停止状態から出力軸20を弾性変形方向と反対方向である目標駆動方向に駆動させる場合にも、このように弾性変形することを考慮して摩擦補償用トルクを印加することで摩擦補償を適切に行うことができ、ロボットの動作軌跡の精度を適切に高めることができる。
When the motor
更に、ロボット2の動作停止時では摩擦補償をオフすることなく更新を停止し、ロボット2の動作再開時では、ロボット2の動作再開時の摩擦補償とロボット2の動作再開直前の摩擦補償との差分を補正量として計算し、ロボット2の動作再開時の積分補償をロボット2の動作開始直前の積分補償から補正量を差引くことで計算するように構成したので、ロボット2の動作再開直後においても外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で両者の均衡を保つことができ、ロボット2の動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。又、このように積分補償の状態量を変化させるだけで良いので、制御系の安定性に悪影響を与えることなく、単純な演算だけで実装可能である。
Further, when the operation of the
(第2の実施形態)
次に、本発明の第2の実施形態について、図20乃至図23を参照して説明する。尚、上記した第1の実施形態と同一部分については説明を省略し、異なる部分について説明する。第2の実施形態は、ロボット2の動作停止時にオーバーシュートの現象が発生することを考慮し、第1の実施形態で説明した動作開始時に行う摩擦補償と同様の制御を動作停止時にも行うことで、ロボット2の動作軌跡をより高い精度で再現するものである。
(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
即ち、ロボット2の動作停止時では、停止指令がロボット制御装置3から出力された後に速度をゼロとするので、停止指令が出力されてから速度をゼロとするまでの僅かな期間がタイムラグとして発生する。又、物体には物理現象としての慣性が存在する。よって、停止指令が出力された後に上記したタイムラグ分だけ遅れて実際の速度がゼロとなる。ロボット2の動作としては、このタイムラグが発生している期間中の動作により、目標角度を僅かに越えてしまい(オーバーシュートしてしまい)、その後に目標角度に対して僅かに越えてしまったオーバーシュート分を戻す動作を行うことで、目標角度に近づけるようにしている。
That is, when the operation of the
一方、ロボット2は例えば生産性の効率が高いことが要求されていることから、なるべく短い時間で正確な位置に到達されることが要求されており、よって、上記したようなオーバーシュートが発生した状況でもなるべく短い時間で目標角度に到達することが要求されている。そこで、通常のトルク制御である時間と目標角度までの差で積分を行い、積分値に比例したトルクを印加するものであるが、この方法では、そもそもオーバーシュート分の角度が極めて僅かであり、全体の動作軌跡の精度を高めることを考慮すると、最初は極めて僅かな積分量では極めて僅かなトルクを印加することしかできず、オーバーシュート分を戻る(方向反転)時に発生する静止摩擦を超える(打ち勝つ)ことができない。最初から大きなトルクを印加してしまうと、静止摩擦を超えることができるようになるが、全体の動作軌跡を精度良く形成することができなくなる。
On the other hand, since the
このような事情から、オーバーシュートした後では、オーバーシュート分を直ちに戻すことができず、外見的にオーバーシュートした角度であたかも停止した状態になる。その後、時間経過にしたがって積分が進み、積分値が一定値に達し、印加するトルクが方向反転時の静止摩擦を超えるようになると、そこで初めてオーバーシュート分を戻す制御を開始する。このようにオーバーシュート分を戻す制御を開始するまでに時間がかかり、オーバーシュートを解消するまでに時間がかかるという問題がある。 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.
そこで、第2の実施形態では、ロボット2の動作停止時においても、目標角度とモータ軸側エンコーダ角度との差分の絶対値が最大弾性変形量角度よりも大きな角度である設定角度を超えていることを条件に、ロボット2の動作開始時に弾性変形量を考慮した摩擦補償を行うことと同様に、弾性変形量を考慮した摩擦補償を行うことにより、オーバーシュート分を戻す制御を早く開始し、オーバーシュートを解消するまでにかかる時間を短縮し、ロボット2の動作軌跡をより高い精度で再現するものである。
Therefore, in the second embodiment, even when the operation of the
第2の実施形態では、CPU12は、弾性変形量更新処理を行い(ステップS4)、摩擦補償用トルク決定処理を行った後に(ステップS5)、停止時摩擦補償処理を行う(ステップS41)。この停止時摩擦補償処理は第1の実施形態で説明した停止時チャタリング防止処理を含む処理である。CPU12は、メイン処理から停止時摩擦補償処理に移行すると、指令速度がゼロであるか否か(ロボット2の動作停止命令であるか否か)を判定する(ステップS31)。
In the second embodiment, the
ここで、CPU12は、指令速度がゼロであると判定すると(ステップS31にて「YES」)、第1の実施形態とは異なり、目標角度とモータ軸側エンコーダ角度との差分の絶対値を計算し、その計算した目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えているか否かを判定する(ステップS51)。設定角度とは予め最大弾性変形量角度よりも大きな角度として設定された角度である。そして、CPU12は、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていると判定すると(ステップS51にて「YES」)、第1の実施形態で説明したステップS32以降の処理を行い、即ち、弾性変形量を考慮した摩擦補償を行う。これに対して、CPU12は、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていないと判定すると(ステップS51にて「NO」)、第1の実施形態で説明したステップS34以降の処理を行い、即ち、弾性変形量を考慮した摩擦補償を行うことはなく、続けて、現在の保存補償トルクを摩擦補償用トルクとして設定し、摩擦補償値を固定した停止時チャタリング防止の摩擦補償を行う。
If the
尚、第1の実施形態で説明したように、CPU12は、メイン処理を所定周期(サンプリング単位周期)で行うことから、停止時摩擦補償処理を所定周期で行うことになり、即ち、指令速度がゼロであると判定し続けており、且つ目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えていると判定し続けている期間で、弾性変形量を考慮した摩擦補償を行う。
As described in the first embodiment, since the
図22は停止時摩擦補償処理を行わなかった場合の目標角度に近づくまでの挙動を示しており、図23は停止時摩擦補償処理を行った場合の目標角度に近づくまでの挙動を示している。図22及び図23から明らかなように、停止時摩擦補償処理を行わなかった制御と停止時摩擦補償処理を行った制御とを比較すると、後者は前者よりも目標角度に近づくまでの戻り始め動作を開始するまでの時間が短いので、戻り始め動作を早く開始した分、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度の範囲内に収束するまでの時間が短くなっている。尚、停止時摩擦補償処理を行わなかった場合でのロボットの動作が停止される直前の摩擦補償用トルクは、停止指令が出力されたと同時に摩擦補償を行わなくなるので、図22中「A」にて示す位置であるが、一方、停止時摩擦補償処理を行った場合でのロボットの動作が停止される直前の摩擦補償用トルクは、停止指令が出力された以降でも実際の角度が設定角度の範囲内となるまで摩擦補償を継続するので、図23中「B」にて示す位置である。 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.
以上に説明したように第2の実施形態によれば、ロボット2の動作停止時においても、目標角度とモータ軸側エンコーダ角度との差分が最大弾性変形量角度よりも大きな角度である設定角度を超えていることを条件に、ロボット2の動作開始時に弾性変形量を考慮した摩擦補償を行うことと同様に、弾性変形量を考慮した摩擦補償を行うように構成したので、戻り始め動作を開始するまでにかかる時間を短縮することができる。これにより、ロボット2の動作開始時だけでなく動作停止時までも含めて動作軌跡をより理論に近づけることができ、ロボット2の動作軌跡を高い精度で再現することができる。
As described above, according to the second embodiment, even when the operation of the
又、目標角度とモータ軸側エンコーダ角度との差分の絶対値が設定角度を超えなくなると(ある程度まで目標角度に収束すると)、第1の実施形態で説明した停止時チャタリング防止処理と同様の処理を行うように構成したので、ロボット2の動作停止時から動作開始時までの停止期間で外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で均衡を保つことができ、ロボット2の動作開始時においても外乱力及び摩擦の和と積分補償及び摩擦補償の和との間で両者の均衡を保つことができ、ロボット2の動作開始から動作停止までの全体の動作軌跡の精度をより高めることができる。
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
(その他の実施形態)
本発明は、上記した実施形態にのみ限定されるものではなく、以下のように変形又は拡張することができる。
対象とするロボットは6軸の垂直多関節型ロボットに限らず他のロボットであっても良い。
(Other embodiments)
The present invention is not limited to the embodiment described above, 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.
図面中、2はロボット、3はロボット制御装置、12はCPU(最大弾性変形量角度記憶手段、弾性変形量計算手段、弾性変形方向検出手段、モータ制御手段)、16はモータ、18はモータ軸、19モータ軸側ギア、20は出力軸、21は出力軸側ギアである。 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 (5)
前記モータ軸のねじれ、前記モータ軸と前記モータ軸側ギアとの接続箇所のねじれ、前記モータ軸側ギアの変形、前記出力軸側ギアの変形及び前記出力軸側ギアと前記出力軸との接続箇所のねじれに起因する変形を弾性変形としてそれらの合算値である弾性変形量の最大値を最大弾性変形量角度として記憶する最大弾性変形量角度記憶手段と、
前記モータ軸の回転角度を検出するモータ軸側エンコーダが電源投入時に示す角度を弾性変形量ゼロ角度とし、前記モータ軸側エンコーダが検出した角度であるモータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えているか否かを判定し、前記モータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えていない場合に前記モータ軸側エンコーダ角度を弾性変形中の角度として判定して弾性変形量を計算し、一方、前記モータ軸側エンコーダ角度と前記弾性変形量ゼロ角度との差分が前記最大弾性変形量角度を超えている場合に前記モータ軸側エンコーダ角度を動作開始時の角度として判定する弾性変形量計算手段と、
前記弾性変形量ゼロ角度と前記モータ軸側エンコーダ角度との差分が示す正負を判定して前記モータ軸側ギアの弾性変形方向を検出する弾性変形方向検出手段と、
前記モータ軸側ギアに摩擦補償用トルクを印加するモータ制御手段とを備え、
前記モータ制御手段は、弾性変形量を更新する弾性変形更新処理を実行することに続いて摩擦補償用トルクを決定する摩擦補償用トルク決定処理を実行する処理を周期的に行い、前記弾性変形更新処理においては、前記弾性変形量計算手段により計算された弾性変形量と正方向の最大弾性変形量及び負方向の最大弾性変形量とを比較し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていなく且つ負方向の最大弾性変形量を超えていないと判定したときには弾性変形中であると判定して現在の弾性変形量を維持し、前記弾性変形量計算手段により計算された弾性変形量が正方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその正方向の最大弾性変形量を弾性変形量として設定し、前記弾性変形量計算手段により計算された弾性変形量が負方向の最大弾性変形量を超えていると判定したときには弾性変形中でないと判定してその負方向の最大弾性変形量を弾性変形量として設定し、前記摩擦補償用トルク決定処理においては、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とを比較し、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが逆方向であると判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に減少させ、目標方向と前記弾性変形方向検出手段により検出された弾性変形方向とが同方向であると判定したときには弾性変形量が最大弾性変形量であるか否かを判定し、弾性変形量が最大弾性変形量でないと判定したときには摩擦補償用トルクをサンプリングタイム単位毎に連続的又は段階的に増加させ、弾性変形量が最大弾性変形量であると判定したときには予め設定されている最大補償トルクを摩擦補償用トルクとして設定することを特徴とするロボット制御装置。 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 And the difference between the motor shaft side encoder angle and the zero elastic deformation amount angle does not exceed the maximum elastic deformation amount angle. The elastic deformation amount is calculated by determining the motor shaft side encoder angle as an angle during elastic deformation, while the difference between the motor shaft side encoder angle and the zero elastic deformation amount angle exceeds the maximum elastic deformation amount angle. Elastic deformation amount calculating means for determining the motor shaft side encoder angle as an angle at the start of operation when
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; 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, it is determined 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. The elastic deformation amount is calculated by determining the shaft side encoder angle as an angle during elastic deformation, while the difference between the motor shaft side encoder angle and the output shaft side encoder angle exceeds the maximum elastic deformation amount angle. Elastic deformation amount calculating means for determining the motor shaft side encoder angle as an angle at the start of operation in the case,
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 that calculates a state quantity of integral compensation at the start of operation of the robot 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)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009282813A JP5299255B2 (en) | 2009-03-18 | 2009-12-14 | Robot controller |
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009066138 | 2009-03-18 | ||
JP2009066138 | 2009-03-18 | ||
JP2009282813A JP5299255B2 (en) | 2009-03-18 | 2009-12-14 | Robot controller |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2010240827A true JP2010240827A (en) | 2010-10-28 |
JP5299255B2 JP5299255B2 (en) | 2013-09-25 |
Family
ID=43094464
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009282813A Active JP5299255B2 (en) | 2009-03-18 | 2009-12-14 | Robot controller |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5299255B2 (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105487484A (en) * | 2014-10-07 | 2016-04-13 | 发那科株式会社 | Motor control appratus |
WO2020008751A1 (en) * | 2018-07-02 | 2020-01-09 | パナソニックIpマネジメント株式会社 | Robot control method and robot control device |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05282019A (en) * | 1992-03-31 | 1993-10-29 | Makino Milling Mach Co Ltd | Method and device for acceleration control of servo system |
-
2009
- 2009-12-14 JP JP2009282813A patent/JP5299255B2/en active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05282019A (en) * | 1992-03-31 | 1993-10-29 | Makino Milling Mach Co Ltd | Method and device for acceleration control of servo system |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105487484A (en) * | 2014-10-07 | 2016-04-13 | 发那科株式会社 | Motor control appratus |
JP2016076119A (en) * | 2014-10-07 | 2016-05-12 | ファナック株式会社 | Motor controller for correcting reverse delay of motor |
US9760082B2 (en) | 2014-10-07 | 2017-09-12 | Fanuc Corporation | Motor control apparatus for compensating reverse delay of motor |
WO2020008751A1 (en) * | 2018-07-02 | 2020-01-09 | パナソニックIpマネジメント株式会社 | Robot control method and robot control device |
JPWO2020008751A1 (en) * | 2018-07-02 | 2021-07-08 | パナソニックIpマネジメント株式会社 | Robot control method and robot control device |
JP7194910B2 (en) | 2018-07-02 | 2022-12-23 | パナソニックIpマネジメント株式会社 | ROBOT CONTROL METHOD AND ROBOT CONTROL DEVICE |
Also Published As
Publication number | Publication date |
---|---|
JP5299255B2 (en) | 2013-09-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6648913B2 (en) | Control method, robot apparatus, program, recording medium, article manufacturing method, and driving apparatus | |
US8614558B2 (en) | Motor control apparatus and motor control method thereof | |
EP3023208A1 (en) | Control device for motor drive device, control device for multi-axial motor, and control method for motor drive device | |
JP5318727B2 (en) | Information processing method, apparatus and program | |
JP5916583B2 (en) | Weaving control device for articulated robot | |
US20120046789A1 (en) | Walking robot and control method thereof | |
JP6564433B2 (en) | Robot system | |
CN110662636A (en) | Speed reducer angle transmission error identification system and speed reducer angle transmission error identification method | |
JP5299255B2 (en) | Robot controller | |
JP2019177432A (en) | Robot system and control method | |
JP7164368B2 (en) | robot equipment | |
JP4498061B2 (en) | Welding robot controller | |
US11890759B2 (en) | Robot control method | |
JP2020015124A (en) | Robot control method, article manufacturing method, robot control device, robot, program and recording medium | |
JP2021088042A (en) | Robot control device, gripping system and robot hand control method | |
JP5262880B2 (en) | Robot controller | |
JP4784432B2 (en) | Multi-legged walking robot | |
JP5402125B2 (en) | Robot controller | |
JP7194910B2 (en) | ROBOT CONTROL METHOD AND ROBOT CONTROL DEVICE | |
JP7113200B2 (en) | ROBOT CONTROL METHOD AND ROBOT CONTROL DEVICE | |
JP2021028786A (en) | Drive control unit, robot and robot system | |
JP7182952B2 (en) | CONTROL METHOD, CONTROL PROGRAM, RECORDING MEDIUM, CONTROL DEVICE, ROBOT SYSTEM, AND PRODUCT MANUFACTURING METHOD | |
JP2012061560A (en) | Robot control device | |
WO2022176456A1 (en) | Spring constant correction device, method therefor, and recording medium | |
JP2024071866A (en) | Robot control method and robot system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20110822 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20121218 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20130521 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130603 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5299255 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |