JPH02297611A

JPH02297611A - Sliding mode control system including feedforward of speed and acceleration

Info

Publication number: JPH02297611A
Application number: JP11751889A
Authority: JP
Inventors: Nobutoshi Torii; 信利鳥居; Akira Nihei; 亮二瓶; Tetsuro Kato; 哲朗加藤
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1989-05-12
Filing date: 1989-05-12
Publication date: 1990-12-10

Abstract

PURPOSE:To obtain a servo system without generating overshoot and delay by stably converging the servo system even when a parameter for inertia, etc., fluctuates by performing feedforward control by applying a sliding mode. CONSTITUTION:A digital servo circuit 3 consisting of a digital signal processor, etc., performs the feedback control of the position, the speed, and the current of a servo motor at every axis of a robot 5. Also, a feedback signal register 4 on which the feedback value of the driving current and feedback pulse as the amount of travel theta of each servo motor in the robot 5 are written is provided. In the control of the servo motor, a system is adapted to the fluctuation of inertia by varying a torque command value corresponding to that of all the inertia applied on the servo motor by performing sliding mode control. Thereby, it is possible to perform the feedforward control for speed and acceleration in the control of the servo motor where inertia remarkably fluctuates.

Description

【発明の詳細な説明】産業上の利用分野本発明は、ロボット等のυ１１１１対象のパラメータ変
動が大きい制御対象に対するスライディングモード制御
方式に関する。特に、速度及び加速度のフィードフォア
ードυノ御を行う制御系におけるスライディングモード
制御方式に關する。DETAILED DESCRIPTION OF THE INVENTION Field of the Invention The present invention relates to a sliding mode control method for a controlled object such as a robot that has large parameter fluctuations of υ1111. In particular, it concerns a sliding mode control system in a control system that performs feedforward control of velocity and acceleration.

従来の技術第３図は、位置に対し比例（Ｐ）制御、速度に対し比例
、積分（ＰＩ）制御を行うロボット等のサーボモータの
制御において、フィードフォアード制御を行うときのブ
ロック線図である。Conventional technology Fig. 3 is a block diagram when performing feed-forward control in controlling a servo motor of a robot, etc., which performs proportional (P) control for position and proportional and integral (PI) control for speed. .

図中、伝達関数１０のＫＰは位置ループにおける比例ゲ
イン、伝達関数１２は速度ループにおける伝達関数で、
Ｋ１は積分定数、Ｋ２は比例定数である。また、伝達関
数１４はモータの伝達関数で、Ｊはイナーシャであり、
伝達関数１６は速度すを積分して位置θを算出する伝達
関数である。In the figure, KP of transfer function 10 is the proportional gain in the position loop, transfer function 12 is the transfer function in the velocity loop,
K1 is an integral constant, and K2 is a proportional constant. Further, the transfer function 14 is the transfer function of the motor, J is the inertia,
The transfer function 16 is a transfer function that calculates the position θ by integrating the velocity S.

また、伝達関数１８は位置指令値θｒを微分して速度指
令値に加算する速度のフィードフォアードの項、伝達関
数２０は伝達関数１８の出力をさらに微分しイナーシャ
を乗じて加速度を算出しトルク指令値に加算する加速度
のフィードフォアードの項である。Further, the transfer function 18 is a speed feedforward term that differentiates the position command value θr and adds it to the speed command value, and the transfer function 20 further differentiates the output of the transfer function 18 and multiplies it by inertia to calculate acceleration and torque command This is the feed-forward term of acceleration that is added to the value.

位置指令値θｒからフィードバックされた現在位置を減
算し、位置偏差ε（＝θｒ−θ）を求め、これに比例定
数ＫＰを乗じて速度指令値を求め、さらに位置指令値θ
ｒを微分した速度フィードフォアード項の出力を加算し
て、速度フィードフォアートされた速度指令値からフィ
ードバックされた実速度υを減じた値を速度ループによ
ってＰＩ制御を行い、伝達関数１２の出力と速麿フイー
ドフォアード項の出力をさらに微分し、イナーシトＪを
乗じて得られる加速度のフイードフオアード項の出力を
加算してトルク指令値Ｔとして求め、該トルク指令値Ｔ
に対応する電流をモータに流し、モータを駆動する。Subtract the current position fed back from the position command value θr to find the position deviation ε (= θr - θ), multiply this by the proportionality constant KP to find the speed command value, and then calculate the position command value θ
The output of the speed feed forward term obtained by differentiating r is added, and the value obtained by subtracting the fed back actual speed υ from the speed command value subjected to speed feed forwarding is subjected to PI control using a speed loop, and the output of the transfer function 12 and The output of the feed-forward term of the acceleration is further differentiated, and the output of the feed-forward term of the acceleration obtained by multiplying by the inert J is added to obtain the torque command value T.
A current corresponding to the current is applied to the motor to drive the motor.

以上がフィードフオアード制御が行われるモータ制御の
概略の動作である。The above is the general operation of motor control in which feedforward control is performed.

発明が解決しようとする課題上述したフイードフオアード制御の場合、加速度のフイ
ードフオアード項２０にイナーシャＪが関係しているの
で、ロボットのように、このイナーシャＪが大きく変動
するものにおいては、加速度のフィードフォアードの項
２０の出力がイナーシャＪの変動に応じて大きく変動す
ることになり、大きなオーバーシュート等が生じ、実現
不可能であった。Problems to be Solved by the Invention In the case of the above-mentioned feedforward control, inertia J is related to the feedforward term 20 of acceleration. , the output of the acceleration feedforward term 20 would vary greatly depending on the variation of the inertia J, resulting in a large overshoot and the like, making it impossible to realize.

そこで、本発明の目的は、イ少−シャが大きく変動する
サーボモータの制御において、速度及び加速度のフィー
ドフオアード制御を可能にすることにある。SUMMARY OF THE INVENTION An object of the present invention is to enable feedforward control of speed and acceleration in the control of a servo motor in which the initial force fluctuates greatly.

課題を解決するための手段本発明は、サーボモータの制御において、スライディン
グモード制御を行うことによって、サーボモータに加わ
る全イナーシャの変動に応じてトルク指令値を変えてイ
ナーシャ変動に適応させることにより、速度及び加速度
のフィードフオアード制御を可能にした。Means for Solving the Problems The present invention performs sliding mode control in controlling a servo motor, and changes the torque command value in accordance with fluctuations in the total inertia applied to the servo motor to adapt to fluctuations in inertia. Enables feedforward control of velocity and acceleration.

作　　用スライディングモード制御を行うことにより、イナーシ
ャの変動があっても切換え面に収束するようにトルク指
令値が切換えられ、イナーシャ変動に適応させるから、
速度、加速度のフィードフォアード制御を行ってもオー
バーシュート等のイナーシャ変動の影響をなくし、フィ
ードフオアード制御によりサーボモータの遅れをなくす
ことができる。By performing sliding mode control, the torque command value is switched so that it converges on the switching surface even if there is a fluctuation in inertia, and the torque command value is adapted to the fluctuation in inertia.
Feedforward control of speed and acceleration can eliminate the effects of inertia fluctuations such as overshoot, and feedforward control can eliminate servo motor delays.

実施例第３図においてモータの入出力を見ると、次の第（１）
式が成立する。Looking at the input and output of the motor in Fig. 3 of the embodiment, the following (1)
The formula holds true.

Ｊθ＝Ｔ　　　　　　　　・・・・・・（１）ε＝θｒ
−θ　　　　　　・・・・・・（２）と＝θｒ−１）　
　　　　　　・・・・・・（３）ご＝θｒ−θ　　　　
　　・・・・・・（４）なお、き、υｒ、ｄは微分を、
と、θｒ、θは２同機分を意味し、υｒは速度フイード
フオアードを表わす。Jθ=T ・・・・・・(1) ε=θr
−θ ・・・・・・(2) and =θr−1)
・・・・・・(3) Go=θr−θ
・・・・・・(4) In addition, ki, υr, d are the differentials,
, θr and θ mean two aircrafts, and υr indicates the speed feed.

ここで、切換え面Ｓとして、従来の切換え面Ｓ＝Ａ＋Ｃ
・εに積分要素を付加し、第（５）式で示す切換え面と
する。Here, as the switching surface S, the conventional switching surface S=A+C
- Add an integral element to ε to obtain the switching surface shown in equation (5).

Ｓ＝Ａ＋Ｃ・ε＋Ｄ−ｆ（ｔ＋ｃ・ε）　　　・・・・
・・（５）なお、第（５）式において、Ｃは位置偏差ε
と速度偏差との時定数、Ｄは積分要素の時定数としての
定数である。S=A+C・ε+D−f(t+c・ε)...
...(5) In equation (5), C is the positional deviation ε
and the speed deviation, and D is a constant as the time constant of the integral element.

また、トルク指令値（制御対象に入力されるトルク指令
値）王を次の第（６）式であるとする。Further, it is assumed that the torque command value (torque command value input to the controlled object) is expressed by the following equation (6).

Ｔ−ＪＯωｃ　−＃＋ＪＯ・ωｃ−Ｃ・ε＋Ｔ１　　・
・・（６）なお、第（６）式で、ＪＯは制御対象の予想
される最小イナーシャ、Ｔ１は切換え入力値であり、ω
Ｃは時定数としての定数である。T−JOωc −#+JO・ωc−C・ε+T1 ・
...(6) In equation (6), JO is the expected minimum inertia of the controlled object, T1 is the switching input value, and ω
C is a constant as a time constant.

リアプノフ関数候補として次の第（７）式を考える。Consider the following equation (7) as a Lyapunov function candidate.

Ｖ−（１／２＞・Ｓ２　　　　　　・・・・・・（７）
上記リアブノフ関敗Ｖは常に正で最小値が「０」であり
、もし、！く０であればリアブノフ関数■は最小値「０
」に収束する。また、これにより切換え面Ｓは常に収束
し、応答性がＳ−０の一定の応答関数によって決定され
る。V-(1/2>・S2...(7)
The above-mentioned Lyabunov score V is always positive and the minimum value is "0", and if! If 0, the Lyabunov function ■ has the minimum value ``0
”. Furthermore, the switching surface S always converges, and the response is determined by a constant response function of S-0.

上記第（５）式より＄−と＋（Ｃ＋Ｄ）＃＋Ｄ−Ｃ・ε　・・・（８）また
、第（１）式と第（４）式よりＣ−θｒ−Ｔ／Ｊ　　　　　　　　　・・・・・・（９
）上記第（９）式に第（６）式を代入して整理すると、
Ａ　−（９ｒ−ＪＯωｃ−Ａ／Ｊ−ＪＯωｃ−Ｃ・ε／
Ｊ−Ｔ１／Ｊ・・・・・・（１０）上記第（１０）式を第（８）式に代入し整理すると、５
−（Ｃ＋Ｄ−ωｃ−ＪＯ／Ｊ）・乙＋（Ｄ−Ｃ−ＪＯ・
ωｃ　−Ｃ／Ｊ　）・ε−ＴＩ／Ｊ＋θｒ　　　　　　
　・・・・・・（１１）上記第（５）式を乙について解
くと、Ａ＝Ｓ−Ｃ・　ε−Ｄ”　　ｆ　（ａ＋ｃ　・　ε）　
　　　　　　　　・・・・・・　く１２）上記第（１２
）式を第（１１）式に代入し整理すると、＄−（Ｃ＋Ｄ
−ωｃ−ＪＯ／Ｊ）・Ｓ −４０２・ε＋（（）　−Ｃ＋（）２−ωＣ−ＤＪＯ／
Ｊ）−ｆ＜Ａ＋Ｃ−ε）＋ＴＩ／Ｊ−θｒ］　　・（１
３）第（７）式を微分すると、ｖ＝ｓ−８・・・・・・（１４）第（１４）式の＄に第（１３）式を代入するとＶ＝（Ｃ
＋Ｄ−ωｃ−ＪＯ／Ｊ）・Ｓ２−［Ｃ２・ε＋（Ｄ　−
Ｃ＋Ｄ２−ωｃ−ＤＪＯ／Ｊ）−ｆ（＃十Ｇ・ε）＋Ｔ
１／Ｊ−θｒ］　−８・（１５）そこで、定数ωＣを第
（１６）式に示すように決定する。From the above equation (5), $- and +(C+D)#+D-C・ε...(8) Also, from equations (1) and (4), C-θr-T/J... ...(9
) By substituting equation (6) into equation (9) above, we get
A −(9r-JOωc-A/J-JOωc-C・ε/
J-T1/J... (10) Substituting the above equation (10) into equation (8) and rearranging, 5
-(C+D-ωc-JO/J)・Otsu+(D-C-JO・
ωc −C/J )・ε−TI/J+θr
・・・・・・(11) Solving the above equation (5) for B, A=S−C・ε−D” f (a+c・ε)
・・・・・・ Ku12) Above (12)
) is substituted into equation (11) and rearranged, we get $-(C+D
−ωc−JO/J)・S −402・ε+(() −C+()2−ωC−DJO/
J)-f<A+C-ε)+TI/J-θr] ・(1
3) Differentiating equation (7), v=s-8...(14) Substituting equation (13) for $ in equation (14), we get V=(C
+D-ωc-JO/J)・S2-[C2・ε+(D −
C+D2-ωc-DJO/J)-f(#10G・ε)+T
1/J-θr] −8·(15) Therefore, the constant ωC is determined as shown in equation (16).

ωｃ＝　（Ｃ＋Ｄ）　−、ｌａｘ　／ＪＯ−・−・−（
１６）なお、Ｊ　ＩＩａＸは制御対象において想定され
る最大イナーシャである。ωc= (C+D) −, lax /JO−・−・−(
16) Note that J IIaX is the maximum inertia expected in the controlled object.

第（１６）式に示すように定数ωＣを決定すると、第（
１５）式の右辺第１項は、（Ｃ＋Ｄ−ωｃ　−ＪＯ／Ｊ　）・Ｓ２＝　（Ｃ＋Ｄ　
−（Ｃ（−Ｄ）　・Ｊｍａｘ／Ｊ）　・Ｓ２＜　０・・
・・・・（１７）となるＬＩｎ＋ａｘ／Ｊ　　＞　　１であるから）。When the constant ωC is determined as shown in equation (16), the
15) The first term on the right side of equation is (C+D-ωc -JO/J)・S2= (C+D
-(C(-D) ・Jmax/J) ・S2< 0...
(17) Since LIn+ax/J > 1).

故に、リアプノフ関数Ｖの微分Ｍを常に９〈０とするに
は、第（１５〉式より、第（１８）式が成立するように
切換え人力Ｔ１を決定すればよい。Therefore, in order to always make the differential M of the Lyapunov function V 9<0, it is sufficient to determine the switching human power T1 so that the equation (18) is satisfied from the equation (15).

−［Ｃ２・ε＋（Ｄ−Ｃ＋Ｄ２−ωｃ−Ｄ−ＪＯ／Ｊ）
−ｆ（ａ＋Ｃ−ε）＋Ｔ１／Ｊ−θｒ］　・Ｓ　＜　０
−（１８）そこで、切換え人力Ｔ１をεの関数に１（ε
）とｆ　（／；＋Ｃ・ε）の関数に２　＜ｆ　（ｊ−＋
　Ｃ・ε））及び加速度フィードフォアードθｒの関数
に３（Ｑｒ）に分け、第（１９）式のとおりとする。−[C2・ε+(D−C+D2−ωc−D−JO/J)
-f(a+C-ε)+T1/J-θr] ・S < 0
−(18) Therefore, the switching human power T1 is set as a function of ε by 1(ε
) and f (/;+C・ε), 2 < f (j−+
C·ε)) and the acceleration feedforward θr are divided into 3 (Qr) functions as shown in Equation (19).

Ｔ１　＝　Ｋｌ　（５）＋に２　（ｆ　（ａ＋ｃ−ε）
　）＋に３　（Ｑｒ）・・・・・・（１９）上記第（１８）式が成立するには、（ｉ）　　Ｓ　≧０のときＴＩ　　＝　Ｋｌ　（ε）　十に２　＜ｆ　（／、＋Ｃ
−ε））＋に３　（６ｒ）〉　−Ｃ２・Ｊ・ε−［Ｊ　
　＜Ｄ−Ｃ＋Ｄ２　）　　−ωｃ−Ｄ−ＪＯ］・／（Ｑ
＋Ｃ−ε）＋Ｊ４ｒ　　　　　　−・・−＜２０）であ
ればよいので、（イ）　Ｋ１（ε）に関して ε≧０のときＫｌ（ε）＝−Ｃ−ＪＯ・ε　　　　・・・・・・（２
１）ε〈ＯのときＫｌ（ε）＝−Ｃ−Ｊｍａｘ−ε　　　・・−・−・（
２２）（ロ）　　Ｋ２　＜ｆ　（ａ＋Ｃ・ε））に関し
てｆ（６十Ｃ・ε）≧Ｏのときに２（ｆ（Ａ＋Ｃ・ε）　）　＝−［ＪＯ（Ｄ−Ｃ＋０
２）　−ＪＯ・ωＣ−Ｄ］　　　　・・・・・・（２３
）ｆ（６十Ｃ・ε＞＜０のときに２＜ｆ（Ａ＋Ｃ−ε））＝　−［Ｊｍａｘ　−（Ｄ−
Ｃ−１−Ｃ２）　−ＪＯ・ωＣ−Ｄ］　　　　・・・・
・・（２４）（ハ）　Ｋ３（Ｑｒ）に関して（加速度フ
ィードフオアードに関して）４ｒ≧Ｏのときに３（Ｑｒ）＝Ｊｍａｘ−θｒ　　　　　　−・・−（
２ｂ）Ｑｒ＜Ｑのときに３（Ｑｒ）＝ＪＯ−θｒ　　　　　　　−・・・（２
Ｇ）（ｉｉ）Ｓ＜ＯのときＴＩ　　＝　Ｋ１　（ε）＋に２　（ｆ　（Ａ＋Ｃ・ε
））〈−Ｃ２・Ｊ・ε−［Ｊ　（Ｃ−Ｄ＋０２）−ＪＯ
−ωｃ−Ｄ］　−ｆ　（ａ＋ｃ・５）＋Ｊθｒ　　　−
（２７＞であればよいので、（イ）　Ｋ１（ε）に関して ε≧０のときＫｌ（ε）　−−Ｃ’ＪＩＩｌａＸ　−１：　　　　−
・−・・−（２ｇ）さく０のときＫｌ（ε）＝−Ｃ”・ＪＯ・ε　　　・・・・・・（２
９）（ロ）　　Ｋ２＜ｆ（き＋Ｃ・ε））に関してｆ　
（Ａ＋Ｃ・ε）≧　００ときに２　＜ｆ　（ａ＋ｃ・ε））＝　−［Ｊｍａｘ　−（
Ｄ−Ｃ＋［）２）　−ＪＯ・ωＣ−Ｄ］　　　　　・・
・（３０）ｆ　＜Ａ＋Ｃ・ε）＜Ｏの　ときに２　＜ｆ　（Ａ＋Ｃ・ε））−−［ＪＯ・（Ｄ−Ｃ＋
Ｄ２）　−ＪＯ・ωｃ−Ｑコ　　　　　　　　・・・　
（３１）（ハ）　Ｋ３（Ｑｒ）に関して（加速度フイー
ドフォアードに関して）Ｊｒ　≧Ｏのときに３（６ｒ）＝ＪＯθｒ　　　　　　　　・・・・・−
（３２）ｅｒ＜Ｑのときに３　（ｅｒ＞＝Ｊｍａｘ　−ｏｒ　　　　−−−−−
−（３３）とすれば、リアブノフ関数Ｖの微分Ｍは常に
！〈０となり、切換え面に収束し制御系は安定する。T1 = Kl (5) + 2 (f (a+c-ε)
) + 3 (Qr) (19) For the above equation (18) to hold true, (i) When S ≧ 0, TI = Kl (ε) 2 to 10 < f (/, +C
-ε))+to3 (6r)〉 -C2・J・ε−[J
<D-C+D2) -ωc-D-JO]・/(Q
+C-ε)+J4r -...-<20), so (a) When ε≧0 regarding K1(ε), Kl(ε)=-C-JO・ε...(2
1) When ε〈O, Kl(ε)=−C−Jmax−ε ・−・−・(
22) (b) Regarding K2 < f (a+C・ε)), when f(60C・ε)≧O, 2(f(A+C・ε) ) =−[JO(D−C+0
2) −JO・ωC−D] ・・・・・・(23
) f(60C・ε><0 when 2<f(A+C−ε))=−[Jmax−(D−
C-1-C2) -JO・ωC-D] ・・・・
... (24) (c) Regarding K3 (Qr) (regarding acceleration feed forward) When 4r≧O, 3 (Qr) = Jmax - θr - - (
2b) When Qr<Q, 3(Qr)=JO−θr −・・・(2
G) (ii) When S<O, TI = K1 (ε) + 2 (f (A+C・ε
))〈−C2・J・ε−[J (C−D+02)−JO
−ωc−D] −f (a+c・5)+Jθr −
(27>), (a) When ε≧0 with respect to K1(ε), Kl(ε) −−C′JIIlaX −1: −
・−・・−(2g) When the depth is 0, Kl(ε)=−C”・JO・ε ・・・・・・(2
9) (b) Regarding K2<f(ki+C・ε)), f
(A+C・ε)≧00 when 2 <f (a+c・ε))= −[Jmax −(
D−C+[)2) −JO・ωC−D] ・・
・(30) When f <A+C・ε)<O, 2 <f (A+C・ε))−-[JO・(D−C+
D2) -JO・ωc−Qco...
(31) (c) Regarding K3 (Qr) (regarding acceleration feed forward) When Jr ≧ O, 3 (6r) = JOθr ......-
(32) 3 when er<Q (er>=Jmax −or −−−−−
-(33), then the differential M of the Lyabunov function V is always! <0, converges to the switching surface, and the control system becomes stable.

第２図は本発明をロボット制御に適用した一実施例の制
御系のブロック図である。第２図中、１は、ロボット５
の各軸への移動指令を分配する数値制御装置等のホスト
コンピュータ（以下、ホストＣＰＵという）、２はホス
トＣＰＵから書込まれた各軸の移動指令をデジタルサー
ボ回路３のプロセッサへ受は渡す共有メモリである。ま
た、３はデジタルシグナルプロセッサ等で構成されるデ
ジタルサーボ回路であり、ロボット５の各軸のサーボモ
ータの位置、速度、電流のフィードバック制御を行うも
のである。４は、ロボット５における各サーボモータの
駆動電流のフィードバック値。FIG. 2 is a block diagram of a control system of an embodiment in which the present invention is applied to robot control. In Figure 2, 1 is robot 5
A host computer (hereinafter referred to as host CPU) such as a numerical control device that distributes movement commands to each axis of 2 receives and passes movement commands for each axis written from the host CPU to the processor of digital servo circuit 3. It is shared memory. Further, 3 is a digital servo circuit composed of a digital signal processor, etc., which performs feedback control of the position, speed, and current of the servo motor of each axis of the robot 5. 4 is a feedback value of the drive current of each servo motor in the robot 5;

移動ωθとしてのフィードバックパルスが書込まれる帰
還信号レジスタである。This is a feedback signal register into which a feedback pulse as a movement ωθ is written.

第１図（ａ）、（ｂ）は本実施例において、上記デジタ
ルサーボ回路のプロセッサが実行する動作処理フローチ
ャートであり、該プロセッサは所定周期（移動指令の分
配周期）毎、第１図（ａ）。FIGS. 1(a) and 1(b) are flowcharts of the operation processing executed by the processor of the digital servo circuit in this embodiment. ).

（ｂ）の処理を実行する。Execute the process in (b).

まず、共有メモリ２及び帰還信号レジスタ４より位置の
指令値θｒ、フィードバックパルス量θを読取（ステッ
プ１００）　、従来と同様に位置偏差ε（−θｒ−θ）
及び速度偏差ｔを算出しくステップ１０１）、第（５）
式の演算を行って切換え面Ｓの値を計篩する（ステップ
１０２）。First, the position command value θr and the feedback pulse amount θ are read from the shared memory 2 and the feedback signal register 4 (step 100), and the position deviation ε(-θr-θ) is read as before.
and calculate the speed deviation t.Step 101), step (5)
The equation is calculated to calculate the value of the switching surface S (step 102).

なお、上迷した定数Ｃ１Ｄ及び予想されるイナーシャの
最大値Ｊｍａｘ、最小１ｉｎＪｏ　、及び、これらり、
イナーシャＪｍａｘ、ＪＯで決まる定数ωＣの値は、予
めデジタルサーボ回路３中のメモリ内に設定されている
。または、プログラム化されている。In addition, the unstable constant C1D, the maximum value Jmax of expected inertia, the minimum 1inJo, and these,
The value of the constant ωC determined by the inertia Jmax and JO is set in advance in the memory in the digital servo circuit 3. Or programmed.

デジタルサーボ回路３のプロセッサはステップ１０２で
算出した切換え面Ｓの値がｒＯＪ以上が否か判断しくス
テップ１０３）、Ｓ≧０ならば、次に位置偏差εがｒＯ
Ｊ以上か否か判断しくステップ１０４）、Ｓ≧０ならば
、第（２１）式の演算を行って切換え人力Ｔ１の位置偏
差εの関数部に１（ε）の値をレジスタＲ１に格納する
（ステップ１０５）。また、ステップ１０４でε〈０で
あれば、第（２２）式の演算を行ってレジスタＲ１に格
納する（ステップ１０６）。The processor of the digital servo circuit 3 judges whether the value of the switching surface S calculated in step 102 is greater than or equal to rOJ (step 103), and if S≧0, then the position deviation ε is rOJ or not.
Step 104) to judge whether S is greater than or equal to J, and if S≧0, calculate equation (21) and store a value of 1 (ε) in the function section of position deviation ε of switching human power T1 in register R1. (Step 105). Further, if ε<0 in step 104, the calculation of equation (22) is performed and the result is stored in register R1 (step 106).

次に、ｆ　＜乙＋Ｃ・ε）の演算を行って、この値ｆ　
（１，＋Ｃ・ε）が「０」以上か否か判断しくステップ
１０７）、０以上であれば、第り２３）式の演算を行っ
て切換え人力Ｔ１のｆ　（Ａ＋Ｃ・ε）の関数部に２　
（ｆ　（／ｓ＋ｃ・ε））の値をレジスタＲ２に格納す
る（ステップ１０８）。また、ｆ　＜Ａ＋Ｃ・ε）〈０
であれば、第（２４）式の演算を行ってレジスタＲ２に
格納する（ステップ１０９）。Next, perform the calculation f<Otsu+C・ε) and obtain this value f
It is determined whether (1, +C・ε) is greater than or equal to 0 (Step 107), and if it is greater than 0, the function of f (A+C・ε) of the switching human power T1 is calculated by calculating the equation 23). to 2
The value of (f (/s+c·ε)) is stored in register R2 (step 108). Also, f <A+C・ε)<0
If so, the calculation of equation (24) is performed and the result is stored in register R2 (step 109).

次に、ｅｒの値を計算しく前周期の位置の指令θｒと分
周期の位置の指令θｒとの差によってｅｒが求められ、
前周期で求められたｏｒと分周期で求められたｏｒの差
によって（９ｒの値は求められる）、該値５ｒが「０」
以上か否か判断しくステップ１１０）、ｒＯＪ以上なら
ば第（２５）式の′ＩＡｍを行ってレジスタＲ３に格納
する（ステップ１１１）。また１、９ｒの値が負であれ
ば第（２６）式の演算を行ってレジスタＲ３に格納する
（ステップ１１２）。Next, to calculate the value of er, er is found by the difference between the position command θr of the previous cycle and the position command θr of the divided cycle,
The value 5r is "0" based on the difference between the OR found in the previous cycle and the OR found in the divided cycle (the value of 9r is found).
It is judged whether or not it is above rOJ (step 110), and if it is above rOJ, 'IAm of equation (25) is performed and stored in register R3 (step 111). If the values of 1 and 9r are negative, the calculation of equation (26) is performed and stored in the register R3 (step 112).

そして、レジスタＲ１，Ｒ２，Ｒ３に格納された値を加
算し、切換え人力Ｔ１の値を求め〈ステップ１２２＞、
次に第（６）式の演算を行ってトルク指令値Ｔを算出し
くステップ１２３）、このトルク指令値Ｔを電流補償ル
ープ処理へ受は渡す（ステップ１２４）。デジタルサー
ボ回路３のプロセッサは、このトルク指令値及び電流フ
ィードバック値によって従来と同様に電流補償ループ処
理を行ってロボットの各軸のサーボモータへ駆動電流を
流し、サーボモータを駆動する。Then, the values stored in registers R1, R2, and R3 are added to find the value of switching human power T1 (step 122).
Next, the torque command value T is calculated by calculating equation (6) (step 123), and this torque command value T is passed to the current compensation loop process (step 124). The processor of the digital servo circuit 3 uses the torque command value and the current feedback value to perform current compensation loop processing in the same way as in the past, to flow drive current to the servo motors of each axis of the robot, and drive the servo motors.

一方、ステップ１０３で切換え面Ｓの値が負と判断され
ると、プロセッサは位置偏差εがｒＯＪ以上か否か判断
しくステップ１１３）、ε≧Ｏと判断されると、第（２
８）式の演算を行ってレジスタＲ１に格納しくステップ
１１４）、ε〈０と判断されると第（２９）式の演算を
行ってレジスタＲ１に格納する（ステップ１１５〉。On the other hand, if the value of the switching surface S is determined to be negative in step 103, the processor determines whether the positional deviation ε is greater than or equal to rOJ (step 113), and if it is determined that ε≧O, the processor
8) Calculate equation (29) and store in register R1 (step 114); if it is determined that ε<0, calculate equation (29) and store in register R1 (step 115).

次に、ｆ　（ａ＋ｃ・ε）の値を求め、この値が「０」
以上であれば第（３０）式の演樟を行ってレジスタＲ２
に格納しくステップ１１６．１１７）、負であれば第（
３１）式の実線を行ってレジスタＲ２に格納する（ステ
ップ１１８）。次に、ｏｒが「０」以上か否か判断しく
ステップ１１９）、θｒ≧０ならば第（３２〉式の演算
を行ってレジスタＲ３に格納し、ｏｒ＜Ｑならば第（３
３）式の演樟を行ってレジスタＲ３に格納する（ステッ
プ１２０．１２１）。そして、前述同様、レジスタＲ１
，Ｒ２，Ｒ３の値を加算し、切換え人力Ｔ１の値を求め
（ステップ１２２）、第（６）式の演算を行ってトルク
指令値下を求める（ステップ１２３）。そして、このト
ルク指令値Ｔを電流補償ループへ渡しくステップ１２４
）、電流制御を実行することとなる。Next, find the value of f (a+c・ε), and if this value is “0”
If the above is the case, perform formula (30) and register R2.
(steps 116 and 117), and if it is negative, the th (
31) Perform the solid line in the equation and store it in register R2 (step 118). Next, it is determined whether or is greater than or equal to 0 (step 119). If θr≧0, the calculation of the (32)th expression is performed and stored in the register R3, and if or<Q, the (32th)
3) The formula is deduced and stored in register R3 (steps 120 and 121). Then, as before, register R1
, R2, and R3 to find the value of the manual switching force T1 (step 122), and calculate the torque command value lower by calculating equation (6) (step 123). Then, step 124 passes this torque command value T to the current compensation loop.
), current control will be executed.

発明の効果本発明は、スライディングモードを適用しフィードフォ
アード制御を行うようにしたから、イナーシャ等のパラ
メータの変動があっても、サーボ系は安定に収束し、オ
ーバーシュートのない、かつ、遅れのないサーボ系を得
ることができる。その結果、本発明を適用したサーボ制
御によるロボット等はサイクルタイムが速く軌跡粘度の
よいものが得られる。Effects of the Invention The present invention applies a sliding mode to perform feed-forward control, so even if there are fluctuations in parameters such as inertia, the servo system can stably converge, and there is no overshoot and no delay. You can get a servo system that doesn't have one. As a result, a servo-controlled robot to which the present invention is applied can have a fast cycle time and good trajectory viscosity.

[Brief explanation of drawings]

第１図（ａ、）、（ｂ）は本発明の一実施例における動
作処理フローチャート、第２図は本発明を適用した一実
施例のロボット制御系のブロック図、第３図はフィード
フォアード制御を行う位置、速度ループ制御系のブロッ
ク線図である。ｏｒ・・・位置指令値、θ・・・現在位置、ε・・・位
置偏差、υ・・・実速度、Ｔ・・・トルク指令値。Ｍ　２　口第　３　口Figures 1 (a,) and (b) are operation processing flowcharts in an embodiment of the present invention, Figure 2 is a block diagram of a robot control system in an embodiment to which the present invention is applied, and Figure 3 is feed-forward control. FIG. 2 is a block diagram of a position and velocity loop control system that performs the following. or...Position command value, θ...Current position, ε...Position deviation, υ...Actual speed, T...Torque command value. M 2nd mouth 3rd mouth

Claims

[Claims]

In servo motor control, sliding mode control is used to change and adapt the torque command value according to inertia fluctuations, and performs feed-forward control of speed and acceleration.A sliding mode that includes feed-forward control of speed and acceleration. control method.