GS456 Advaced Automatc Cotrol Term Project (03 Fall Semester) Dyamc Smulato ad Cotrol Expermet for Cart-Pedulum System Dyamc Smulato Modelg ad Cotrol Desg Cotrol Expermet Cotrol Smulato ad Expermet of a Cart-Pedulum System Msso. Cotrol ad Estmator Desg Dervato of the (coupled olear) dyamc equato of moto. Desg of a PID cotrol law wth jot agle feedback Desg of a state feedback cotrol law wth jot agle feedback Desg of a state estmator to recostruct full-state (by assumg that the cart posto ad the pedulum agle are measurable) Msso. Dyamc Moto Smulato (Matlab or C++) Numercal tegrato of the (olear) dyamc equato of moto (apply Ruge-Kutta algorthm) Cotrol smulato of Iverted pedulum Data plottg of smulato results Msso 3. Iverted pedulum Cotrol expermet Uderstad the H/W ad S/W archtecture of the Lear-Motor cotrol system Make a cotrol fucto to mplemet your cotroller Perform cotrol ga tug Data acqusto ad plottg of expermetal results
Cotrol Smulato ad Expermet of a Cart-Pedulum System [] (subroute RK4) Wrte RK4(Ruge-Kutta 4 th order) route for umercal tegrato of the dyamc equato of moto (Iput: cart drvg force u(t) Output: cart ad pedulum moto (x &theta) Two d order equatos of moto whch are dyamcally coupled: 반드시마찰력을고려할것! H H θ c θ g( θ, x) 0 H H + + = x cx g ( θ, x) u θ H H 0 c θ g( θ, x) f( θ, θ, xx, ) = x H H + = u cx g ( θ, x) f( θθ,, xxu,, ) Four smultaeous st order equatos: : Numercal tegrato usg RK4 algorthm RK4 tegrato step = samplg tme (ex) ms = 0.00 sec Plat parameters : m =? kg, m =? kg, 진자길이 ( 막대 ) l =? m cart pedulum 3 [] (subroute JotCtrl) Wrte jot cotrol fucto Jot torque = cotrol put + arbtrary dsturbace(your assumpto!) PID cotrol: Ks () = kp + ks D + ki es () s ut () = ket () + k et () + k etdt () p D I dscrete-tme represetato: ek ( ) ek ( ) uk ( ) = kek p ( ) + kd + ktek I ( ) + uk ( ) T Perform your Ga tug! followg the order: kp kd ki Dsturbace put: (ex.) dt () = Ds( ω t) or square wave(wth magtude D, frequecy ω ) Frst suggesto D = 0.u, ω = 0 ~ 50 rad / sec, max d d Ivestgate how the cotrol error s vared accordg to ( D ad ω ) d d Total torque put : τ () t = u ( t ) + d ( t ) Cosder actuator torque lmt : u u() t u (ex.) u = 5~50[ N] max max max 4
[3] (subroute Estmator) Wrte the state estmator fucto [4] (subroute Ma) ) Whe usg MATLAB drect m-fle codg s preferred rather tha usg SIMULINK ) Whe usg Mcrosoft VISUAL C++ Combed State Feedback ad Estmator Loop dsturbace motor torque lmt dt () Cart-pedulum (olear moto model) ref. commad θ d () t et () + ut () τ () t K(s) + Coupled dyamc equato of moto x() t θ () t ˆx ˆx θˆ ˆ θ State estmator 5 PID cotrol loop & State estmato loop motor torque lmt dsturbace dt () Cart-pedulum Moto model ref. commad θ d () t + et () K(s) ut () + τ () t Coupled dyamc x() t equato of moto θ () t θˆ ˆ θ ˆx ˆx State estmator 6
Example of MatLab Code for RK4 Itegrato (scrpt m-fle) % ma.mm % === Example of RK4 Itegrato algorthm %------- d order ODE ---------------------- % y_ddot + a*y_dot + b*y = 0 % wth (I.C.) y(t=0) =, y_dot(t=0) = clear; %---------- System Parameters -------------- a = 0.; b = 00.; % (I.C.) x() = 3.0; x() = 0.0; y + ay + by = 0 (I.C.) y(0) = 3, y (0) = 0 Sprg을 3m 앞으로당겼다놓는경우 k b= 00 = = 0 rad / sec m ω Natural freq.( 고유진동수), ω = a = 0 = ζω Dampg coeff.( 감쇠계수), ζ = Try dfferet set of ( ω, ζ)! h = 0.0; % Itegrato tme terval tf = 3.0; % Fal tme t = [ 0: h: tf]'; %--- RK4 tegrato start ----------------------------- for = : : sze(t,)- % Slopes k, k, k3, k4 k = x() ; k = -a*x() - b*x(); k = x() + 0.5*k*h ; k = -a*( x() + 0.5*k*h ) - b*( x() + 0.5*k*h ); k3 = x() + 0.5*k*h ; k3 = -a*( x() + 0.5*k*h ) - b*( x() + 0.5*k*h ); k4 = x() + k3*h ; k4 = -a*( x() + k3*h )- b*( x() + k3*h ); % Updated value x(+) = x() + (k + *k + *k3 + k4)*h/6; x(+) = x() + (k + *k + *k3 + k4)*h/6; ed; fgure(); subplot(); plot(t, x); xlabel('tme(sec)'); ylabel('x'); ttle('posto'); grd; subplot(); plot(t, x); xlabel('tme(sec)'); ylabel('x'); ttle('velocty'); grd; 7 Example of MatLab Code for RK4 Itegrato (fucto m-fle): % === Example of RK4 Itegrato algorthm % It requres m-fles wth subfuctos rk4.m & dy_eq.m %======================================== % 화일명 : rk4_ma.m or 임의명.m fucto rk4_ma() % 각 subfucto을독립적인 m-fle로저장하면 ma 함수이름생략가능 %------- d order ODE ---------------------- % y_ddot + a*y_dot + b*y = u(t) wth (I.C.) y(0), y_dot(0) clear; %---------- System Parameters -------------- %global a b h; a = 3.; % c/m b = 00.; % k/m h = 0.0; % Itegrato tme terval % (I.C.) x(,:) = [3.0 0.0]; %x() = 3.0; x() = 0.0; tf = 3.0; % Fal tme t = [ 0: h: tf]'; % Subfucto % rk4.m fucto x = rk4( x, u, a, b, h ) % Slopes k, k, k3, k4 <-- x vectors k = dy_eq( x, u, a, b, h ); k = dy_eq( x + 0.5*k*h, u, a, b, h ); k3 = dy_eq( x + 0.5*k*h, u, a, b, h ); k4 = dy_eq( x + k3*h, u, a, b, h ); x = x + h*( k + *k + *k3 + k4 )/6; % Subfucto % dy_eq.m fucto f = dy_eq( x, u, a, b, h ) f(:,) = x(:,); f(:,) = -a*x(:,) *( - b*x(:,) + u; %--- RK4 tegrato start ----------------------------- for = : : sze(t,)- % Iput u() = 0; x(+,:) = rk4( x(,:), u(), a, b, h ); ed; fgure(); subplot(); plot(t, x(:,)); xlabel('tme(sec)'); ylabel('x'); ttle('posto'); grd; subplot(); plot(t, x(:,)); xlabel('tme(sec)'); ylabel('x'); ttle('velocty'); grd; 8
Example of MatLab Code for RK4 Itegrato (le fucto) % rk4_le_f.m fucto rk4_le_f() %---------- System Parameters -------------- a = 3.; % c/m b = 00.; % k/m h = 0.0; % Itegrato tme terval x(,:) = [3.0 0.0]; %x() = 3.0; x() = 0.0; % (I.C.) tf = 30; 3.0; t= [0:h:tf]'; % le fuctos for % y_ddot + a*y_dot + b*y = u(t) wth (I.C.) y(0), y_dot(0) %----------------------------------- f = le('x'); f = le('-a*x - b*x + u', 'x','x','u','a','b'); %--- RK4 tegrato ----------------------------- for = : : sze(t,)- u() () = 0; % Iput k = f(x(,)); k = f(x(,), x(,), u(), a, b); k = f(x(,)+0.5*k*h); k = f(x(,)+0.5*k*h, x(,)+0.5*k*h, u(), a, b); k3 = f(x(,)+0.5*k*h); k3 = f(x(,)+0.5*k*h, x(,)+0.5*k*h, u(), a, b); k4 = f(x(,)+k3*h); k4 = f(x(,)+k3*h, x(,)+0.5*k3*h, u(), a, b); x(+,) = x(,) + h*( k + *k + *k3 + k4 )/6; x(+,) = x(,) + h*( k + *k + *k3 + k4 )/6; ed; 9 Iverted Pedulum o the Movg Cart Graphc amato usg VC++ wth OpeGL or Matlab Expermetal setup & Basc C++ code Mass of cart Pedulum mass Pedulum legth Dampg coeffcet of cart Dampg coeffcet of pedulum Samplg tme System Parameters.54 kg 0.56 kg 400 mm 0.~0.3 0.~0.3 0.3 ms 0
Smulato results ) Cofrm the free moto of pedulum ad cart (opeloop case) Wth respect to the tal codto chage Wth respect to the dampg coeffcet chage ) Closed-loop loop cotrol smulato Normal pedulum (wth stable eq. pot = 0 deg ) Iverted pedulum (wth ustable eq. pot = 80 deg ) Ivestgate how the cotrol performace s vared accordg to the chages of PID cotrol ga ad the magtude of dsturbaces. 3) State estmator smulato Ivestgate how the estmato performace s vared accordg to the chage of estmator ga. Cart & Iverted Pedulum System Setup (Room 40) Cotrol program (VC++) Couter Rotory Ecoder DAC (cotrol commad) Lear Motor Lear Ecoder Cotrol PC Couter Motor Drver
Cotrol Expermet ) Cotrol error of pedulum Referece commad = 80 deg ) Stadg-o cotrol wth Swg moto Ital posture = 0 deg. Make a swg moto ope-loop mode Whe the pedulum arrves aroud 80 deg chage to closed-loop cotrol mode posto cotrol for ref. put = 80 deg Cotrol program GUI 3 Reportg ad Presetato Term Project Report Modelg of Cart-pedulum moto Basc moto aalyss & Stablty aalyss Cotrol desg & Estmator desg - Trasfer fucto, root-locus, frequecy respose - State feedback & state estmator Smulato results Expermet results Dscusso Report(PPT fle) 제출 & 0 mute Presetato: /9( 월 ) Smulato result Graphc amato s preferred ) Expermet vdeo Fal Exam /( 목 ) 4
연립상미방수치해를위한 Ruge-Kutta 법 (RK4 Numercal Itegrato Method for IVP of Smultaeous ODEs) 미분방정식 : 미지함수와미지함수의도함수로구성되어있는방정식 상미분방정식 (Ordary dfferetal equato, ODE) 함수가한개의독립변수만포함 편미분방정식 (Partal dfferetal equato, PDE) 함수가한개이상의독립변수를포함 미방에대한초기치문제 : IVP(Ital Value Problem) 5 Mass-Damper-Sprg System 의예 y k c m X-방향의운동방정식유도 Ft () Ft () mx x kx cx m Mass-damper-sprg system: Ft () kx cx = mx : k( 스프링상수), c( 댐핑계수) mx + cx + kx = F() t ( 차미방) ( 차미방) 원연립 차미방으로 y = y let x= y, x = y F c k y = y y m m m y() t = x() t 차미방에대한수치적분알고리즘적용 를구함 y() t = x () t 6
Pedulum 의예 상미분방정식 (ODE) ) 선형상미방 (Lear ODE): 선형함수만을포함하는미분방정식 ) 비선형상미방 (Nolear ODE): 비선형함수가포함된미분방정식 Pedulum( 진자 ) 비선형상미방특성 θ x T (teso) l m m mx m y mg my Mass-damper-sprg system: x= lsθ x= l θ cosθ l θ sθ y = lcosθ y = l θ sθ l θ cosθ 진자의 Tagetal 방향의운동방정식 g mg sθ = ml θ θ + sθ = 0 ( 비선형상미방) l 선형화된운동방정식 ( for small θ s θ θ,cosθ ) g g θ + sθ = 0 θ + θ = 0 ( 선형상미방) l l mlθ = mlθ = m 수치해구하기 : let θ = y, θ = y y = y g y = y l 차미방에대한 ( 수치적분알고리즘) y () t = θ () t y() t = θ () t 7 상미방에대한수치적분 차상미방 dy = f( x, y) y = g( x) 에해당하는수치해 (x값에대한 y의 data ) 를찾음. 차상미방에대한수치적분알고리즘의일반적인형태 y = y + φh + 새로운값 (at x= x ) (at ) ( ) + = 이전값 x= x + 기울기 구간간격 h 각알고리즘의차이 기울기φ를결정하는방법의차이 8
Euler Method dy 차상미방 : = y = f( x, y) y+ = y + φ h 기울기 φ 를시작점에서의도함수로사용 φ = y = f( x, y ) dy y y y y x x h + + = = y = f x y + + (, ) y = y + f( x, y ) h: Euler method y + φ h= f( x, y ) h=δy y <Euler 법에서의오차의누적 > 9 개선된 Euler 법 (RK 법 ) [] Euler 법 : y = y + + f( x, y) h 기울기 φ = y = f( x, y) : 구간시작점에서의기울기 [] Heu법 : y = + yh + y 0 y (, ) (, ) + y f x y + f x y + + + 기울기 φ = y = = : 구간시작과끝기울기의평균 [3] 중앙점법 (Mdpot method): y = y + + f ( x + /, y+ /)h h 기울기 φ = y + / = f( x+ /, y+ / ) : 구간중앙점에서의기울기 Mdpot: x + / h y = y + f ( x, y ) + / 0
개선된 Euler 법 (RK 법 ) <Euler 법과 Heu 법의성능비교 > Ruge-Kutta 법 dy 차상미방 = f( x, y) 에대한수치해 y = y + φ( x, y, h) h + φ( x, y, h): 적분구간 ( h) 에서의대표적인기울기 Icremetal fucto ( 증분함수 ) 증분함수의일반적인형태 φ ( x, y, h) = ak + ak+ + ak a ~ a 은상수, k ~ k 은기울기값들 k = f( x, y) k = f( x + ph, y + qkh) k 3 = f ( x + p h, y + q k h + q k h ) k = f ( x + p h, y + q,kh+ q,kh+ + q, k h) RK 법의분류 ) = : 차 RK 법 (RK) Euler 법 ) = : 차 RK 법 (RK) Heu 법, Mdpot 법 3) = 3 : 3 차 RK 법 (RK3) 4) = 4 : 4 차 RK 법 (RK4)
3 차 Ruge-Kutta 법 [RK3] y = y + + ( k 4k k3) h 6 + + 4 기울기 k, k, k에각각,, 가중치 3 6 6 6 구간시작점의기울기 k = f( x, y) k = f( x + h, y + kh) k3 = f ( x + h, y kh+ kh ) 구간끝점의기울기 구간중앙점의기울기 3 4 차 Ruge-Kutta 법 : 가장보편적인방법 [RK4] y+ = y + ( k+ k + k3 + k4) h k, k, k, k 3 4 6,,, k = f ( x, y ) 구간시작점의기울기 k = f( x + h, y + kh) 구간중앙점의기울기 k 3 = f( x + h, y + kh ) 구간중앙점의기울기 k4 = f( x + h, y + k3h) 구간끝점의기울기 기울기 에각각 가중치 6 6 6 6 4
연립미분방정식 차미방 개의 차연립미방으로변환 차미방또는개의 차연립미방의해를구하기위해서는 개의조건이필요 dy 차상미방 : = y = f( x, y) 원연립 차상미방 dy = f( x, y, y,, y) dy = f( x, y, y,, y) : y ~ dy = f( x, y, y,, y) y 각각의초기치 개필요 5 차미분방정식 원 차연립방정식 d y ( ) ( ) 차상미방 : = y = f( x, y, y, y,, y ) ( ) ( yy,, y,, y ) 에대한초기치 개필요 ( ) Let ( y = y, y = y, y = y3,, y = y ) The, 차상미방 dy y = = f = y dy y = = f = y 3 : 원연립 차상미방으로변환 dy y = = f = f( x, y, y,, y) 차상미방의경우 : y = f( x, y, y ) y(0) = y0 ( yy, ) 에대한초기치 개 y (0) y 필요 = 0 y = y y = f = y y Let (0) = y0 = wth = = x (0) y y = y f f(, y, y) y y 0 6
차미분방정식에대한 RK4 알고리즘 d y y(0) = y0 y f( x, y, y ) wth y (0) = y 0 Let = = dy = y = f ( x, y, y ) y = y (0) y = y 0 y dy wth y y (0) y = = 0 = f ( x, y, y ) = f ( x, y, y ) For ( y = f) : y+, = y,+ ( k, + k, + k3, + k4, ) h RK4 수치해 : 6 For ( y = f) : y+, = y, + ( k, + k, + k3, + k4, ) h 6 k, = f ( x, y,, y,) for y = f k, f( x, y,, y, ) y = for = f k, = f ( x + 0.5 h, y, + 0.5 k,h, y, + 0.5k,h) for y = f k, = f ( x + 0.5 h, y,+ 0.5k,h, y, + 0. 5 k,h) y = f for k3, = f ( x + 0. 5h, y,+ 0.5k,h, y, + 0.5k,h) for y = f k3, f ( x 0. 5, h y, 0.5 k,h, y, 0.5 k,h) y = f = + + + for k4 = 3 4, f ( x + h, y,+ k3,h, y, + k3,h) for y = f k4, f ( x + h, y, k3, h, y, k3,h) y = f = + + for 7 모듈화된 RK4 코드작성 (xout 구간내에서 h 간격으로적분 ) (xout 간격으로적분값저장 ) (RK4 method 에의해 h 구간끝의적분값계산 ) xout 간격으로출력 ( 미분방정식계산 ) 8