基于狀態(tài)空間方程的倒立擺小車的控制方法設(shè)計

1、倒立擺小車的建模系統(tǒng)參數(shù)：(M)小車質(zhì)量0.5 kg(m)倒立擺質(zhì)量0.2 kg(b)小車的阻尼系數(shù)0.1 N/m/sec(l)倒立擺長度0.3 m(I)倒立擺轉(zhuǎn)動慣量0.006 kg.m2(F)施加在小車上的力(x)小車的位置坐標(biāo)(theta)倒立擺與垂直方向的夾角（從垂直向下開始計）動力學(xué)建模：考慮水平方向的力矩平衡：以上兩式合并得到：考慮垂直施加在倒立擺上的力及質(zhì)心力矩平衡方程：消去P N ，得到如下公式簡化：簡化后模型：傳遞函數(shù)模型：設(shè)：PID 控制：或者如下表示：M = 0.5; m = 0.2; b = 0.1; I = 0.006; g = 9.8; l = 0.3; q =(M

2、+m)*(I+m*l2)-(m*l)2;s=tf('s');P_pend=(m*l*s/q)/(s3+ (b*(I+ m*l2)*s2/q- (M+ m)*m*g*l)*s/q- b*m*g*l/q);Kp=1;Ki=1;Kd=1;C=pid(Kp,Ki,Kd);T=feedback(P_pend,C);t=0:0.01:10;impulse(T,t)title('ResponseofPendulumPosition to an Impulse Disturbance under PID Control: Kp =1, Ki = 1, Kd = 1');Kp=1

3、00;Ki=1;Kd=1;C=pid(Kp,Ki,Kd);T=feedback(P_pend,C);t=0:0.01:10;impulse(T,t)axis(0,2.5,-0.2,0.2);title('ResponseofPendulumPositiontoanImpulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd =1' );Kp=100;Ki=1;Kd=20;C=pid(Kp,Ki,Kd);feedback(P_pend,C);t=0:0.01:10;impulse(T,t)axis(0,-0.2,0.2);

4、title('ResponseofPendulumPositionImpulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd= 20');T=2.5,toan這時，小車的位置 ?傳遞函數(shù)：P_cart=(I+m*l2)/q)*s2-(m*g*l/q)/(s4+(b*(I+m*l2)*s3/q - (M + m)*m*g*l)*s2/q - b*m*g*l*s/q); T2 = feedback(1,P_pend*C)*P_cart; t = 0:0.01:5; impulse(T2, t);title('Res

5、ponse of Cart Position to an Impulse Disturbanceunder PID Control: Kp = 100, Ki = 1, Kd = 20');基于狀態(tài)空間方程的倒立擺小車的控制方法設(shè)計? 開環(huán)根? (LQR)? 增加補(bǔ)償? 基于觀測器的控制1 狀態(tài)空間方程設(shè)計目標(biāo)：1倒立擺倒立狀態(tài)2小車向右移動 0.2 米首先假設(shè)系統(tǒng)狀態(tài)全部測量獲得：求開環(huán)的根，即A 的特征值M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;p = I*(M+m)+M*m*l2;%denominator for the

6、A and B matricesA=0 100;0-(I+m*l2)*b/p(m2*g*l2)/p0;0001;0-(m*l*b)/pm*g*l*(M+m)/p0;B=0;(I+m*l2)/p;0;m*l/p;C=1000;0010;D=0;0;states = 'x''x_dot''phi''phi_dot'inputs = 'u' ;outputs = 'x' ;'phi'sys_ssss(A,B,C,D, 'statename',states,'inpu

7、tname',inputs,);poles = eig(A)poles =0-5.6041='outputname',outputs-0.14285.5651線性二次型整定(LQR)1檢查可控性co = ctrb(sys_ss);controllability = rank(co)controllability = 4設(shè) R=1 Q = C'*CQ =1000000000100000Q = C'*C;R=I;K = lqr(A,B,Q,R)Ac = (A-B*K);Bc = B;Cc = C;Dc = D;states = 'x''

8、;x_dot''phi''phi_dot'inputs = 'r'outputs = 'x' ;'phi'sys_clss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);=t = 0:0.01:5;r =0.2*ones(size(t); y,t,x=lsim(sys_cl,r,t);AX,H1,H2 = plotyy(t,y(:,1),t,y(:,2),'

9、;plot' );set(get(AX(1),'Ylabel'), 'String', 'cart position (m)')set(get(AX(2),'Ylabel'), 'String', 'pendulum angle (radians)')title( 'Step Response with LQR Control')K =-1.0000-1.656718.68543.4594Q = C'*C;Q(1,1) = 5000;Q(3,3) = 100R=1;K

10、 = lqr(A,B,Q,R)Ac = (A-B*K);Bc = B;Cc = C;Dc = D;states = 'x''x_dot''phi''phi_dot'inputs = 'r'outputs = 'x' ;'phi'sys_clss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);=t = 0:0.01:5;r =0.2*ones

11、(size(t);y,t,x=lsim(sys_cl,r,t);AX,H1,H2 = plotyy(t,y(:,1),t,y(:,2),'plot');set(get(AX(1),'Ylabel'), 'String', 'cart position (m)')set(get(AX(2),'Ylabel'), 'String', 'pendulum angle (radians)')title('Step Response with LQR Control')Q =5

12、00000000000010000000K =-70.7107 -37.8345 105.529820.9238增加補(bǔ)償系數(shù)：Cn = 1 0 0 0;sys_ss = ss(A,B,Cn,0);Nbar = rscale(sys_ss,K)Nbar =-70.7107sys_clss(Ac,Bc*Nbar,Cc,Dc,'statename',states,'inputname',inputs,' ,outputs);t = 0:0.01:5;r =0.2*ones(size(t);y,t,x=lsim(sys_cl,r,t);AX,H1,H2 = pl

13、otyy(t,y(:,1),t,y(:,2),'plot');set(get(AX(1),'Ylabel'), 'String', 'cart position (m)')set(get(AX(2),'Ylabel'), 'String', 'pendulum angle (radians)'title('Step Response with Precompensation and LQR Control''outputname)=基于能觀器的控制檢查能觀性：

14、ob = obsv(sys_ss);observability = rank(ob)observability =4能觀器的根要比控制器的根快5 到 10 倍的收斂控制器的根：poles = eig(Ac)poles =-8.4910 + 7.9283i-8.4910 - 7.9283i-4.7592 + 0.8309i-4.7592 - 0.8309i置根法設(shè)置能觀器的根：P = -40 -41 -42 -43;L = place(A',C',P)'L =1.0e+03 *0.0826 -0.00101.6992 -0.0402-0.00140.0832-0.0762

15、1.7604Ace = (A-B*K) (B*K);zeros(size(A) (A-L*C);Bce = B*Nbar;zeros(size(B);Cce = Cc zeros(size(Cc);Dce = 0;0;states = 'x''x_dot''phi''phi_dot'inputs = 'r'outputs = 'x' ;'phi'sys_est_clss(Ace,Bce,Cce,Dce,'statename',states,outputs);'

16、e1''e2''e3''e4' ;'inputname',inputs,'outputname'=t = 0:0.01:5;r = 0.2*ones(size(t);y,t,x=lsim(sys_est_cl,r,t);AX,H1,H2 = plotyy(t,y(:,1),t,y(:,2),'plot');set(get(AX(1),'Ylabel'), 'String', 'cart position (m)')set(get(AX(2),&

17、#39;Ylabel'), 'String', 'pendulum angle (radians)')title('Step Response with Observer-Based State-Feedback Control')Function Nbar=rscale(a,b,c,d,k)% Given the single-input linear system:%.%x = Ax + Bu%y = Cx + Du% and the feedback matrix K,% the function rscale(sys,K) or

18、rscale(A,B,C,D,K)% finds the scale factor N which will% eliminate the steady-state error to a step reference% for a continuous-time, single-input system% with full-state feedback using the schematic below:%/-%R+u| .|% -> N ->() ->| X=Ax+Bu |-> y=Cx -> y%-|-/%|%|<- K <-|%8/21/96 Yanjie Sun of the University of Michigan% under the supervision of Prof. D. Tilbury %6/12/98 John Yook, Dawn Tilbury revised error(nargchk(2,5,nargin);% - Determine which syntax is being used -nargin1 = nargin;i