DC 서브 모터 제어

V_a = L_a (i (t))/t + R_zi (t) + K_b (θ (t))/t

V_a - 입력전압

L_a - 아마추어 인덕턴스

R_a - 아마추어 저항

K_b - 역기전력 상수

θ (t) - 모터 회전각

모터에 연결되어 있는 새프트의 동적 방정식

J_m (^″θ (t))/t + B_m (^′θ (t))/t = τ_m

J_m - 모터의 관성

B_m - 모터의 댐핑

τ_m - 출력 토크

K_t - 토크상수

τ_m 는 K_t와 전류에 비례

τ_m = K_ti (t)

라플라스 변환

θ (s)[J_ms^2 + B_ms] = K_tI (s)

θ (s) = K_t/(J_ms^2 + B_ms) I (s)

V_a (s) =[L_as + R_a + (K_tK_b)/(J_ms + B_m)] I (s)

안정화

V_a = K_1 (V_dK_2 - K_ci (t))

입력 전압 과 모터 의 위치인 출력과의 관계식

(θ (s))/(V_d (s)) = (K_1K_2K_t/L_aJ_m)/s[s^2 + (B_m/J_m + R_a/L_a + (K_1K_c)/L_a) s + (R_aB_m + K_1K_cB_m + K_tK_b)/(L_aJ_m)]

K_1 - 전압이득값 - 10

K_2 - 이득값 - 1

K_c - 전류이득값 - 0.5

K_t - 토크 상수 - 4.  oz - in/A

K_b - 역기전류 상수 - 0.003 V/(rad/sec)

R_a - 아마추어 저항 - 1 ohm

L_a - 아마추어 인덕턴스 - 0.1H

B_m - 모터 댐핑 - 0.0001 oz - in/(rad/sec)

J_m - 모터 관성 - 0.001 oz - in - sec^2

(θ (s))/(V_d (s)) = ((10 * 1 * 4.)/(0.1 * 0.001))/(s (s^2 + (0.0001/0.001 + 1/0.1 + (10 * 0.5)/0.1) s + (1 * 0.0001 + 10 * 0.5 * 0.0001 + 4. * 0.003)/(0.1 * 0.001)))

= 400000/(s (s^2 + 60.1 s + 126))

디지털 PID 제어

시간영역에서 PID 저에기

u (t) = K_pe (t) + K_dOverscript[e, .] (t) + K_i∫e (t) t

e (t) = θ_d (t) - θ (t)

미분제어기 - 오버슈트 줄여줌

Overscript[e, .] (t) = (e (nT) - e ([n - 1] T))/T

z변환

U (z) _d = Z {Overscript[e, .] (t)} = (1 - z^(-1))/T = (z - 1)/T_z

적분제어기 - 정상상태 오차 없앰

U (z) _I = T/(z - 1)

U (z) = K_p + K_iT/(z - 1) + K_d (z - 1)/T_z

진자 운동

L - 진자 길이

y = L - L Cos[θ] = L (1 - Cos[θ])

선속도 υ

υ = ω×L = Overscript[L θ, .] Sin[90°] = Overscript[L θ, .]

운동에너지

K = 1/2m υ^2 = 1/2m (Overscript[L θ, .])^2 = 1/2mL^2Overscript[θ, .]^2

포텐셜 에너지

U = mgy = mgL (1 - Cos[θ])

라그란지안 동력학

L = K - U = 1/2mL^2Overscript[θ, .]^2 - mgL (1 - Cos[θ])

라그란지안 동력학 식

/t (∂ L/Overscript[∂ q_i, .]) + ∂ U/∂ q_i = Q_i

∂ L/Overscript[∂ θ, .] = m L^2Overscript[θ, .]

/t∂ L/Overscript[∂ θ, .] = m L^2Overscript[θ, ..]

∂ U/∂ θ = m g L Sin[θ]

ml^2Overscript[θ, ..] + m g l Sin[θ] = 0

Overscript[θ, ..] + g/l Sin[θ] = 0

1

t = 0 ;

dt = 0.02 ; <br />

l = 1 ;

m = 1 ;

g = 9.81 ;

th = 0 ;

dth = 0 ;

estimth = 0 ;

newth = 0 ;

newdth = 0 ;

x = 1 ;

y = 0 ;

oldx = 0 ;

oldy = 0 ; <br />

oldt = 0 ;

isAnimating = 0 ;

isManipulating = 0 ;

isAnimating = If[t≠oldt, True, False] ;

isManipulating = If[x≠oldx || y≠oldy, True, False] ;

th = If[isManipulating, ArcTan[-y, x], If[isAnimating, newth, th]] ;

l = If[isManipulating, Sqrt[x * x + y * y], l] ;

dth = If[isManipulating, 0, If[isAnimating, newdth, dth]] ; <br />

estimth = th + dth dt - 0.5 g/l Sin[th] dt dt ;

newdth = dth - g/l 0.5 (Sin[th] + Sin[estimth]) dt ;

newth = th + dth dt - 0.5 g/l 0.5 (Sin[th] + Sin[estimth]) dt dt ; <br />

x = l * Sin[th] ;

y = -l * Cos[th] ;

oldx = x ;

oldy = y ;

oldt = t ;

2

t = 0 ;

dt = 0.02 ;

l1 = 0.5 ;

m1 = 1 ;

l2 = 0.5 ;

m2 = 1 ;

g = 9.81 ;

th1 = 0 ;

th2 = 0 ;

dth1 = 0 ;

dth2 = 0 ;

ddth1 = 0 ;

ddth2 = 0 ;

newth1 = 0 ;

newdth1 = 0 ;

newth2 = 0 ;

newdth2 = 0 ;

x1 = 0.5 ;

y1 = 0 ;

x2 = 0.8 ;

y2 = 0.3 ;

initialEnergy = 0 ;

energy = 0 ;

oldx1 = 0 ;

oldy1 = 0 ;

oldx2 = 0 ;

oldy2 = 0 ;

oldt = 0 ;

isAnimating = 0 ;

isManipulating = 0 ;

isAnimating = If[t≠oldt, True, False] ;

isManipulating = If[x1≠oldx1 || y1≠oldy1 || x2≠oldx2 || y2≠oldy2, True, False] ;

th1 = If[isManipulating, ArcTan[-y1, x1], If[isAnimating, newth1, th1]] ;

th2 = If[isManipulating, ArcTan[-(y2 - y1), (x2 - x1)], If[isAnimating, newth2, th2]] ;

l1 = If[isManipulating, Sqrt[x1 * x1 + y1 * y1], l1] ;

l2 = If[isManipulating, Sqrt[(x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)], l2] ;

initialEnergy = If[isManipulating, -l1 g Cos[th1] (m1 + m2) - l2 m2 g Cos[th2], initialEnergy] ;

dth1 = If[isManipulating, 0, If[isAnimating, newdth1, dth1]] ;

dth2 = If[isManipulating, 0, If[isAnimating, newdth2, dth2]] ;

x1 = l1 * Sin[th1] ;

y1 = -l1 * Cos[th1] ;

oldx1 = x1 ;

oldy1 = y1 ;

x2 = l2 * Sin[th2] + x1 ;

y2 = -l2 * Cos[th2] + y1 ;

oldx2 = x2 ;

oldy2 = y2 ;

ddth1 = -(g ((2 m1 + m2) Sin[th1] + m2 Sin[th1 - 2 th2]) + l1 m2 Sin[2 (th1 - th2)] dth1^2 + 2 l2 m2 Sin[th1 - th2] dth2^2)/(2 l1 (m1 + m2 - m2 Cos[th1 - th2]^2)) ;

ddth2 = (Sin[th1 - th2] (g (m1 + m2) Cos[th1] + l1 (m1 + m2) dth1^2 + l2 m2 Cos[th1 - th2] dth2^2))/(l2 (m1 + m2 - m2 Cos[th1 - th2]^2)) ;

newth1 = th1 + dth1 dt + 0.5 ddth1 dt^2 ;

newth2 = th2 + dth2 dt + 0.5 ddth2 dt^2 ;

newdth1 = dth1 + ddth1 dt ;

newdth2 = dth2 + ddth2 dt ;

energy = 0.5 (m1 + m2) l1^2 newdth1^2 + 0.5 m2 l2^2 newdth2^2 + m2 l1 l2 newdth1 newdth2 Cos[newth1 - newth2] - l1 g Cos[newth1] (m1 + m2) - l2 m2 g Cos[newth2] ;

newdth1 = If[energy<initialEnergy, 1.01 newdth1, 0.99 newdth1] ;

newdth2 = If[energy<initialEnergy, 1.01 newdth2, 0.99 newdth2] ; oldt = t ;