(2018 ) ( -1) TA Email : ohki@i.kyoto-u.ac.jp, ske.ta@bode.amp.i.kyoto-u.ac.jp : 411 : 10 308 1 1 2 2 2.1............................................ 2 2.2.................................................. 3 2.3................................................. 4 3 5 3.1................................................. 5 3.2............................................ 7 3.3................................. 9 3.4.................................. 9 4 11 4.1.......................................... 11 4.2 ( )............................................. 11 4.3.................................................. 12 5 19 5.1................................................. 21 5.2.................................................. 21 5.3............................................ 22 6 25 6.1 Nyquist............................................ 25 6.2......................................... 26 6.3.............................................. 30 6.4...................................... 31 7 33
8 2 35 9 38 10 38,, 2. Matlab Simulink 2018 PC Matlab Scilab 2
,,.,.. 1 10 5 ( ) 2 10 11 3 10 12 ( ) 4 10 18 ( ) PI 5 10 19 ( ) 6 10 25 ( ) 7 10 26 ( ) 8 11 1 ( ) 9 11 2 ( ),.,.,, ( :, ).., 0,.,,. 3
1,,,,,.,.,,,. ( ) MATLAB Simulink Quanser QuaRC 2018 1. 1. Matlab 2. Matlab Simulink 3. 1
2 (SRV02) Quanser 2.1 2 3 3 D[m] D α[rad] α = D L L[m] 2 3 α 4 4 3 D 1[inch] *1 1[V] AD *1 Quanser inch 2
4 2.2 5, 6 SRV02 14:1 *2 1024 θ[rad] 5 hub motor 6 *2 14:1 3
2.3 (Quanser UPM1503) (I/O) ( 8) ±5[V] *3 I/O DA AD 7 8 I/O 2014 7 9 2018 10 1 2 u[v] *3 ±10[V] ±5[V] 4
9 θ[rad] α[rad] 10 ( ) (α) 3 ( ) 3.1 11 u, θ, τ u V 5
u K a V R L e θ m θ i τ m gear τ amp motor hub 11 K a V = K a u (1) u V = Ri + L di dt + e i e R L L V = Ri + e (2) e θ m K m e = K m dθ m dt (3) τ m i K τ τ m τ m = K τ i (4) (2), (3), (4) V, τ m, dθm dt ( ) R dθ m V τ m K m = 0 (5) K τ dt τ m τ θ m θ K g1, K g2 τ m = K g1 τ, θ m = K g2 θ (6) (1), (5), (6) (1), (6) (5) K a u ( RKg1 K τ ) τ (K m K g2 ) dθ dt = 0 6
D s := Kτ KmKg2 Kτ Ka RK g1, K s := RK g1 τ = D s dθ dt + K su (7) 3.2 [1] 12 α[rad] *4 12 13 1 2 1 2 *4 7
13 13 2 γ := θ + α (8) 1 (θ) 2 (γ) α 14 14 J 1, J 2 1,2 K l 1 2 θ, γ 1 2 8
d 2 θ J 1 dt 2 = K l(θ γ) + τ (9) d 2 γ J 2 dt 2 = K l(γ θ) (10) 3.3 (9) τ (7) d 2 θ J 1 dt 2 = K dθ l(θ γ) D s dt + K su (11) d 2 γ J 2 dt 2 = K l(γ θ) (12) 3.4 u θ, α, γ P θ (s), P α (s), P γ (s) u, θ, α, γ û, ˆθ, ˆα, ˆγ ˆθ ˆα ˆγ = P θ (s) P α (s) P γ (s) P θ (s), P α (s), P γ (s) (11), (12) û J 1 s 2 ˆθ = Kl (ˆθ ˆγ) D s sˆθ + K s û (13) J 2 s 2ˆγ = K l (ˆγ ˆθ) (14) P γ (s) (13),(14) ˆθ ˆγ ( J1 s 2 + D s s + K l ) ˆθ = Klˆγ + K s û (15) (15), (16) ˆθ ˆγ K l ˆθ = ( J2 s 2 + K l ) ˆγ (16) ( ) J 1 J 2 s 4 + J 2 D s s 3 + (J 1 + J 2 )K l s 2 + K l D s s ˆγ = K l K s û P γ (s) P γ (s) = ˆγ û = K l K s s (J 1 J 2 s 3 + J 2 D s s 2 + (J 1 + J 2 )K l s + K l D s ) 9
(16) P θ (s) P θ (s) = ˆθ û = (J 2 s 2 + K l )K s s (J 1 J 2 s 3 + J 2 D s s 2 + (J 1 + J 2 )K l s + K l D s ) (8) ˆα = ˆθ + ˆγ P α (s) P α (s) = P γ (s) P θ (s) = J 2 K s s 2 s (J 1 J 2 s 3 + J 2 D s s 2 + (J 1 + J 2 )K l s + K l D s ) u θ,α,γ P θ (s) P α (s) 1 = s (J P γ (s) 1 J 2 s 3 + J 2 D s s 2 + (J 1 + J 2 )K l s + K l D s ) (J 2 s 2 + K l )K s J 2 K s s 2 K l K s (17) (17) P θ (s) 1/s P α (s) s = 0 P θ (s) s = ±j K l J 2 θ α 10
4 4.1 P θ (s), P α (s), P γ (s) P θ (s) P α (s) P γ (s) = 1 s (J 1 J 2 s 3 + J 2 D s s 2 + (J 1 + J 2 )K l s + K l D s ) (J 2 s 2 + K l )K s J 2 K s s 2 K l K s 3 P (s) (a 1, a 2, a 3 ) (b 1, b 2 ) P θ (s) P α (s) 1 = s(s P γ (s) 3 + a 3 s 2 + a 2 s + a 1 ) b 2 s 2 + b 1 b 2 s 2 b 1 (18) J 1,J 2,K s,k l (18) a 1,a 2,a 3,b 1,b 2 4.2 ( ) u, y u y P (s) u(t) = a sin ωt, t 0 y(t) y(t) = P (jω) a sin(ωt + P (jω)), t 1 P (jω) P (jω), 11
. 15 P (jω) = b a P (jω) = ωt o 15 P (s) s = jω P (jω) P θ (jω) P θ (jω) Bode Nyquist [2] Bode Nyquist 4.3 P (s) ( u, y) *5,, 16. PI, K p K I C(s) = K I s. G cl (s) = P θ(s)c(s) 1 + P θ C(s) + K p., θ (19) (K p s + K I )(b 2 s 2 + b 1 ) = s 5 + a 3 s 4 + (a 2 + K p b 2 )s 3 + (a 1 + K I b 2 )s 2 (20) + K p b 1 s + K I b 1,,,. Step 1: p ω 1, ω 2, ω p *5 12
16 PI Simulink Plant P (s) Step 2: ω i, i = 1,..., p G cl (jω i ), i = 1,..., p. Step 3: G cl (jω), ω 0 G cl (jω i ), i = 1,..., p G cl (s) 4.3.1 Step 2 u(t) = sin ω i t 15 p u(t) = P (s) y p f i sin(ω i t + ϕ i ) (21) i=1 p y(t) = G cl (jω i ) f i sin ( ω i t + ϕ i + G cl (jω i ) ) (22) i=1 y(t) sin(ω i t) cos(ω i t) y(t) p ( y(t) = f i ci cos (ω i t + ϕ i ) + s i sin (ω i t + ϕ i ) ) (23) i=1 (c i, s i ), i = 1,..., p G cl (jω i ) = c 2 i + s2 i G cl (jω i ) = arctan ( ci s i ) (24a) (24b) 13
t = t 1, t 2,, t q y(t 1 ), y(t 2 ),, y(t q ) (23) J = 1 2 q y(t k) k=1 p f i (c i cos(ω i t k + ϕ i ) + s i sin(ω i t k + ϕ i )) i=1 (c i, s i ), i = 1,..., m ( 2 ) y(t 1 ), y(t 2 ),, y(t q ) *6 Y = [ y(t 1 ) y(t 2 ) y(t q ) ] R 1 q (25) C(f i, ω i ) = [ f i cos(ω i t 1 + ϕ i ) f i cos(ω i t 2 + ϕ i ) f i cos(ω i t q + ϕ i ) ] R 1 q S(f i, ω i ) = [ f i sin(ω i t 1 + ϕ i ) f i sin(ω i t 2 + ϕ i ) f i sin(ω i t q + ϕ i ) ] R 1 q P = [ ] c 1 s 1 c 2 s 2 c p s p R 1 2p, X = 2 J C(f 1, ω 1 ) S(f 1, ω 1 ) C(f 2, ω 2 ) S(f 2, ω 2 ). C(f p, ω p ) S(f p, ω p ) 2 R 2p q J = 1 (Y PX)(Y PX) 2 = 1 [ Y Y PXY Y X P + PXX P ] (26) 2 XX P J = 1 2 (P ˆP)(XX )(P ˆP) + Y Y Y X (XX ) 1 XY ˆP = Y X (XX ) 1 (27) (c i, s i ), i = 1,..., p (27) ˆP 2 J ( 2 Step 2 Step 2.1: Step 2.2: ω i f i ϕ i i = 1,..., p (21) t k y(t k ), k = 1,..., q *6 J = 0 (c i, s i ), i = 1,..., p J > 0 2 14
Step 2.3: y(t k ), k = 1,..., q f i, i = 1,..., p Y, X P 2 ˆP (27) Step 2.4: ˆP (c i, s i ), i = 1,..., p G cl (jω i ) (24) (27), X R 2p q,, ( 8 ). 500Hz, X q 500 8 60 = 240000, XX.,,. (27), q,,. V (q) := (X(q)X(q) ) 1, P(q) = Y (q)x(q) (X(q)X(q) ) 1 x(q) = [f 1 sin(ω 1 t q + ϕ 1 ), f 1 cos(ω 1 t q + ϕ 1 ),, f p sin(ω p t q + ϕ p ), f p cos(ω p t q + ϕ p )], *7 1 ( ) P(q) =P(q 1) 1 + x(q) y(t q ) + P(q 1)x(q) x(q) V (q 1), V (q 1)x(q) (28) 1 V (q) =V (q 1) 1 + x(q) V (q 1)x(q) V (q 1)x(q)x(q) V (q 1) (29) ( ). V (q 1).,, V (0), (0 ).,, P(0) V (0),,. 4.3.2 ω i, i = 1,..., p P (s) Step 3 [2] 5.6 *8 P θ (s) Step 3.1: P θ (s), ω = b 1 /b 2, *7. *8 15
, (Step 2) G cl (jω) ω o s = ±jω o G cl (s), 20 log G cl (jω) 20 log(k p b 2 ) 40 log ω (30), b 2. b 1 = ωob 2 2, P θ (s). ω o log ω 0 Step 3.2: G cl (s) i = 1, 2,, p,. 20 log G cl (jω i ) =20 log 1 (jω) 5 + a 3 (jω i ) 4 + (a 2 + K p b 2 )(jω i ) 3 + (a 1 + K I b 2 )(jω i ) 2 + jk p b 1 ω i + K I b 1 20 log 1 (jk p ω i + K I )(b 2 (jω i ) 2 + b 1 ) (31),, 20 log G cl(jω i ) =20 log 1 (jω) 5 + a 3 (jω i ) 4 + (a 2 + K p b 2 )(jω i ) 3 + (a 1 + K I b 2 )(jω i ) 2 + jk p b 1 ω i + K I b 1 (32) (a 1, a 2, a 3 )., G cl (jω i),,.,. Step 3.3: (a 1, a 2, a 3 ) (32) (a 1, a 2, a 3 ),. Matlab, G cl (s) Bode G cl (jω). b 1, b 2, p 0 P θ (s) a 1 a 2 a 3 a 1 a 2 a 3 > 0 (K p, K I ) = ( 0.1, 0.1) 2 PI G cl (s) Bode, 16
1. K r θ θ, α Ĝcl 1 jω + α = 1 jω α pole Ĝcl 1 G cl P θ (s) = 100s 2 + 20000 s(s 3 + 40s 2 + 1000s + 10000) 17 0.1 [rad/sec] 600 [rad/sec] 500 Hz Nyquist 250 Hz 100 Hz 600 [rad/sec] 1 {f i } 100 17 3.1 b 1 b 2 17
17 18 18
3.2 18 G cl (0) = 1 17 0.7 [rad/sec] 19 19 3.3 l1 fminsearch 100 20 1 5 21 P (s), C(s) 1 19
20 * o 21 1 2 3 4 (r, d) (y, u) (r, d) (y, u) 4 y r d y 20
P (s), C(s) P (s) = N(s) D(s), C(s) = N c(s) D c (s) (33) (D, N), (D c, N c ) 5.1 21 21 φ(s) := D(s)D c (s) + N(s)N c (s) (34) Hurwitz, φ(s) = 0 Matlab G [num,den] = tfdata(g); num = cell2mat(num); den = cell2mat(den); max(real(roots(den))) Nyquist 2 3 5.2 d = 0 r(t) = y { 1 (t > 0) 0 (t < 0) 22 21
(settling time) T s y y s ±2% ±5% (raising time) T r y y s 10% 90% (maximum overshoot) A max y y s (peak time) T max 22 5.3 y r e = r y e ê(s) ê = S(s)ˆr + R(s) ˆd S(s), R(s) r d e P (s),c(s) S(s) = 1 1 + P (s)c(s), R(s) = P (s)s(s) = P (s) 1 + P (s)c(s) (35) (33) S(s) = D(s)D c(s), R(s) = N(s)D c(s) φ(s) φ(s) (36) S(s) r(t) d(t) y r 22
{ a (t > 0) r(t) = 0 (t < 0) { b (t > 0) d(t) = 0 (t < 0) a : b : ˆr(s) = a s, ˆd(s) = b s φ(s) Hurwitz e s e s = lim t e r (t) = lim s 0 sê(s) = e sr + e sd (37) e sr e sd D(s),N(s),N c (s),d c (s) e sr = D(0)D c(0) φ(0) a, e sd = N(0)D c(0) b (38) φ(0) a, b e s = 0 D(0)D c (0) = 0 N(0)D c (0) = 0 D(s) N(s) 21 r d y r C(s) s = 0 C 0 (s) C(s) C(s) = 1 s C 0(s), C 0 (0) 0 C(s) 1/s C(s) P (s) e s 0 1 23
20 30 40 50 60 70 80 90 100 10Gain (db) 2 10 10 1 100 101 102 10 20 30 40 50 60 70 80 90 100 2 10 10 1 100 101 102 Phase (deg) Frequency (rad/sec) Frequency (rad/sec) PI PI(Proportional-Integral) C(s) = K p + K i s PI e(t) e s 0 PI Bode (39) 23 PI Bode P θ (s) P (s) P ( ) C(s) = K p e s (37), (38) (MATLAB PI C(s) = K p + K i /s e s = 0 (37), (38) 24
6 21 12 6.1 Nyquist 12 G(s) := P (s)c(s) Nyquist G(s) Nyquist [2] Nyquist P (s) C(s) G(s) Π G(s) Nyquist ( 1, j0) N 12 N = Π G(s) Π Π = 0 Nyquist Nyquist P (s) C(s) G(s) = P (s)c(s) 12 G(s) Nyquist ( 1, j0) 25
13 Nyquist 6.2 G(s) = P (s)c(s) 6.2.1 Im G(jω pc ) = 0 ω pc GM GM = 1 G(jω pc ) Nyquist G(s) Nyquist ( 1, j0) G(s) GM Nyquist ( 1, j0) GM Nyquist ( 1, j0) G(jω gc ) = 1 ω gc PM PM = 180 + G(jω gc ) Nyquist G(s) ( ) PM[ ] Nyquist ( 1, j0) PM[ ] Nyquist ( 1, j0) 26
14 15 Bode 16 PM = 40 60, GM = 10 20 db 6.2.2 Bode P (s) P (s) = P (s)(1 + (s)) (40) (s) 27
16 Bode *9 (s) r(ω) (jω) r(ω) ω R (41) (40),(41) P (s) P (s) ( P, C) Nyquist P (s) P (s) G(s) = P (s)c(s) G(s) = P (s)c(s)(1 + (s)) = G(s) + G(s) (s) (s) G(s) G(s) (41) (s) G(s) Nyquist ( 1, j0) 17 G(s) G (s) Nyquist (41) ω G(jω) G(jω) r(ω) G(jω) Nyquist G(jω) G(jω) r(ω) G(jω) ( ) ω ( 1, j0) 1 + G(jω) r(ω) G(jω) ω R G(jω) ( 1, j0) *9 P (s) 28
17 (P, C) G(s) = P (s)c(s) (40),(41) P (jω)c(jω) 1 + P (jω)c(jω) r(ω) 1 ω R (42) T (s) := P (s)c(s) 1 + P (s)c(s) S(s) + T (s) = 1 (43) r(ω) r(ω) r(ω) T (s) P (s) ω 1 T (jω) P (jω)c(jω) G(s) (42) T (jω)r(ω) < 1, ω (44) 29
sup T (jω)r(ω) (45) ω 1 sup ω G(jω) G H Matlab norm(g,inf) 6.3 r d y ŷ = T (s)ˆr + P (s)s(s) ˆd (46) S(0) = 0, T (0) = 1 T (s) T (0) 1/ 2 ( 3dB ) ω b ω b ω b * 10 T (s) = G(s)/(1 + G(s)) PM 90 G(s) ω gc ω b ω gc ω b [3] ω gc d y d y P (s)s(s) P (s) S(s) G(jω) > 1 S(jω) 1 G(jω) 1 G(jω) Bode [2] 7.1 S(jω) T (jω) (43) S(jω) S(jω) G(jω) G(s) *10 T r T r 1/ω b [2] 30
6.4 G(s) C(s) (i) PM = 40 60, GM = 10 20 db (ii) ω 1 G(jω) ω 1 G(jω) (iii) ω gc (iv) ω gc G(jω) 20 [db/dec] G(s) 18 G 20dB/dec 0 ω 1 ω gc ω2 ω 18 31
P (s) := P γ (s) PI C(s) = K p + K i /s 1. G(s) (i) (iv) PI K p, K i a P γ (s) G(s) Bode GM, PM, ω gc 2. PI 3. ±5 [V] 1 C(s) 90 (i) (iv) K p, K i G(s) Bode P γ (s) 4. 3. PI a (i) (iv) 32
7 2 19 2 12 M(s) P 1 (s)m(s) 19 2 C(s) 2 M(s) P (s) 1 M(s) * 11 ŷ = P (s)(û + ˆd) û = C(s)[M(s)ˆr ŷ] + P (s) 1 M(s)ˆr û r, d y ŷ = M(s)ˆr + P (s) 1 + P (s)c(s) ˆd (47) C(s) M(s) r y M(s) M(s) 0 M(0) = 1 M(s), P (s) 1 M(s) M(0) = 1 *11 M(s) P (s) M(s) P (s) 33
P (s) P (s) 1 P (s) 1 M(s) 34
8 2 2 2 4 1 2 2 γ Step 1: P γ (s) γ û = C γ (s)(ˆr ˆγ) 20 γ Step 2: γ = θ + α C γ (s) (θ, α) û = [ C 1 (s) C 2 (s) ] [ˆr ˆθ ], C ˆα 1 (s) = C 2 (s) = C γ (s) * 12 C 1 (s) C 2 (s) C γ (s) C 1 (s) C 2 (s) Step 3: 2 (i) (θ, α) Step 1 2 p m P (s) m p C(s) Re λ 0 λ [ ] Ip P (λ) det 0 C(λ) I m I q q q (ii) Step 2 C 1 (s) C 2 (s) [ *12 r = r 1 + r 2 û = C 1 (s) ] [ ˆr 1 C 2 (s) ˆθ ] ˆr 2 ˆα 35
(iii) Step 3 2 α θ γ = θ + α θ Step 1: α û = C α (s)ˆα α 0 Step 2: P θ (s), P α (s) C α (s) P (s) θ P (s) 2 (C θ, M) (i) 2 (a) (b) 21 2 (ii) 2 u P θ (s), P α (s) d C θ (s), C α (s) 36
(iii) C θ (s), M(s) C α (s) C θ (s), M(s) C α (s) (a) u ±5 [V] (b) γ 0 90 0 90 0 3 γ (c) (b),(c) γ 5% (i) Bode (ii) ( ) ( ) 37
9 2, 2. 1. PI 2.,.,. 10 * 13 1 ( P PI [1] Alessandro de Luca and Bruno Siciliano. Trajectory control of a non-linear one-link flexible arm. International Journal of Control, Vol. 50, No. 5, pp. 1699 1715, 1989. [2].., 2002. [3],.., 1998. *13 38