205 년고등자동제어 Term Project Dynamic Simulation and Control Experiment for Cart-Pendulum System Dynamic Simulation Modeling and Control Design Control Experiment
Control Simulation and Experiment of a Cart-Pendulum System Mission. Control and Estimator Design Derivation of the (coupled nonlinear) dynamic equation of motion. Design of a PID control law with joint angle feedback Design of a state feedback control law with joint angle feedback Design of a state estimator to reconstruct full-state (by assuming that the cart position and the pendulum angle are measurable) Mission 2. Dynamic Motion Simulation (Matlab or C++) Numerical integration of the (nonlinear) dynamic equation of motion (apply Runge-Kutta algorithm) Control simulation of Inverted pendulum Data plotting of simulation results Mission 3. Inverted pendulum Control experiment Understand the H/W and S/W architecture of the Linear-Motor control system Make a control function to implement your controller Perform control gain tuning Data acquisition and plotting of experimental results 2
Control Simulation and Experiment of a Cart-Pendulum System [] (subroutine RK4) Write RK4(Runge-Kutta 4 th order) routine for numerical integration of the dynamic equation of motion (Input: cart driving force u(t) à Output: cart and pendulum motion (x & theta) Two 2nd order equations of motion which are dynamically coupled: 반드시마찰력을고려할것! éh H2 ù ì && q ü ìc & g q ü ì ( q, x) ü ì0ü ê H2 H ú í ý + í ý + í ý = í ý ë 22 û î&& xþ îc g 2x& þ î 2( q, x) þ îuþ - ì && q ü éh H2 ù æ ì0 ü ìc & q ü ì g( q, x) ü ì f( q, & q, x, x& ) ü í ý = ê î && x þ ë H2 H ú + - = 22 û ç í ý í ý í ý u c g 2x 2( q, x) í ý è î þ î & î þ f2( q, & þ ø î q, x, x&, u) þ Four simultaneous st order equations: Numerical integration using RK4 algorithm RK4 integration step = sampling time (ex) ms = 0.00 sec Plant parameters : m =? kg, m =? kg, 진자길이 ( 막대 ) l = cart pendulum? m 3
[2] (subroutine JointCtrl) Write joint control function Joint torque = control input + arbitrary disturbance(your assumption!) æ - PID control: K( s) = ç k p + kds + ki e( s) è s ø u( t) = k e( t) + k e& ( t) + k e( t) dt p discrete-time representation: D I ò æ e( k) - e( k -) u( k) = k pe( k) + kd ç + kite( k) + u( k -) è T ø Perform your Gain tuning! following the order: k k k p D I - Disturbance input: (ex.) First suggestion d( t) = Dsin( w t) or square wave(with magnitude D = 0.2u, w = 0 ~ 50rad / sec max d d D, frequency w ) Investigate how the control error is varied according to ( D and w ) d d - - Total torque input : t ( t) = u( t) + d( t) Consider actuator torque limit : - u u( t) u (ex.) u = 5 ~ 50[ N] max max max 4
[3] (subroutine Estimator) Write the state estimator function [4] (subroutine Main) ) When using MATLAB à direct m-file coding is preferred rather than using SIMULINK 2) When using Microsoft VISUAL C++ à Combined State Feedback and Estimator Loop motor torque limit disturbance d( t) Cart-pendulum (nonlinear motion model) ref. command q ( t d ) + - e( t) K(s) u( t) + t ( t) Coupled dynamic equation of motion x( t) q ( t) ˆx ˆx& qˆ ˆ q & State estimator 5
PID control loop & State estimation loop motor torque limit disturbance d( t) Cart-pendulum Motion model ref. command q ( t d ) + - e( t) K(s) u( t) + t ( t) Coupled dynamic equation of motion x( t) q ( t) qˆ ˆx ˆ & q ˆx& State estimator 6
Example of MatLab Code for RK4 Integration (script m-file) % main.m % === Example of RK4 Integration algorithm %------- 2nd order ODE ---------------------- % y_ddot + a*y_dot + b*y = 0 % with (I.C.) y(t=0) =, y_dot(t=0) = clear; %---------- System Parameters -------------- a = 0.; b = 00.; % (I.C.) x() = 3.0; x2() = 0.0; y + ay + by = 0 (I.C.) y(0) = 3, y (0) = 0 Spring을 3m 앞으로당겼다놓는경우 k 2 b = 00 = = wn Natural freq.( 고유진동수), wn = 0 rad / sec m a = 0 = 2zw Damping coeff.( 감쇠계수), z = Þ Try different set of n ( w, z )! n h = 0.0; % Integration time interval tf = 3.0; % Final time t = [ 0: h: tf]'; %--- RK4 integration start ----------------------------- for i = : : size(t,)- % Slopes k, k2, k3, k4 k = x2(i) ; k2 = -a*x2(i) - b*x(i); k2 = x2(i) + 0.5*k2*h ; k22 = -a*( x2(i) + 0.5*k2*h ) - b*( x(i) + 0.5*k*h ); k3 = x2(i) + 0.5*k22*h ; k32 = -a*( x2(i) + 0.5*k22*h ) - b*( x(i) + 0.5*k2*h ); k4 = x2(i) + k32*h ; k42 = -a*( x2(i) + k32*h ) - b*( x(i) + k3*h ); % Updated value x(i+) = x(i) + (k + 2*k2 + 2*k3 + k4)*h/6; x2(i+) = x2(i) + (k2 + 2*k22 + 2*k32 + k42)*h/6; end; figure(); subplot(2); plot(t, x); xlabel('time(sec)'); ylabel('x'); title('position'); grid; subplot(22); plot(t, x2); xlabel('time(sec)'); ylabel('x2'); title('velocity'); grid; 7
Example of MatLab Code for RK4 Integration (function m-file): % === Example of RK4 Integration algorithm % It requires m-files with subfunctions rk4.m & dyn_eqn.m %======================================== % 화일명 : rk4_main.m or 임의명.m function rk4_main() % 각 subfunction 을독립적인 m-file 로저장하면 main 함수이름생략가능 %------- 2nd order ODE ---------------------- % y_ddot + a*y_dot + b*y = u(t) with (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; % Integration time interval % (I.C.) x(,:) = [3.0 0.0]; %x() = 3.0; x2() = 0.0; tf = 3.0; % Final time t = [ 0: h: tf]'; % Subfunction % rk4.m function x = rk4( x, u, a, b, h ) % Slopes k, k2, k3, k4 <-- 2 x vectors k = dyn_eqn( x, u, a, b, h ); k2 = dyn_eqn( x + 0.5*k*h, u, a, b, h ); k3 = dyn_eqn( x + 0.5*k2*h, u, a, b, h ); k4 = dyn_eqn( x + k3*h, u, a, b, h ); x = x + h*( k + 2*k2 + 2*k3 + k4 )/6; % Subfunction 2 % dyn_eqn.m function f = dyn_eqn( x, u, a, b, h ) f(:,) = x(:,2); f(:,2) = -a*x(:,2) - b*x(:,) + u; %--- RK4 integration start ----------------------------- for i = : : size(t,)- % Input u(i) = 0; x(i+,:) = rk4( x(i,:), u(i), a, b, h ); end; figure(); subplot(2); plot(t, x(:,)); xlabel('time(sec)'); ylabel('x'); title('position'); grid; subplot(22); plot(t, x(:,2)); xlabel('time(sec)'); ylabel('x2'); title('velocity'); grid; 8
Example of MatLab Code for RK4 Integration (inline function) % rk4_inline_fn.m function rk4_inline_fn() %---------- System Parameters -------------- a = 3.; % c/m b = 00.; % k/m h = 0.0; % Integration time interval x(,:) = [3.0 0.0]; %x() = 3.0; x2() = 0.0; % (I.C.) tf = 3.0; t = [ 0: h: tf]'; % inline functions for % y_ddot + a*y_dot + b*y = u(t) with (I.C.) y(0), y_dot(0) %----------------------------------- f = inline('x2'); f2 = inline('-a*x2 - b*x + u', 'x','x2','u','a','b'); %--- RK4 integration ----------------------------- for i = : : size(t,)- u(i) = 0; % Input k = f(x(i,2)); k2 = f2(x(i,), x(i,2), u(i), a, b); k2 = f(x(i,2)+0.5*k*h); k22 = f2(x(i,)+0.5*k*h, x(i,2)+0.5*k2*h, u(i), a, b); k3 = f(x(i,2)+0.5*k2*h); k32 = f2(x(i,)+0.5*k2*h, x(i,2)+0.5*k22*h, u(i), a, b); k4 = f(x(i,2)+k3*h); k42 = f2(x(i,)+k3*h, x(i,2)+0.5*k32*h, u(i), a, b); x(i+,) = x(i,) + h*( k + 2*k2 + 2*k3 + k4 )/6; x(i+,2) = x(i,2) + h*( k2 + 2*k22 + 2*k32 + k42 )/6; end; 9
Inverted Pendulum on the Moving Cart Graphic animation using VC++ with OpenGL or Matlab Experimental setup & Basic C++ code System Parameters Mass of cart Pendulum mass Pendulum length Damping coefficient of cart Damping coefficient of pendulum Sampling time 2.54 kg 0.56 kg 400 mm 0.~0.3 0.~0.3 ms 0
Simulation results ) Confirm the free motion of pendulum and cart (openloop case) With respect to the initial condition change With respect to the damping coefficient change 2) Closed-loop control simulation Normal pendulum (with stable eq. point = 0 deg ) Inverted pendulum (with unstable eq. point = 80 deg ) Investigate how the control performance is varied according to the changes of PID control gain and the magnitude of disturbances. 3) State estimator simulation Investigate how the estimation performance is varied according to the change of estimator gain.
Cart & Inverted Pendulum System Setup (Room 402) Control program (VC++) Counter Rotory Encoder DAC (control command) Linear Motor Linear Encoder Control PC Counter Motor Driver 2
Control Experiment ) Control error of pendulum Reference command = 80 deg 2) Standing-on control with Swing motion Ø Initial posture = 0 deg. Ø Make a swing motion in open-loop mode Ø When the pendulum arrives around 80 deg à change to closed-loop control mode à position control for ref. input = 80 deg Control program GUI 3
Report and Presentation Ø Term Project Report Modeling of Cart-pendulum motion Basic motion analysis & Stability analysis Control design & Estimator design - Transfer function, root-locus, frequency response - State feedback & state estimator Simulation results Experiment results Discussion Ø Report(PPT file) 제출및발표 (0 분 ): 2/7( 목 ) Simulation result à Graphic animation is preferred ) Experiment video ØFinal Exam 2/8( 금 ) 4
연립상미방수치해를위한 Runge-Kutta 법 (RK4 Numerical Integration Method for IVP of Simultaneous ODEs) 미분방정식 : 미지함수와미지함수의도함수로구성되어있는방정식 상미분방정식 (Ordinary differential equation, ODE) 함수가한개의독립변수만포함 편미분방정식 (Partial differential equation, PDE) 함수가한개이상의독립변수를포함 미방에대한초기치문제 : IVP(Initial Value Problem) 5
Mass-Damper-Spring System 의예 y k c m F( t) x X-방향의운동방정식유도 F( t) mx&& kx cx& m Mass-damper-spring system: F( t) - kx - cx& = mx && : k( 스프링상수), c( 댐핑계수) mx && + cx& + kx = F( t) (2 차미방) (2 차미방) 2 원연립 차미방으로 æ y& = y2 let x = y, x& = y ç 2 F c k ç y& 2 = - y2 - y è m m m ø æ y( t) = x( t) 차미방에대한수치적분알고리즘적용 ç 를구함 è y2( t) = x& ( t) ø 6
Pendulum 의예 상미분방정식 (ODE) ) 선형상미방 (Linear ODE): 선형함수만을포함하는미분방정식 2) 비선형상미방 (Nonlinear ODE): 비선형함수가포함된미분방정식 Pendulum( 진자 ) à 비선형상미방특성 x T (tension) q l y m m mg Mass-damper-spring system: 2 x = l sinq && x = l && q cosq - l & q sinq 2 y = l cosq && y = -l && q sinq - l & q cosq 진자의 Tangential 방향의운동방정식 g - mg sinq = ml && q && q + sinq = 0 ( 비선형상미방) l 선형화된운동방정식 ( for small q sin q» q,cosq» ) && g g q + sinq = 0 && q + q = 0 ( 선형상미방) l l mlq &2 = m mx&& mlq && my&& = m 수치해구하기 : let q = y, & q = y2 æ y& = y2 ç g ç y& 2 = - y è l ø ( 차미방에대한 ) 수치적분알고리즘 æ y( t) = q ( t) ç y2( t) = & è q ( t) ø 7
상미방에대한수치적분 차상미방 dy = f ( x, y) y = g( x) 에해당하는수치해 (x값에대한 y의 data ) 를찾음. dx 차상미방에대한수치적분알고리즘의일반적인형태 y = y + fh i+ i Û 새로운값(at x = x ) = 이전값(at x = x ) + 기울기 구간간격( h) i+ 각알고리즘의차이 기울기f를결정하는방법의차이 i 8
Euler Method dy 차상미방 : = y = f ( x, y) dx yi+ = yi + fh 기울기f를시작점에서의도함수로사용 f = y = f ( x, y ) æ dy y - y y - y ç è dx ø x x h i i i i+ i i+ i» = = yi = f xi yi i i+ - i i+ i i i (, ) y = y + f ( x, y ) h: Euler method y i + f h = f ( x, y ) h = Dy i y i i <Euler 법에서의오차의누적 > 9
개선된 Euler 법 (RK2 법 ) [] Euler 법 : yi+ = yi + f ( xi, yi ) h 기울기 f = y i = f ( xi, yi ) : 구간시작점에서의기울기 [2] Heun법 : yi+ = yi + y ih y + y f x y f x y 기울기 = i = = 2 2 : 구간시작과끝기울기의평균 0 ( i, i ) + ( i+, ) i i+ i+ f y [3] 중앙점법 (Midpoint method): y i+ = y + i f ( x, y ) h i+ / 2 i+ / 2 기울기 f = y = f ( x, y i+ / 2 i+ / 2 i+ / 2 : 구간중앙점에서의기울기 Midpoint: æ xi + / 2 ç ç è ø h y = y + f ( x, y ) i+ / 2 i i i 2 ) 20
개선된 Euler 법 (RK2 법 ) <Euler 법과 Heun 법의성능비교 > 2
Runge-Kutta 법 dy 차상미방 = f ( x, y) 에대한수치해 dx y = y + f( x, y, h) h i+ i i i f( x, y, h) : 적분구간 ( h) 에서의대표적인기울기 i i Incremental function ( 증분함수 ) 증분함수의일반적인형태 f( x, y, h) = a k + a k + L+ a k i i 2 2 n n a ~ a 은상수, k ~ k 은기울기값들 n æ k = f ( xi, yi ) ç k = f ( x + p h, y + q k h) 2 i i ç k3 = f ( xi + p2h, yi + q2kh + q22k2h) ) n = : 차 RK 법 (RK) Euler 법 2) n = 2 : 2 차 RK 법 (RK2) Heun 법, Midpoint 법 3) n = 3 : 3 차 RK 법 (RK3) 4) n = 4 : 4 차 RK 법 (RK4) n ç ç M M ç kn = f ( xi + pn- h, yi + qn-,kh + qn-,2k2h + + qn-, n-kn-h) è L ø RK 법의분류 22
3 차 Runge-Kutta 법 [RK3] yi+ = yi + k + k k 6 ( 4 + ) 2 3 h 4 기울기 k, k, k 에각각,, 가중치 2 3 6 6 6 구간시작점의기울기 æ k = f ( xi, yi ) ç ç k2 = f ( xi + h, yi + kh) 구간중앙점의기울기 2 2 ç k3 = f ( xi + h, yi - kh + 2 k2h) 구간끝점의기울기 è ø 23
4 차 Runge-Kutta 법 : 가장보편적인방법 [RK4] yi+ = yi + ( k + 2k2 + 2k3 + k4 ) h k, k, k, k 2 3 4 6,,, æ k = f ( xi, yi ) 구간시작점의기울기 ç ç k2 = f ( xi + h, yi + kh) 구간중앙점의기울기 ç 2 2 ç k3 = f ( xi + h, yi + k2h) 구간중앙점의기울기 2 2 ç è k4 = f ( xi + h, yi + k3h) 구간끝점의기울기 ø 기울기 에각각 2 2 가중치 6 6 6 6 24
연립미분방정식 n 차미방 à n 개의 차연립미방으로변환 n 차미방또는 n 개의 차연립미방의해를구하기위해서는 n 개의조건이필요 dy 차상미방 : = y = dx f ( x, y) n 원연립 차상미방 ì dy ü ï = f( x, y, y2, L, yn) dx ï ï dy ï 2 ï = f2( x, y, y2, L, yn) ï í dx ý : y ~ ï M M ï ï dy ï n ï = fn( x, y, y2, L, yn) ï î dx þ y n 각각의초기치 n 개필요 25
n 차미분방정식 à n 원 차연립방정식 n d y ( n) ( n-) n 차상미방 : = y = f ( x, y, y, y, L, y ) n dx ( n-) ( y, y, y, L, y ) 에대한초기치 n개필요 Let ( n-) ( y = y, y = y2, y = y3, L, y = yn ) Then, n 차상미방 ì dy y ü ï = = f = y2 dx ï ï dy ï 2 ï y 2 = = f2 = y ï 3 í dx ý:n원연립 차상미방으로변환 ï M M ï ï dy ï n ï y n = = fn = f ( x, y, y2, L, yn) ï î dx þ 2차상미방의경우 : y = f ( x, y, y ) æ y(0) = y0 ( y, y ) 에대한초기치 2 개ç y (0) = y 필요 è 0 ø y = y ì y = f = y ü æ y (0) = y Let ç with = = æ è y = y2 ø 2 0 í y2 f2 f ( x, y, y2) ý ç y2(0) y î þ è = 0 ø 26
2 차미분방정식에대한 RK4 알고리즘 2 d y y(0) = y0 y f ( x, y, y æ ) with 2 dx y (0) = y 0 Let = = ç è ø ì dy = y = f ( x, y, y ) 2 2 æ y = y ï dx ï æ y (0) = y 0 ç Þ y í dy ý with ç y2 2 y (0) y è = ø = 2 0 ï = f ( x, y, y ) = f ( x, y, y ) ï è ø 2 2 2 î dx ü þ For ( y = f ) : yi+, = yi, + ( k, + 2k2, + 2k3, + k4, ) h RK4 수치해 : 6 For ( y 2 = f2 ) : yi+,2 = yi,2 + ( k,2 + 2k2,2 + 2k3,2 + k4,2 ) h 6 æ k, = f ( xi, yi,, yi,2) for y = f ç k, 2 = f2( xi, yi,, yi, 2) y è for = f 2 2 ø æ k2, = f ( xi + 0.5 h, yi, + 0.5 k,h, yi,2 + 0.5k,2h) for y = f ç k2,2 = f2 ( xi 0.5 h, yi, + 0.5k,h, yi,2 + 0. 5k,2h) y = f è + for 2 2 ø æ k3, = f ( xi + 0. 5h, yi, + 0.5k2,h, yi,2 + 0.5k2,2h) for y = f ç k3, 2 f2 ( xi 0. 5 h, yi, 0.5 k2,h, yi,2 0.5 k2,2h) y = f è = + + + for 2 2 ø æ k4, = f ( xi + h, yi, + k3,h, yi,2 + k3,2h) for y = f ç k4,2 f2 ( xi + h, yi, k3, h, yi,2 k3,2h) y = f è = + + for 2 2 ø 27
모듈화된 RK4 코드작성 (xout 구간내에서 h 간격으로적분 ) (xout 간격으로적분값저장 ) (RK4 method 에의해 h 구간끝의적분값계산 ) xout 간격으로출력 à ( 미분방정식계산 ) 28