基于状态空间方程的倒立摆小车的控制方法设计

1、倒立摆小车的建模系统参数：(M)小车质量0.5 kg(m)倒立摆质量0.2 kg(b)小车的阻尼系数0.1 N/m/sec(l)倒立摆长度0.3 m(I)倒立摆转动惯量0.006 kg.m2(F)施加在小车上的力(x)小车的位置坐标(theta)倒立摆与垂直方向的夹角（从垂直向下开始计）动力学建模：考虑水平方向的力矩平衡：以上两式合并得到：考虑垂直施加在倒立摆上的力及质心力矩平衡方程：消去P N ，得到如下公式简化：简化后模型：传递函数模型：设：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这时，小车的位置 ?传递函数：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');基于状态空间方程的倒立摆小车的控制方法设计? 开环根? (LQR)? 增加补偿? 基于观测器的控制1 状态空间方程设计目标：1倒立摆倒立状态2小车向右移动 0.2 米首先假设系统状态全部测量获得：求开环的根，即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设 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增加补偿系数：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置根法设置能观器的根：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