JPH06339292A - Force controlling method by estimation of disturbance load - Google Patents
Force controlling method by estimation of disturbance loadInfo
- Publication number
- JPH06339292A JPH06339292A JP5255266A JP25526693A JPH06339292A JP H06339292 A JPH06339292 A JP H06339292A JP 5255266 A JP5255266 A JP 5255266A JP 25526693 A JP25526693 A JP 25526693A JP H06339292 A JPH06339292 A JP H06339292A
- Authority
- JP
- Japan
- Prior art keywords
- force
- motor
- value
- speed
- control
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/19—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D17/00—Control of torque; Control of mechanical power
- G05D17/02—Control of torque; Control of mechanical power characterised by the use of electric means
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Evolutionary Computation (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Control Of Electric Motors In General (AREA)
- Feedback Control In General (AREA)
Abstract
Description
【０００１】[0001]
【産業上の利用分野】本発明は、工作機械やロボット、
さらに産業機械において、制御対象に力を与えるととも
に、その力を制御する力制御方法に関する。BACKGROUND OF THE INVENTION The present invention relates to machine tools, robots,
Further, the present invention relates to a force control method for applying a force to a controlled object and controlling the force in an industrial machine.
【０００２】[0002]
【従来の技術】産業機械において、制御対象に力を加え
るとともに、その力を制御する力制御を行う場合、従
来、油圧シリンダを用いて油圧によって制御対象に与え
る力を制御する方法が一般的に行われている。また、ロ
ボットによりバリ取りを行うバリ取りロボットの場合、
ロボットの手首先端に取り付けたバリ取りツールをワー
クに押しつけて、その押圧力を制御しながらバリ取りを
行っている。このバリ取りロボットの場合、ロボットの
アーム等の各軸を駆動する駆動源は、油圧シリンダやサ
ーボモータで構成されているが、サーボモータで構成し
た場合、上記力制御を行うために力センサが用いられて
いる。すなわち、機械の先端の制御対象を押し付ける部
分に力センサを取付け、該センサで検出される力をフィ
ードバックして検出値が目標値に一致するようにフィー
ドバック制御を行なっている。2. Description of the Related Art In industrial machines, when a force is applied to a controlled object and force control is performed to control the force, conventionally, a method of controlling the force applied to the controlled object by hydraulic pressure using a hydraulic cylinder is generally used. Has been done. In the case of a deburring robot that deburrs with a robot,
The deburring tool attached to the tip of the wrist of the robot is pressed against the work, and deburring is performed while controlling the pressing force. In the case of this deburring robot, the drive source for driving each axis of the robot arm and the like is composed of a hydraulic cylinder or a servo motor. However, when it is composed of a servo motor, a force sensor is used to perform the above force control. It is used. That is, a force sensor is attached to the portion of the machine tip that presses the controlled object, and the force detected by the sensor is fed back to perform feedback control so that the detected value matches the target value.
【０００３】[0003]
【発明が解決しようとする課題】力センサを用いて力の
フィードバック制御を行う場合、力制御の安定性，信頼
性は力センサの安定性および信頼性に依存し、力センサ
が安定で高精度であれば高い精度の力制御が可能にな
る。しかし、力センサの信頼性は、従来のサーボモータ
の制御に使用される位置検出器の信頼性と比較して一般
的に低く、位置・速度制御に比べ力制御の精度は低い。
さらに、力センサは非常に高価であり、装置全体の価格
が低いものには使用できない。また、力センサはその取
付けが難しいという問題もある。また、モータによって
力制御を行う場合、力センサを使用せざるを得なく、該
力センサを使用すると上述したような問題が生じること
になる。そのため、駆動源として油圧シリンダを用いざ
るを得なく、各種装置において油圧式から電気式への変
換を妨げる要因となっている。When performing force feedback control using a force sensor, the stability and reliability of force control depend on the stability and reliability of the force sensor, and the force sensor is stable and highly accurate. If so, highly accurate force control becomes possible. However, the reliability of the force sensor is generally lower than the reliability of the position detector used for controlling the conventional servomotor, and the accuracy of the force control is lower than that of the position / speed control.
Moreover, force sensors are very expensive and cannot be used with low overall device costs. There is also a problem that the force sensor is difficult to mount. Further, when the force control is performed by the motor, the force sensor must be used, and the use of the force sensor causes the above-mentioned problems. Therefore, a hydraulic cylinder has to be used as a drive source, which is a factor that hinders the conversion from hydraulic to electric in various devices.
【０００４】そこで、本発明の目的は、力センサを必要
とせず、モータによる力制御を容易に実施できる外乱負
荷推定による力制御方法を提供することにある。Therefore, an object of the present invention is to provide a force control method based on disturbance load estimation which does not require a force sensor and can easily perform force control by a motor.
【０００５】[0005]
【課題を解決するための手段】本発明は、モータで駆動
する被駆動体に加わる外界からの負荷を力センサの代わ
りに外乱推定オブザーバによって推定し、該推定外乱負
荷トルクをフィードバックして該推定外乱負荷トルクが
力指令値と一致するようにフィードバック制御する。さ
らに、安定性と、制御対象からの反力を受けないときの
暴走を防止するために、上記推定外乱負荷トルクにモー
タの速度に設定係数を乗じた値を加算した値を力のフィ
ードバック値としてフィードバック制御を行う。また、
制御対象からの反力を受けないときの暴走を防止するた
めに、上記推定外乱負荷トルクにモータの最大許容速度
に設定係数を乗じた値を加算した値をフィードバック
し、この加算した値によって力の指令値であるトルク指
令値をクランプし、モータの回転速度が最大許容値以上
にならないように力制御を行う。また、上記外乱推定オ
ブザーバはモータに指令されるトルク指令値とモータの
実速度から外乱負荷トルクを推定して推定外乱負荷トル
クとして出力するものである。SUMMARY OF THE INVENTION According to the present invention, a load from an external environment applied to a driven body driven by a motor is estimated by a disturbance estimation observer instead of a force sensor, and the estimated disturbance load torque is fed back to estimate the load. Feedback control is performed so that the disturbance load torque matches the force command value. Furthermore, in order to prevent stability and runaway when the reaction force from the controlled object is not received, the value obtained by adding the value obtained by multiplying the estimated disturbance load torque by the motor speed by a setting coefficient is used as the force feedback value. Perform feedback control. Also,
In order to prevent runaway when the reaction force from the controlled object is not received, the value obtained by adding the value obtained by multiplying the estimated allowable load torque by the maximum allowable speed of the motor by the setting coefficient is fed back, and the force is added by this added value. The torque command value that is the command value of is clamped, and force control is performed so that the rotation speed of the motor does not exceed the maximum allowable value. The disturbance estimation observer estimates the disturbance load torque from the torque command value commanded to the motor and the actual speed of the motor, and outputs the estimated disturbance load torque.
【０００６】[0006]
【作用】本発明によれば、外乱推定オブザーバによっ
て、モータに加わる外乱負荷トルクを推定し、この推定
外乱負荷トルクをフィードバックして目標値である力指
令値と一致するようにフィードバック制御する。そのた
め、推定外乱負荷トルクは目標値と一致するように制御
され，推定外乱負荷トルクが実際の負荷トルクと近似し
ていれば、実際の負荷トルクは目標値に制御されること
になる。According to the present invention, the disturbance estimation observer estimates the disturbance load torque applied to the motor, and the estimated disturbance load torque is fed back to perform feedback control so that it coincides with the target force command value. Therefore, the estimated disturbance load torque is controlled to match the target value, and if the estimated disturbance load torque is close to the actual load torque, the actual load torque is controlled to the target value.
【０００７】外乱推定オブザーバは、モータに指令され
るトルク指令値とモータの実速度等によって外乱負荷ト
ルクを推定するものであるから、トルクを与える対象が
なかったり、トルクを与えると動いてしまうような、モ
ータが制御対象からの反力を受けない場合には、推定外
乱負荷トルクが目標値に達せず、目標値に達しようとし
てモータが暴走することが想定される。そこで、モータ
の速度に設定係数を乗じた値をもフィードバックするこ
とによって、モータの実速度が上昇すればフィードバッ
ク値も上昇して目標値と一致するようにすることによ
り、モータの暴走を防止する。また、モータの最大許容
速度に設定係数を乗じた値を推定外乱負荷トルクに加算
した値をフィードバックして新たなトルク指令値を設定
してこの値にクランプしてモータの速度を最大許容速度
に制限して、モータの回転速度が最大許容値以上になら
ないようにして、モータの暴走を防止する。さらに、モ
ータの実速度をフィードバックすることよって振動発生
を防止し、制御の安定性を増すことができる。Since the disturbance estimation observer estimates the disturbance load torque based on the torque command value commanded to the motor, the actual speed of the motor, etc., there is no target to which the torque is applied, or when the torque is applied, the disturbance estimation observer seems to move. If the motor does not receive the reaction force from the controlled object, it is assumed that the estimated disturbance load torque does not reach the target value and the motor runs out of control to reach the target value. Therefore, by also feeding back the value obtained by multiplying the motor speed by the setting coefficient, the feedback value also increases as the actual speed of the motor increases so as to match the target value, thereby preventing runaway of the motor. . Also, the value obtained by multiplying the maximum permissible speed of the motor by a setting coefficient is added to the estimated disturbance load torque is fed back to set a new torque command value and clamped to this value to set the motor speed to the maximum permissible speed. By limiting the rotation speed of the motor so that it does not exceed the maximum allowable value, the runaway of the motor is prevented. Further, by feeding back the actual speed of the motor, it is possible to prevent the occurrence of vibration and increase the stability of control.
【０００８】[0008]
【実施例】以下、本発明の実施例を図を参照しながら詳
細に説明するが、本発明は実施例に限定されるものでは
ない。Embodiments of the present invention will now be described in detail with reference to the drawings, but the present invention is not limited to the embodiments.
【０００９】（実施例の構成）図１は、本発明を適用し
た一実施例の、工作機械の送り軸やロボットのアームの
軸を駆動するサーボモータの力制御のブロック線図であ
る。この実施例では、力制御を比例，積分（ＰＩ）制御
で行うもので、サーボモータに加わる外界からの力を外
乱推定オブザーバを適用して検出するようにしている。
項１のｋ１は力フィードバック制御における積分定数、
ｋ２は比例定数である。また、項２，３はモータの伝達
関数で、ｋｔはトルク定数、Ｊｍはイナーシャであり、
項４はモータの実速度ｖに設定係数βを乗じた値をフィ
ードバックする項である。(Structure of Embodiment) FIG. 1 is a block diagram of force control of a servomotor for driving a feed shaft of a machine tool or an arm of a robot according to an embodiment of the present invention. In this embodiment, the force control is performed by proportional and integral (PI) control, and the force from the external world applied to the servomotor is detected by applying a disturbance estimation observer.
K1 of the term 1 is an integration constant in the force feedback control,
k2 is a proportional constant. Further, the terms 2 and 3 are transfer functions of the motor, kt is a torque constant, Jm is inertia,
The term 4 is a term for feeding back a value obtained by multiplying the actual speed v of the motor by the setting coefficient β.
【００１０】図中の一点鎖線で示される５は、外界から
サーボモータに印加される外乱負荷トルクを検出する外
乱推定オブザーバであり、モータに指令されるトルク指
令Ｔｃとモータの実速度ｖから、推定外乱負荷トルクＴ
ｄ2 を推定するものである。つまり、外乱推定オブザー
バ５は、モータが外界から実際に受ける外乱負荷トルク
ＴＬを直接に測定することなく、トルク指令Ｔｃとモー
タの実速度ｖとに基づいた推定値として外乱負荷トルク
Ｔｄ2 を出力するものである。なお、ＴＬは実際にモー
タが受ける外乱負荷トルクであり、Ｓはラプラス演算子
を表している。Reference numeral 5 shown by a one-dot chain line in the figure is a disturbance estimation observer for detecting the disturbance load torque applied to the servo motor from the outside, and from the torque instruction Tc instructed to the motor and the actual speed v of the motor, Estimated disturbance load torque T
This is to estimate d2. That is, the disturbance estimation observer 5 outputs the disturbance load torque Td2 as an estimated value based on the torque command Tc and the actual speed v of the motor without directly measuring the disturbance load torque TL that the motor actually receives from the external environment. It is a thing. Note that TL is the disturbance load torque actually received by the motor, and S represents the Laplace operator.
【００１１】力指令Ｆｃから外乱推定オブザーバ５で推
定した推定外乱負荷トルクＴｄ2 を減じ（なお、外乱推
定オブザーバ５で求められる推定外乱負荷トルクＴｄ2
は逆極性で得られるため、図１においては力指令Ｆｃに
推定外乱負荷トルクＴｄ2 が加算されるように記載され
ているが、実際は減算される）、さらに、項４において
モータの実速度ｖに設定係数βを乗じた値を減じて力偏
差Ｆerr （＝Ｆｃ＋Ｔｄ２−β・ｖ）を求め、この力偏
差Ｆerr に基づいて項１において比例積分処理を実行し
て、トルク指令（電流指令）Ｔｃを求めモータへ出力す
る。この項４を通るフィードバック制御により、サーボ
モータから制御対象に付加される力（サーボモータが発
生する力）が力指令Ｆｃに一致するように制御される。The estimated disturbance load torque Td2 estimated by the disturbance estimation observer 5 is subtracted from the force command Fc (the estimated disturbance load torque Td2 obtained by the disturbance estimation observer 5 is subtracted).
, The estimated disturbance load torque Td2 is added to the force command Fc in FIG. 1, but it is actually subtracted.) The force deviation Ferr (= Fc + Td2-β · v) is obtained by subtracting the value multiplied by the setting coefficient β, and the proportional-plus-integral process is executed in item 1 based on this force deviation Ferr to calculate the torque command (current command) Tc. Output to the required motor. By the feedback control that passes through the term 4, the force applied from the servo motor to the controlled object (the force generated by the servo motor) is controlled to match the force command Fc.
【００１２】また、外乱推定オブザーバ５の項５２，５
３のｋ３，ｋ４は外乱推定オブザーバのパラメータであ
り、項５１のαは実際にサーボモータに出力されるトル
ク指令となる電流値Ｔｃに乗じられるパラメータの値で
あり、モータのトルク定数の推定値ｋｔ* をイナーシャ
の推定値Ｊm * で除した（α＝ｋｔ* ／Ｊｍ* ）として
表される。５４は積分項で、項５１，５２，５３の出力
をすべて加算した値を積分し、モータの推定速度ｖａを
求める項である。また、項５５は、項５３からの出力に
（１／α）を乗じて推定外乱負荷トルクＴｄ2 を求める
項である。The terms 52 and 5 of the disturbance estimation observer 5
3 k3 and k4 are parameters of the disturbance estimation observer, α of the term 51 is a value of the parameter that is multiplied by the current value Tc that is the torque command actually output to the servo motor, and the estimated value of the torque constant of the motor. the kt * expressed as divided by the inertia estimate Jm * (α = kt * / Jm *). Reference numeral 54 is an integral term, which is a term for integrating a value obtained by adding all the outputs of the terms 51, 52, 53 to obtain an estimated speed va of the motor. The term 55 is a term for multiplying the output from the term 53 by (1 / α) to obtain the estimated disturbance load torque Td2.
【００１３】図１に示される力制御のブロック図におい
て、α＝ｋｔ* ／Ｊｍ* とおき、かつモータのトルク定
数ｋｔをその推定値ｋｔ* と等しい（ｋｔ＝ｋｔ* ）と
し、モータのイナーシャＪｍをその推定値Ｊｍ* （Ｊｍ
＝Ｊｍ* ）として解析すると、項３の演算により （Ｔｃ・ｋｔ＋ＴＬ）（１／Ｊｍ・Ｓ）＝ｖ …（１） が得られ、また項５の出力ｖａを考えると、 ｛Ｔｃ・（ｋｔ／Ｊｍ）＋（ｖ−ｖａ）ｋ３＋（ｖ−ｖａ）（ｋ４／Ｓ）｝ ・（１／Ｓ）＝ｖａ …（２） が得られる。第（１）式を変形すると次式となり、 Ｔｃ＝（ｖ・Ｊｍ・Ｓ−ＴＬ）／ｋｔ …（３） この第（３）式を第（２）式に代入して整理すると、 （ｖ・Ｊｍ・Ｓ−ＴＬ）／Ｊｍ＋（ｖ−ｖａ）ｋ３ ＋（ｖ−ｖａ）（ｋ４／Ｓ）＝ｖａ・Ｓ …（４） Ｓ（ｖ−ｖａ）＋（ｖ−ｖａ）・ｋ３＋（ｖ−ｖａ）（ｋ４／Ｓ） ＝ＴＬ／Ｊｍ …（５） となる。また、第（５）式からＶｅｒｒ（＝ｖ−ｖａ）
を求めると、 Ｖｅｒｒ＝ｖ−ｖａ ＝（ＴＬ／Ｊｍ）［１／｛Ｓ＋ｋ３＋（ｋ４／Ｓ）｝］ …（６） 上記第（６）式から項５３の出力Ｔｄ１は次の第（７）
式によって表される。In the block diagram of the force control shown in FIG. 1, α = kt * / Jm * is set , the torque constant kt of the motor is set equal to its estimated value kt * (kt = kt * ), and the inertia of the motor is set. Jm is the estimated value Jm * (Jm
= Jm * ), (Tckt + TL) (1 / JmS) = v (1) is obtained by the calculation of the term 3, and considering the output va of the term 5, {Tc (kt /Jm)+(v-va)k3+(v-va)(k4/S)}.(1/S)=va (2) is obtained. When the equation (1) is transformed, the following equation is obtained: Tc = (v · Jm · S-TL) / kt (3) When this equation (3) is substituted into the equation (2) and rearranged, (v * Jm * S-TL) / Jm + (v-va) k3 + (v-va) (k4 / S) = va * S ... (4) S (v-va) + (v-va) * k3 + (v -Va) (k4 / S) = TL / Jm (5) Also, from the expression (5), Verr (= v-va)
Verr = v−va = (TL / Jm) [1 / {S + k3 + (k4 / S)}] (6) From the above equation (6), the output Td1 of the term 53 is the following (7)
It is represented by a formula.
【００１４】 Ｔｄ１＝Ｖｅｒｒ・（ｋ４／Ｓ） ＝（ＴＬ／Ｊｍ）｛ｋ４／（Ｓ2 ＋ｋ３・Ｓ＋ｋ４）｝ …（７） 第（７）式において、パラメータｋ３，ｋ４を極が安定
するように選択すると、Ｔｄ１＝ＴＬ／Ｊｍと近似する
ことができ、この関係式は全外乱トルクＴｄ１を推定で
きることを示している。Td1 = Verr · (k4 / S) = (TL / Jm) {k4 / (S 2 + k3 · S + k4)} (7) In the equation (7), the poles of the parameters k3 and k4 are stabilized. When it is selected, Td1 = TL / Jm can be approximated, and this relational expression shows that the total disturbance torque Td1 can be estimated.
【００１５】そして、この全外乱トルクＴｄ１に１／α
（＝Ｊｍ* ／ｋｔ* ）を乗じて推定外乱負荷トルクＴｄ
2 を求め、この推定外乱負荷トルクＴｄ2 を用いて力の
フィードバック制御を行う。Then, 1 / α is added to the total disturbance torque Td1.
Estimated disturbance load torque Td multiplied by (= Jm * / kt * )
2 is obtained, and force feedback control is performed using this estimated disturbance load torque Td2.
【００１６】この推定外乱負荷トルクＴｄ2 を用いた力
のフィードバック制御では、力指令Ｆｃに対して、外乱
推定オブザーバ５で推定した推定外乱負荷トルクＴｄ2
と項４においてモータの実速度ｖに設定係数βを乗じた
値との差によって力偏差を求めて、その力偏差Ｆerr
（＝Ｆｃ＋Ｔｄ２−β・ｖ）を求め、さらに項１におい
てこの力偏差Ｆerr を比例積分処理してトルク指令Ｔｃ
を求める。このトルク指令Ｔｃは電流指令であり、この
電流指令をモータに出力することによってモータのトル
ク制御を行うことができる。In the force feedback control using the estimated disturbance load torque Td2, the estimated disturbance load torque Td2 estimated by the disturbance estimation observer 5 with respect to the force command Fc.
And the difference between the actual speed v of the motor and the value obtained by multiplying the setting coefficient β in item 4, the force deviation is obtained, and the force deviation Ferr
(= Fc + Td2−β · v) is obtained, and the force deviation Ferr is proportional-integrated in the item 1, and the torque command Tc is obtained.
Ask for. This torque command Tc is a current command, and the torque of the motor can be controlled by outputting this current command to the motor.
【００１７】つまり、この項４を通るフィードバック制
御により、サーボモータから制御対象に付加される力
（サーボモータが発生する力）が、力指令値Ｆｃに一致
するように制御されることになり、例えばこの力指令値
Ｆｃをある設定値として与えると、サーボモータに実際
に加わる負荷の大きさに係わらず、モータからは常に設
定されたトルクが発生することになる。That is, by the feedback control that passes through the term 4, the force applied from the servo motor to the controlled object (the force generated by the servo motor) is controlled so as to match the force command value Fc. For example, when this force command value Fc is given as a certain set value, the set torque is always generated from the motor regardless of the magnitude of the load actually applied to the servo motor.
【００１８】図２は、本発明の方法を実施するサーボモ
ータ制御系の要部ブロック図である。図２において、１
０は一般の工作機械やロボット等の機械を制御する制御
装置と同様の制御装置であり、この制御装置１０から移
動指令，力指令，各種制御信号が共有メモリ１１を介し
てディジタルモータ制御回路１２に出力される。ディジ
タルモータ制御回路１２は、プロセッサ（ＣＰＵ），Ｒ
ＯＭ，ＲＡＭ等により構成され、位置，速度，力等のモ
ータ制御をディジタル的に実行し、トランジスタインバ
ータ等で構成されるサーボアンプ１３を介して各軸のサ
ーボモータ１４を制御するものである。また、１５は位
置，速度を検出する位置速度検出器でサーボモータのモ
ータ軸に取り付けられたパルスコーダ等で構成され、デ
ィジタルモータ制御回路１２に検出した位置，速度フィ
ードバック信号を出力している。なお、これらの構成
は、従来の公知のディジタルサーボ回路の構成と同一の
ものを用いることができるが、力制御を行う点において
従来のディジタルサーボ回路と異なっている。FIG. 2 is a block diagram of essential parts of a servo motor control system for carrying out the method of the present invention. In FIG. 2, 1
Reference numeral 0 denotes a control device similar to a control device for controlling a machine such as a general machine tool or a robot. Movement commands, force commands, and various control signals from this control device 10 are sent to the digital motor control circuit 12 via the shared memory 11. Is output to. The digital motor control circuit 12 includes a processor (CPU), R
It is composed of OM, RAM, etc., and digitally executes motor control of position, speed, force, etc., and controls the servo motor 14 of each axis via a servo amplifier 13 composed of a transistor inverter or the like. Further, reference numeral 15 is a position / speed detector for detecting the position / speed, which is composed of a pulse coder or the like attached to the motor shaft of the servo motor, and outputs the detected position / speed feedback signal to the digital motor control circuit 12. Note that these configurations can use the same configurations as those of the conventionally known digital servo circuit, but differ from the conventional digital servo circuit in that force control is performed.
【００１９】（実施例の動作）次に、前記力制御を行う
ための制御系の一動作例について、図３に示すディジタ
ルモータ制御回路のプロセッサが実施する力制御の一処
理のフローチャートに従って説明する。(Operation of Embodiment) Next, an operation example of the control system for performing the force control will be described with reference to a flow chart of a process of force control executed by the processor of the digital motor control circuit shown in FIG. .
【００２０】なお、あらかじめ、外乱推定オブザーバを
構成する定数ｋ３，ｋ４，係数α，βをディジタルモー
タ制御回路１２内に設置しておくものとする。The constants k3, k4 and the coefficients α, β constituting the disturbance estimation observer are set in advance in the digital motor control circuit 12.
【００２１】また、このサーボモータ１４の位置・速度
を制御するときは、従来と同様の位置・速度ループ処理
を実行し、力制御を行うときは、位置・速度制御処理か
ら図３に示す力制御処理に切り換えて力制御を実行す
る。例えば、バリ取りロボットにおいて、バリ取り開始
位置までは、位置・速度制御を行って位置決めした後、
バリ取りのための力制御を実行する。When the position / speed of the servomotor 14 is controlled, the same position / speed loop processing as in the conventional case is executed, and when the force control is performed, the force shown in FIG. Switch to control processing and execute force control. For example, in a deburring robot, after positioning by performing position / speed control up to the deburring start position,
Perform force control for deburring.
【００２２】力制御に切り換えられると、ディジタルモ
ータ制御回路１２のプロセッサは所定周期（通常速度ル
ープ処理周期と同一の周期）毎に図３に示す処理を実行
する。When switched to the force control, the processor of the digital motor control circuit 12 executes the processing shown in FIG. 3 every predetermined cycle (the same cycle as the normal speed loop processing cycle).
【００２３】まず、制御装置１０から共有メモリ１１を
介して送られてきた力指令値Ｆｃを読むとともに、位置
・速度検出器１５で検出されフィードバックされた速度
フィードバック値ｖを読み込む（ステップＳ１）。そし
て、外乱推定オブザーバ５の処理を開始し、ステップＳ
１で読み取った速度フィードバック値ｖからレジスタＲ
（ｖａ）に記憶してある推定速度ｖａを減じて、実速度
と推定速度の差Ｖｅｒｒを求める（ステップＳ２）。な
お、図３においては、このレジスタＲに記憶してある推
定速度をＲ（ｖａ）で表している。さらに、誤差Ｖｅｒ
ｒに設定定数ｋ４を乗じた値を全外乱推定値Ｔｄ１を記
憶しているアキュムレータに加算し、当該周期における
全外乱推定値Ｔｄ１を求める（ステップＳ３）。すなわ
ち、ステップＳ３の処理は図１における要素５３の処理
である。First, the force command value Fc sent from the control device 10 via the shared memory 11 is read, and the speed feedback value v detected by the position / speed detector 15 and fed back is read (step S1). Then, the processing of the disturbance estimation observer 5 is started, and step S
From the speed feedback value v read in 1, register R
The estimated speed va stored in (va) is subtracted to obtain the difference Verr between the actual speed and the estimated speed (step S2). In FIG. 3, the estimated speed stored in the register R is represented by R (va). Furthermore, the error Ver
A value obtained by multiplying r by the setting constant k4 is added to the accumulator that stores the total disturbance estimated value Td1 to obtain the total disturbance estimated value Td1 in the period (step S3). That is, the process of step S3 is the process of the element 53 in FIG.
【００２４】次に、推定速度ｖａを記憶するレジスタＲ
（ｖａ）にステップＳ３で求めた全外乱推定値Ｔｄ１を
加算するとともに、ステップＳ２で求めた差Ｖｅｒｒに
定数ｋ３を乗じた値を加算し、さらに、レジスタＲ（Ｔ
ｃ）に記憶する前周期で読み込んだトルク指令Ｔｃに設
定定数α（ｋｔ* ＝／Ｊｍ* ）を乗じた値を加算し当該
周期の速度推定値ｖａを求め、レジスタＲ（ｖａ）に格
納する（ステップＳ４）。すなわち、ステップＳ４の処
理は、図１における要素５１及び要素５４等の処理によ
って推定速度ｖａを求める処理である。Next, a register R for storing the estimated speed va
The total disturbance estimation value Td1 obtained in step S3 is added to (va), and the value obtained by multiplying the difference Verr obtained in step S2 by a constant k3 is added.
The value obtained by multiplying the torque command Tc read in the previous cycle stored in c) by the setting constant α (kt * = / Jm * ) is added to obtain the speed estimation value va of the cycle and stored in the register R (va). (Step S4). That is, the process of step S4 is a process of obtaining the estimated speed va by the process of the elements 51 and 54 in FIG.
【００２５】次に、ステップＳ３で求めた全外乱推定値
Ｔｄ１を設定係数αで除して推定外乱負荷トルクＴｄ２
を求める。Next, the estimated disturbance load torque Td2 is obtained by dividing the total disturbance estimated value Td1 obtained in step S3 by the set coefficient α.
Ask for.
【００２６】以上のステップＳ２〜ステップＳ５までの
処理が推定外乱負荷トルクを求める外乱推定オブザーバ
５の処理である。The processing from step S2 to step S5 described above is the processing of the disturbance estimation observer 5 for obtaining the estimated disturbance load torque.
【００２７】次に、外乱推定オブザーバ５により求めら
れた推定外乱負荷トルクを用いて、力制御を行う。以下
に示す制御例は、推定外乱負荷トルクにモータの実速度
に設定係数を乗じた値を加算した値を力のフィードバッ
ク値とし、そのフィードバック値が力指令値と一致する
ようにフィードバック制御することにより、安定性と、
制御対象からの反力を受けないときの暴走を防止するも
のである。Next, force control is performed using the estimated disturbance load torque obtained by the disturbance estimation observer 5. In the control example shown below, the value obtained by adding the value obtained by multiplying the estimated disturbance load torque by the actual speed of the motor by the setting coefficient is used as the force feedback value, and feedback control is performed so that the feedback value matches the force command value. Stability and
This is to prevent runaway when the reaction force from the controlled object is not received.
【００２８】前記ステップＳ２〜ステップＳ５の外乱推
定オブザーバ５の処理により求められた推定外乱負荷ト
ルクＴｄ２を力のフィードバック値として利用し、力指
令値Ｆｃから減じることになるが、外乱推定オブザーバ
５で求められる推定外乱負荷トルクＴｄ２は力指令値Ｆ
ｃと極性が逆になって求められるから、実際は力指令値
Ｆｃに求められた推定外乱負荷トルクＴｄ２を加算する
ことによって実質的に差を求めることになる。そして、
さらに、ステップＳ１で求めた速度フィードバック値ｖ
に設定係数βを乗じた値を減じて力偏差Ｆｅｒｒを求め
る。すなわち、次式の演算を行うことにより力偏差Ｆｅ
ｒｒを求める（ステップＳ６）。Although the estimated disturbance load torque Td2 obtained by the processing of the disturbance estimation observer 5 in steps S2 to S5 is used as a force feedback value and subtracted from the force command value Fc, the disturbance estimation observer 5 The estimated disturbance load torque Td2 obtained is the force command value F
Since the polarity is obtained by reversing the polarity from c, the difference is actually obtained by actually adding the obtained estimated disturbance load torque Td2 to the force command value Fc. And
Further, the velocity feedback value v obtained in step S1
The force deviation Ferr is calculated by subtracting the value obtained by multiplying by the setting coefficient β. That is, the force deviation Fe
rr is calculated (step S6).
【００２９】Ｆｅｒｒ＝Ｆｃ＋Ｔｄ２−β・ｖ 次に、積分器として作用するアキュムレータＳｕｍに上
記力偏差Ｆｅｒｒに積分定数ｋ１、及びこの図３に示す
処理の周期Ｔｓを乗じた値を加算することによって積分
処理を実行する（図１中の要素１の積分処理）（ステッ
プＳ７）。そして、該アキュムレータＳｕｍの値と上記
力偏差Ｆｅｒｒに比例定数ｋ２を乗じた値を加算してト
ルク指令Ｔｃを求める（ステップＳ８）。すなわち、図
１の項１の処理を実行することになる。こうして求めら
れたトルク指令Ｔｃを次の周期で使用するためにレジス
タＲ（Ｔｃ）に格納するとともに電流ループに引き渡し
（ステップＳ９，ステップＳ１０）、当該周期の処理を
終了する。Ferrr = Fc + Td2-βv Next, the force deviation Ferr is added to the accumulator Sum acting as an integrator, and the value obtained by multiplying the integration constant k1 and the processing cycle Ts shown in FIG. Processing is executed (integration processing of element 1 in FIG. 1) (step S7). Then, the torque command Tc is obtained by adding the value of the accumulator Sum and the value obtained by multiplying the force deviation Ferr by the proportional constant k2 (step S8). That is, the processing of item 1 in FIG. 1 is executed. The torque command Tc thus obtained is stored in the register R (Tc) for use in the next cycle and is also passed to the current loop (steps S9 and S10), and the processing of the cycle is completed.
【００３０】以降、力制御に切り換えられている限り、
上記図３に示す処理を所定周期毎に実行する。After that, as long as the force control is switched to,
The process shown in FIG. 3 is executed every predetermined period.
【００３１】上記実施例では、フィードバック量として
推定外乱負荷トルクＴｄ2 にモータの実速度ｖに設定係
数βを乗じた値を加算したものを採用しているが、モー
タの実速度ｖの関数をフィードバック値の一部として利
用するのは、制御系の振動を防止して安定性を向上させ
るためと、モータの暴走を防止するためである。このモ
ータの暴走は、トルクを与える対象がなかったり、トル
クを与えると対象が動いてしまう等の制御対象からの反
力がモータに加わらない場合には、外乱推定オブザーバ
で推定する推定外乱負荷トルクＴｄ2 が非常に小さな
り、その結果、力偏差Ｆｅｒｒが大きくなって減少する
ことにより発生する。そのため、速度に比例する値をフ
ィードバックすることによって、このような場合のモー
タの暴走を防止するものである。もし、必ず制御対象か
ら反力を受けるものであるときには、必ずしも速度に比
例する値をフィードバックする必要はない。In the above embodiment, the estimated disturbance load torque Td2 is added to the value obtained by multiplying the actual speed v of the motor by the set coefficient β, but the function of the actual speed v of the motor is fed back. It is used as a part of the value in order to prevent the vibration of the control system to improve the stability and to prevent the runaway of the motor. This motor runaway is the estimated disturbance load torque estimated by the disturbance estimation observer when the reaction force from the controlled object is not applied to the motor, such as there is no object to apply torque or the object moves when torque is applied. This is caused by the fact that Td2 is extremely small, and as a result, the force deviation Ferr becomes large and decreases. Therefore, by feeding back a value proportional to the speed, the runaway of the motor in such a case is prevented. If the reaction force is always received from the controlled object, it is not always necessary to feed back a value proportional to the speed.
【００３２】次に、前記力制御を行うための制御系の第
２の動作例について、図４に示すディジタルモータ制御
回路のプロセッサが実施する力制御の他の処理のフロー
チャートを用いて説明する。Next, a second operation example of the control system for performing the force control will be described with reference to the flowchart of another process of force control executed by the processor of the digital motor control circuit shown in FIG.
【００３３】なお、図４に示す第２の動作処理におい
て、推定外乱負荷トルクを求める外乱推定オブザーバ５
の処理は、前記図３に示したステップＳ１からステップ
Ｓ５までの前記第１の動作例中の処理と同様であり、外
乱推定オブザーバ５により求められた推定外乱負荷トル
クを用いて力制御を行う制御方法の点で相違している。
以下に示す制御例は、モータの最大許容速度に設定係数
を乗じた値に、推定外乱負荷トルクを加算した値をトル
ク指令値としてフィードバック制御を行うことにより、
安定性と、制御対象からの反力を受けないときの暴走を
防止するものである。In the second operation process shown in FIG. 4, the disturbance estimation observer 5 for obtaining the estimated disturbance load torque.
Processing is the same as the processing in the first operation example from step S1 to step S5 shown in FIG. 3, and force control is performed using the estimated disturbance load torque obtained by the disturbance estimation observer 5. They differ in the control method.
In the control example shown below, the value obtained by adding the estimated disturbance load torque to the value obtained by multiplying the maximum allowable speed of the motor by the setting coefficient is used as the torque command value to perform feedback control.
It is for stability and for preventing runaway when not receiving reaction force from the controlled object.
【００３４】図１の制御系では、制御装置１０から共有
メモリ１１を介して送られてくる力指令値Ｆｃと、位置
・速度検出器１５で検出されフィードバックされる実速
度ｖに設定係数βを乗じた値と、外乱推定オブザーバ５
から得られる推定外乱負荷トルクＴｄ２との間で、Ｆｃ
＝−Ｔｄ２＋β・ｖとなるように制御が行われる。つま
り、力偏差Ｆｅｒｒ（Ｆｅｒｒ＝Ｆｃ＋Ｔｄ２−β・
ｖ）を減少させるような制御が行われる。In the control system of FIG. 1, a setting coefficient β is set for the force command value Fc sent from the control device 10 via the shared memory 11 and the actual speed v detected by the position / speed detector 15 and fed back. Multiplied value and disturbance estimation observer 5
Between the estimated disturbance load torque Td2 obtained from
The control is performed so that == Td2 + β · v. That is, the force deviation Ferr (Ferr = Fc + Td2-β ·
Control is performed so as to decrease v).
【００３５】なお、推定外乱負荷トルクＴｄ２を力のフ
ィードバック値として利用する場合、この推定外乱負荷
トルクＴｄ２を力指令値Ｆｃから減じることになるが、
外乱推定オブザーバで求められる推定外乱負荷トルクＴ
ｄ２は力指令値Ｆｃと極性が逆になって求められるた
め、実際は力指令値Ｆｃに求められた推定外乱負荷トル
クＴｄ２を加算することによって実質的に差を求めるこ
とになる。When the estimated disturbance load torque Td2 is used as a force feedback value, the estimated disturbance load torque Td2 is subtracted from the force command value Fc.
Estimated disturbance load torque T obtained by the disturbance estimation observer
Since the polarity of d2 is obtained by reversing the polarity with respect to the force command value Fc, actually, the difference is substantially obtained by adding the estimated disturbance load torque Td2 obtained to the force command value Fc.
【００３６】ここで、トルクを与える制御対象がなかっ
たり、制御対象がトルクを与えると動いてしまうよう
な、モータが制御対象からの反力を受けない場合には、
実際に発生する実トルクは目標値であるトルク指令Ｔｃ
に達しない。このとき、モータの速度ｖはｖ＝（Ｆｃ＋
Ｔｄ２）／βとなる。なお、外乱推定オブザーバ５によ
り得られる推定外乱負荷トルクＴｄ２が、この実トルク
を推定するものとなる。この推定外乱負荷トルクＴｄ２
は、目標値より小さい値であるため、上記式の関係から
速度ｖは上昇する。この速度Ｖの上昇は、モータが提供
可能な最大速度である最大許容速度ｖｍａｘを超える場
合がある。Here, in the case where the motor does not receive a reaction force from the controlled object such that there is no controlled object that applies the torque or the controlled object moves when the torque is applied,
The actual torque that is actually generated is the target torque command Tc.
Does not reach At this time, the motor speed v is v = (Fc +
Td2) / β. The estimated disturbance load torque Td2 obtained by the disturbance estimation observer 5 estimates this actual torque. This estimated disturbance load torque Td2
Is smaller than the target value, the speed v increases from the relationship of the above equation. This increase in speed V may exceed the maximum allowable speed vmax, which is the maximum speed that the motor can provide.
【００３７】そこで、本発明においては、速度ｖが最大
許容速度ｖｍａｘを超える場合には、力指令値Ｆｃを調
整して新たな力指令値Ｆｃ２を求め、その力指令値Ｆｃ
２によって出力される速度が最大許容速度ｖｍａｘとな
るように制御することにより、暴走を防止する。つま
り、ｖ＝（Ｆｃ＋Ｔｄ２）／β＞ｖｍａｘの場合に、Ｆ
ｃをＦｃ２＝−Ｔｄ２＋β・ｖｍａｘに変更することに
より、速度ｖを最大許容速度ｖｍａｘとなるように制御
を行うものである（Ｆｃ＋Ｔｄ２＞０の場合）。Therefore, in the present invention, when the speed v exceeds the maximum allowable speed vmax, the force command value Fc is adjusted to obtain a new force command value Fc2, and the force command value Fc is obtained.
By controlling the speed output by 2 to be the maximum allowable speed vmax, runaway is prevented. That is, when v = (Fc + Td2) / β> vmax, F
By changing c to Fc2 = −Td2 + β · vmax, the speed v is controlled to be the maximum allowable speed vmax (when Fc + Td2> 0).
【００３８】そこで、この第２の動作例においても前記
第１の動作例と同様に、あらかじめ、外乱推定オブザー
バを構成する定数ｋ３，ｋ４，係数α，βをディジタル
モータ制御回路１２内に設置しておく。そして、このサ
ーボモータ１４の位置・速度を制御するときは、従来と
同様の位置・速度ループ処理を実行し、力制御を行うと
きは、位置・速度制御処理から図４に示す力制御処理に
切り替えて力制御を実行する。Therefore, in the second operation example as well, as in the first operation example, constants k3, k4 and coefficients α, β constituting the disturbance estimation observer are installed in advance in the digital motor control circuit 12. Keep it. Then, when controlling the position / velocity of the servomotor 14, the same position / velocity loop processing as in the related art is executed, and when performing force control, the position / velocity control processing is changed to the force control processing shown in FIG. Switch to execute force control.
【００３９】力制御に切り換えられると、ディジタルモ
ータ制御回路１２のプロセッサは所定周期（通常速度ル
ープ処理周期と同一の周期）毎に図４に示す処理を実行
する。When switched to the force control, the processor of the digital motor control circuit 12 executes the processing shown in FIG. 4 every predetermined cycle (the same cycle as the normal speed loop processing cycle).
【００４０】まず、制御装置１０から共有メモリ１１か
ら、力指令値Ｆｃとともにモータの最大許容速度ｖｍａ
ｘを読み込む（ステップＳ２１）。そして、前記第１の
動作例に示したように、図３のステップＳ１からステッ
プＳ５までの外乱推定オブザーバ５の処理によりトルク
フィードバック値である推定外乱負荷トルクＴｄ２を求
めるとともに実速度ｖを取り込む（ステップＳ２２）。
なお、図４では、図３のステップＳ１からステップＳ５
までの外乱推定オブザーバ５の処理については省略して
いる。First, the maximum permissible speed vma of the motor together with the force command value Fc is read from the shared memory 11 from the control device 10.
x is read (step S21). Then, as shown in the first operation example, the estimated disturbance load torque Td2 that is the torque feedback value is obtained by the processing of the disturbance estimation observer 5 from step S1 to step S5 of FIG. Step S22).
In FIG. 4, steps S1 to S5 of FIG.
The processing of the disturbance estimation observer 5 up to is omitted.
【００４１】ステップＳ２１で読み込んだ力指令値Ｆｃ
と、ステップＳ２２で求めた推定外乱負荷トルクＴｄ２
と、設定定数βにより速度ｖを表す（Ｆｃ＋Ｔｄ２）／
β（＝ｖ）の絶対値が最大許容速度ｖｍａｘを超えてる
か否かの判定を行う（ステップＳ２３）。Force command value Fc read in step S21
And the estimated disturbance load torque Td2 obtained in step S22
And the velocity v is represented by the setting constant β (Fc + Td2) /
It is determined whether or not the absolute value of β (= v) exceeds the maximum allowable speed vmax (step S23).
【００４２】図５に示す力指令値と速度との関係を示す
図において、力指令値Ｆｃにより力制御を始め、時点
（ａ）において例えば制御対象からの反力が減少する
と、推定外乱負荷トルクＴｄ２の減少が生じる。この推
定外乱負荷トルクＴｄ２の減少により速度ｖはｖ＝（Ｆ
ｃ＋Ｔｄ２）の関係から加速を始め、時点（ｂ）におい
てモータの持つ最大許容速度ｖｍａｘを超えることにな
る。ステップＳ２３の判定では、この（Ｆｃ＋Ｔｄ２）
／β（＝ｖ）の絶対値が最大許容速度ｖｍａｘを超える
か否かの判定を行い、最大許容速度ｖｍａｘを超えてい
ない場合には新たな力指令値Ｆｃ２として従来と同一の
力指令値Ｆｃを用い（ステップＳ２７）、最大許容速度
ｖｍａｘを超えた場合にはステップＳ２４〜ステップＳ
２６の処理により新たな力指令値Ｆｃ２を設定して力指
令値の変更を行う。In the diagram showing the relationship between the force command value and the speed shown in FIG. 5, when the force control is started by the force command value Fc and the reaction force from the controlled object decreases at time (a), the estimated disturbance load torque is reduced. A decrease in Td2 occurs. Due to the decrease in the estimated disturbance load torque Td2, the speed v becomes v = (F
Acceleration starts from the relationship of c + Td2) and exceeds the maximum allowable speed vmax of the motor at the time point (b). In the determination of step S23, this (Fc + Td2)
It is determined whether or not the absolute value of / β (= v) exceeds the maximum allowable speed vmax. If it does not exceed the maximum allowable speed vmax, the new force command value Fc2 is the same force command value Fc as the conventional one. Is used (step S27), and when the maximum permissible speed vmax is exceeded, steps S24 to S
By the process of 26, a new force command value Fc2 is set and the force command value is changed.
【００４３】最大許容速度ｖｍａｘを超えた場合、（Ｆ
ｃ＋Ｔｄ２）の値の正負を判定し（ステップＳ２４）、
（Ｆｃ＋Ｔｄ２）の値が正の値の場合には前記したよう
に新たな力指令値Ｆｃ２を（β・ｖｍａｘ−Ｔｄ２）に
変更して、速度ｖが最大許容速度ｖｍａｘとなるように
制御を行う（ステップＳ２５）。逆に、（Ｆｃ＋Ｔｄ
２）の値が負の値の場合には新たな力指令値Ｆｃ２を
（−β・ｖｍａｘ−Ｔｄ２）に変更して、速度ｖが最大
許容速度ｖｍａｘとなるように制御を行う（ステップＳ
２６）。When the maximum allowable speed vmax is exceeded, (F
Whether the value of (c + Td2) is positive or negative is determined (step S24),
When the value of (Fc + Td2) is a positive value, the new force command value Fc2 is changed to (β · vmax-Td2) as described above, and control is performed so that the speed v becomes the maximum allowable speed vmax. (Step S25). Conversely, (Fc + Td
If the value of 2) is a negative value, the new force command value Fc2 is changed to (-β · vmax-Td2), and control is performed so that the speed v becomes the maximum allowable speed vmax (step S).
26).
【００４４】図５において、速度ｖが最大許容速度ｖｍ
ａｘを超えた時点（ｂ）に対応する時点（ｃ）におい
て、この力指令値のＦｃからＦｃ２への変更が行われ
る。なお、図５ではＦｃ２が（β・ｖｍａｘ−Ｔｄ２）
に変更された場合を示している。In FIG. 5, the speed v is the maximum allowable speed vm.
At the time point (c) corresponding to the time point (b) that exceeds ax, the force command value is changed from Fc to Fc2. In FIG. 5, Fc2 is (β · vmax-Td2)
It has been changed to.
【００４５】この後のステップＳ２９〜ステップＳ３３
の処理は、前記第１の動作例において図３で示したステ
ップＳ６〜ステップＳ１０と同様の処理となる。つま
り、前記ステップＳ２５〜ステップＳ２７で求めた力指
令値Ｆｃ２から推定外乱負荷トルクＴｄ２とステップＳ
２１で読み込んだ最大許容速度ｖｍａｘに設定係数βを
乗じた値とを減じて力偏差Ｆｅｒｒ（＝Ｆｃ＋Ｔｄ２−
β・ｖ）を求める（ステップＳ２９）。Subsequent steps S29 to S33
The processing of is the same as the processing of steps S6 to S10 shown in FIG. 3 in the first operation example. That is, the estimated disturbance load torque Td2 and step S2 are calculated from the force command value Fc2 obtained in steps S25 to S27.
The maximum allowable speed vmax read in step 21 is subtracted from the value obtained by multiplying the setting coefficient β by the force deviation Ferr (= Fc + Td2-
β · v) is obtained (step S29).
【００４６】次に、積分器として作用するアキュムレー
タＳｕｍに上記力偏差Ｆｅｒｒに積分定数ｋ１、及びこ
の図４に示す処理の周期Ｔｓを乗じた値を加算すること
によって積分処理を実行する（図１中の要素１の積分処
理）（ステップＳ３０）。そして、該アキュムレータＳ
ｕｍの値と上記力偏差Ｆｅｒｒに比例定数ｋ２を乗じた
値を加算してトルク指令Ｔｃを求める（ステップＳ３
１）。すなわち、図１の項１の処理を実行することにな
る。こうして求められたトルク指令Ｔｃを次の周期で使
用するためにレジスタＲ（Ｔｃ）に格納するとともに電
流ループに引き渡し（ステップＳ３２，ステップＳ３
３）、当該周期の処理を終了する。Next, the integration processing is executed by adding the accumulator Sum acting as an integrator to the force deviation Ferr and the product of the integration constant k1 and the processing cycle Ts shown in FIG. 4 (FIG. 1). (Integration processing of element 1 in the inside) (step S30). And the accumulator S
The torque command Tc is obtained by adding the value of um and the force deviation Ferr to the value obtained by multiplying the proportional constant k2 (step S3).
1). That is, the processing of item 1 in FIG. 1 is executed. The torque command Tc thus obtained is stored in the register R (Tc) so as to be used in the next cycle, and is delivered to the current loop (steps S32 and S3).
3), the process of the cycle is finished.
【００４７】ここで、例えば、図５の時点（ｄ）におい
て、制御対象からの反力が増加すると、推定外乱負荷ト
ルクＴｄ２の増加する。この推定外乱負荷トルクＴｄ２
の増加により速度ｖはｖ＝（Ｆｃ＋Ｔｄ２）の関係から
減速し、最大許容速度ｖｍａｘよ小さくなる（時点
（ｅ））。このため、力指令値Ｆｃ２は時点（ｆ）では
じめの力指令値Ｆｃに変更されることになる。Here, for example, at time point (d) in FIG. 5, when the reaction force from the controlled object increases, the estimated disturbance load torque Td2 increases. This estimated disturbance load torque Td2
The speed v is decelerated from the relationship of v = (Fc + Td2) due to the increase of, and becomes smaller than the maximum allowable speed vmax (time point (e)). Therefore, the force command value Fc2 is changed to the initial force command value Fc at the time point (f).
【００４８】以降、力制御に切り換えられている限り、
上記図４に示す処理を所定周期毎に実行する。After that, as long as the force control is switched to,
The process shown in FIG. 4 is executed every predetermined period.
【００４９】上記第１の動作例、及び第２の動作例にお
いては、フィードバック量として推定外乱負荷トルクＴ
ｄ２にモータの実速度ｖ、あるいは最大許容速度ｖｍａ
ｘに設定係数βを乗じた値を加算したものとし、モータ
の実速度ｖの関数をフィードバック値の一部に利用する
のは、制御系の振動を防止して安定性を向上させるとと
もに、制御対象からの反力がモータに加わらない場合の
モータの暴走を防ぐためである。つまり、制御対象から
の反力がモータに加わらない場合には、外乱推定オブザ
ーバで推定する推定外乱負荷トルクＴｄ２は非常に小さ
くなり、その結果、力偏差Ｆｅｒｒが大きくなり減少す
ることがないことから、モータが暴走することになる。
そこで、速度に比例する値をフィードバックすることに
より、あるいは一定値にクランプした速度に比例する値
をフィードバックすることにより、モータの暴走を防止
するものである。従って、制御対象からの反力を必ず受
けるような制御系の場合には、必ずしも速度やクランプ
した速度にに比例する値をフィードバックする必要はな
い。In the first operation example and the second operation example, the estimated disturbance load torque T is used as the feedback amount.
d2 is the actual speed v of the motor or the maximum allowable speed vma
It is assumed that x is a value obtained by multiplying the setting coefficient β and the function of the actual speed v of the motor is used as a part of the feedback value in order to prevent the vibration of the control system and improve the stability. This is to prevent runaway of the motor when the reaction force from the target is not applied to the motor. That is, when the reaction force from the controlled object is not applied to the motor, the estimated disturbance load torque Td2 estimated by the disturbance estimation observer becomes extremely small, and as a result, the force deviation Ferrr does not increase and decreases. , The motor will run out of control.
Therefore, by feeding back a value proportional to the speed or by feeding back a value proportional to the speed clamped to a constant value, the runaway of the motor is prevented. Therefore, in the case of a control system that always receives a reaction force from the controlled object, it is not always necessary to feed back a value proportional to the speed or the clamped speed.
【００５０】[0050]
【発明の効果】以上説明したように、本発明によれば、
外乱推定オブザーバによってモータに加わる負荷トルク
を推定して、該推定負荷トルクをフィードバックして該
推定負荷トルクが指令値された力と一致するようにフィ
ードバック制御するため、力センサ等を必要とせず、簡
単にセンサにより駆動される被駆動体による力制御を実
行することができるとともに、安価な力制御装置を得る
ことができる。そのため、従来、力制御ができないこと
から駆動源を油圧シリンダにして油圧による制御しかで
きなかった産業機械等の分野においてもモータを駆動源
として使用し、力制御を簡単に導入することができる。As described above, according to the present invention,
The load torque applied to the motor is estimated by the disturbance estimation observer, and the estimated load torque is fed back to perform feedback control so that the estimated load torque matches the commanded force. Therefore, a force sensor or the like is not required, It is possible to easily execute force control by the driven body driven by the sensor and obtain an inexpensive force control device. Therefore, the force control can be easily introduced by using the motor as the drive source even in the field of industrial machines or the like, where conventionally the force control cannot be performed and the hydraulic cylinder is used as the drive source and only hydraulic control can be performed.
【図１】本発明を適用した一実施例の力制御のブロック
線図である。FIG. 1 is a block diagram of force control according to an embodiment of the present invention.
【図２】同一実施例を実施するサーボモータ制御系の要
部ブロック図である。FIG. 2 is a block diagram of a main part of a servo motor control system that implements the same embodiment.
【図３】同一実施例における力制御の一処理例のフロー
チャートである。FIG. 3 is a flowchart of a processing example of force control in the same embodiment.
【図４】同一実施例における力制御の他の処理例のフロ
ーチャートである。FIG. 4 is a flowchart of another processing example of force control in the same embodiment.
【図５】力指令値と速度との関係を示す図である。FIG. 5 is a diagram showing a relationship between a force command value and speed.
５ 外乱推定オブザーバ １０ 制御装置 １１ 共有メモリ １２ ディジタルモータ制御回路 １３ サーボアンプ １４ サーボモータ １５ 位置・速度検出器 5 Disturbance estimation observer 10 Control device 11 Shared memory 12 Digital motor control circuit 13 Servo amplifier 14 Servo motor 15 Position / speed detector
Claims (4)
からの負荷を外乱推定オブザーバによって推定し、該推
定外乱負荷トルクをフィードバックして該推定外乱負荷
トルクが力指令値と一致するようにフィードバック制御
する外乱負荷推定による力制御方法。1. A disturbance estimation observer estimates a load from an external environment applied to a driven pair driven by a motor, and the estimated disturbance load torque is fed back so that the estimated disturbance load torque matches a force command value. Force control method by controlling disturbance load control.
に設定係数を乗じた値を加算した値を力のフィードバッ
ク値とする請求項１記載の外乱負荷推定による力制御方
法。2. The force control method by disturbance load estimation according to claim 1, wherein a value obtained by adding a value obtained by multiplying the estimated disturbance load torque to a motor speed by a setting coefficient is used as a force feedback value.
許容速度に設定係数を乗じた値を加算し、該加算値によ
り力の指令値をクランプしてモータの回転速度が最大許
容値以上にならないよう制御する請求項１記載の外乱負
荷推定による力制御方法。3. The estimated disturbance load torque is added to a value obtained by multiplying the maximum allowable speed of the motor by a setting coefficient, and the force command value is clamped by the added value so that the rotation speed of the motor does not exceed the maximum allowable value. The force control method by disturbance load estimation according to claim 1, wherein
されるトルク指令値とモータの実速度から外乱負荷トル
クを推定する請求項１，２または請求項３記載の外乱負
荷推定による力制御方法。4. The force control method according to claim 1, 2 or 3, wherein the disturbance estimation observer estimates the disturbance load torque from a torque command value commanded to the motor and an actual speed of the motor.
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP5255266A JPH06339292A (en) | 1993-04-02 | 1993-09-20 | Force controlling method by estimation of disturbance load |
| PCT/JP1994/000547 WO1994024621A1 (en) | 1993-04-02 | 1994-04-01 | Method of force control by disturbance load estimation |
| US08/347,301 US5734242A (en) | 1903-09-29 | 1994-04-01 | Force control method based on disturbance load estimation |
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP9841493 | 1993-04-02 | ||
| JP5-98414 | 1993-04-02 | ||
| JP5255266A JPH06339292A (en) | 1993-04-02 | 1993-09-20 | Force controlling method by estimation of disturbance load |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH06339292A true JPH06339292A (en) | 1994-12-06 |
Family
ID=26439588
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP5255266A Pending JPH06339292A (en) | 1903-09-29 | 1993-09-20 | Force controlling method by estimation of disturbance load |
Country Status (3)
| Country | Link |
|---|---|
| US (1) | US5734242A (en) |
| JP (1) | JPH06339292A (en) |
| WO (1) | WO1994024621A1 (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO1999009641A1 (en) * | 1997-08-13 | 1999-02-25 | Fanuc Ltd | Controller and control method for pressing by servo motor |
| JP2000141262A (en) * | 1998-11-06 | 2000-05-23 | Fanuc Ltd | Robot control device |
| JP2018186627A (en) * | 2017-04-25 | 2018-11-22 | アイシン精機株式会社 | Rotary electric machine control device |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6326758B1 (en) * | 1999-12-15 | 2001-12-04 | Reliance Electric Technologies, Llc | Integrated diagnostics and control systems |
| JP3773686B2 (en) * | 1999-02-17 | 2006-05-10 | パイオニア株式会社 | Servo control device and servo control method |
| JP3441680B2 (en) | 1999-07-28 | 2003-09-02 | ファナック株式会社 | Ejector control device for injection molding machine |
| US7301296B1 (en) * | 2001-07-23 | 2007-11-27 | Rockwell Automation Technologies, Inc. | Integrated control and diagnostics system |
| JP3526022B2 (en) * | 2000-03-06 | 2004-05-10 | 株式会社安川電機 | Oscillation criticality detection method for servo control system |
| JP2002202802A (en) * | 2000-12-28 | 2002-07-19 | Seiko Instruments Inc | Disturbance estimation type control system, gas compressor control system, and designing method for disturbance estimation type control system |
| US8463441B2 (en) | 2002-12-09 | 2013-06-11 | Hudson Technologies, Inc. | Method and apparatus for optimizing refrigeration systems |
| US7345443B2 (en) * | 2004-03-26 | 2008-03-18 | Kabushiki Kaisha Yaskawa Denki | Motor control apparatus |
| US7627440B2 (en) * | 2007-08-28 | 2009-12-01 | Rockwell Automation Technologies, Inc. | Inertia and load torque estimating method and apparatus |
| LT6113B (en) * | 2013-05-13 | 2015-02-25 | Uždaroji Akcinė Bendrovė "My Research" | A servomechanism with proportionally controlled impact force |
| CN104331084B (en) * | 2014-09-30 | 2017-05-03 | 中国运载火箭技术研究院 | Pneumatic rudder deflection range calculation method based on direction rudder roll control strategy |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61266083A (en) * | 1985-05-20 | 1986-11-25 | Canon Inc | Servo circuit |
| JPH02111278A (en) * | 1988-10-17 | 1990-04-24 | Fanuc Ltd | Servo control method |
| JPH02297603A (en) * | 1989-05-12 | 1990-12-10 | Fanuc Ltd | Servo control system using sliding mode and also disturbance estimation observer |
| JPH04265682A (en) * | 1991-02-21 | 1992-09-21 | Fuji Electric Co Ltd | Servo amplifier provided with function for limiting upper limit rotational speed |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4555757A (en) * | 1983-05-05 | 1985-11-26 | Westinghouse Electric Corp. | Interface system for hybrid control loops |
| JP2535334B2 (en) * | 1986-10-29 | 1996-09-18 | フアナツク株式会社 | Digital negative feedback control system |
| GB2220580B (en) * | 1988-07-15 | 1992-08-12 | Terence Malcolm Brooking | Apparatus for playing a game of skill |
| US5374684A (en) * | 1989-01-24 | 1994-12-20 | The Dow Chemical Company | Method for making aggregates or clusters of water-swellable polymers having increased hydration rate over unassociated water-swellable polymers |
| JP2906256B2 (en) * | 1989-09-25 | 1999-06-14 | セイコーインスツルメンツ株式会社 | Servo control device |
| US5101145A (en) * | 1991-02-01 | 1992-03-31 | Allen-Bradley Company, Inc. | Velocity proportional integral regulator with negative feedforward to control response of torque disturbances |
| JP2846504B2 (en) * | 1991-05-30 | 1999-01-13 | 三菱電機株式会社 | Motor control device |
-
1993
- 1993-09-20 JP JP5255266A patent/JPH06339292A/en active Pending
-
1994
- 1994-04-01 WO PCT/JP1994/000547 patent/WO1994024621A1/en active Application Filing
- 1994-04-01 US US08/347,301 patent/US5734242A/en not_active Expired - Fee Related
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61266083A (en) * | 1985-05-20 | 1986-11-25 | Canon Inc | Servo circuit |
| JPH02111278A (en) * | 1988-10-17 | 1990-04-24 | Fanuc Ltd | Servo control method |
| JPH02297603A (en) * | 1989-05-12 | 1990-12-10 | Fanuc Ltd | Servo control system using sliding mode and also disturbance estimation observer |
| JPH04265682A (en) * | 1991-02-21 | 1992-09-21 | Fuji Electric Co Ltd | Servo amplifier provided with function for limiting upper limit rotational speed |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO1999009641A1 (en) * | 1997-08-13 | 1999-02-25 | Fanuc Ltd | Controller and control method for pressing by servo motor |
| US6486629B2 (en) | 1997-08-13 | 2002-11-26 | Fanuc Ltd. | Method for controlling force application with servo motor and appartus therewith |
| JP2000141262A (en) * | 1998-11-06 | 2000-05-23 | Fanuc Ltd | Robot control device |
| JP2018186627A (en) * | 2017-04-25 | 2018-11-22 | アイシン精機株式会社 | Rotary electric machine control device |
Also Published As
| Publication number | Publication date |
|---|---|
| WO1994024621A1 (en) | 1994-10-27 |
| US5734242A (en) | 1998-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP0566741B1 (en) | Abnormal load detecting method | |
| EP0417312B1 (en) | Feedforward control unit for servomotor | |
| KR940003005B1 (en) | Arrangement for speed regulation of electric motor | |
| KR0144650B1 (en) | Method of detecting and regulating load on servomotor | |
| JPH06339292A (en) | Force controlling method by estimation of disturbance load | |
| EP1505463B1 (en) | Position control device and position control method for machine tools | |
| WO1995005703A1 (en) | Apparatus and method for controlling a servomotor | |
| US5986422A (en) | Control mode changing over method for servo control system | |
| KR970002258B1 (en) | Method of servomotor control | |
| US5936369A (en) | Method of estimating disturbance load on servomotor | |
| US5637969A (en) | Vibration control method | |
| JP2875646B2 (en) | Backlash acceleration correction device | |
| JPH10249597A (en) | Motor torque controlling method in press machine and press machine therefor | |
| JP3322826B2 (en) | Pressurization control method and apparatus by servo motor | |
| JP3481004B2 (en) | Backlash compensation method using disturbance observer | |
| US6122998A (en) | Force control method for a bar feeder of a lathe | |
| EP0192775B1 (en) | System for controlling line tracking | |
| JPH0392911A (en) | Robot control method for sliding mode control | |
| KR970002259B1 (en) | Method for controlling servomotor | |
| JP2788360B2 (en) | Motor speed control device | |
| JPH06131050A (en) | Method for detecting collision of movable part driven by servo motor | |
| EP0604663A1 (en) | Method for estimating inertia and disturbance torque, and method for detecting abnormal load | |
| JP3308656B2 (en) | Servo motor control method | |
| JPH10105247A (en) | Overshoot preventing method for servomotor | |
| JPH079204A (en) | Force controlling method in bar feeder of lathe |