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

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 =

2、9.8; l = 0.3; q = (m+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(response of pendulum position to an impulse disturbance under pid contro

3、l: kp = 1, ki = 1, kd = 1);kp = 100; 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(response of pendulum position to an impulse disturbance under pid control: kp = 100, ki = 1, kd = 1);kp = 100; ki = 1; kd = 20; c = pid(kp,ki,kd);

4、t = feedback(p_pend,c); t=0:0.01:10; impulse(t,t) axis(0, 2.5, -0.2, 0.2); title(response of pendulum position to an impulse disturbance under pid control: kp = 100, ki = 1, kd = 20); 这时，小车的位置?传递函数： 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

5、 = feedback(1,p_pend*c)*p_cart; t = 0:0.01:5; impulse(t2, t); title(response of cart position to an impulse disturbance under 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

6、= 0.006;g = 9.8;l = 0.3;p = i*(m+m)+m*m*l2; %denominator for the a and b matricesa = 0 1 0 0; 0 -(i+m*l2)*b/p (m2*g*l2)/p 0; 0 0 0 1; 0 -(m*l*b)/p m*g*l*(m+m)/p 0;b = 0; (i+m*l2)/p; 0; m*l/p;c = 1 0 0 0; 0 0 1 0;d = 0; 0;states = x x_dot phi phi_dot;inputs = u;outputs = x; phi;sys_ss = ss(a,b,c,d,st

7、atename,states,inputname,inputs,outputname,outputs);poles = eig(a)poles = 0 -5.6041 -0.1428 5.5651线性二次型整定 (lqr)1 检查可控性co = ctrb(sys_ss);controllability = rank(co)controllability = 4设 r=1q = c*cq = 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0q = c*c;r = i;k = lqr(a,b,q,r)ac = (a-b*k);bc = b;cc = c;dc = d;states =

8、 x x_dot phi phi_dot;inputs = r;outputs = x; phi;sys_cl = ss(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),plot);set(get(ax(1),ylabel),string,cart position (m)set(get(ax(2),ylabel),string,

9、pendulum angle (radians)title(step response with lqr control)k = -1.0000 -1.6567 18.6854 3.4594q = c*c;q(1,1) = 5000;q(3,3) = 100r = 1;k = 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_cl = ss(ac,bc,cc,dc,statename,states,inputname,inputs,

10、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),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 = 5000 0 0 0 0 0 0 0 0 0 100 0 0 0 0 0k = -7

11、0.7107 -37.8345 105.5298 20.9238增加补偿系数：cn = 1 0 0 0;sys_ss = ss(a,b,cn,0);nbar = rscale(sys_ss,k)nbar = -70.7107sys_cl = ss(ac,bc*nbar,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),plot);set(get

12、(ax(1),ylabel),string,cart position (m)set(get(ax(2),ylabel),string,pendulum angle (radians)title(step response with precompensation and lqr control) 基于能观器的控制检查能观性：ob = obsv(sys_ss);observability = rank(ob)observability = 4 能观器的根要比控制器的根快5到10倍的收敛控制器的根：poles = eig(ac)poles = -8.4910 + 7.9283i -8.4910

13、- 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.0010 1.6992 -0.0402 -0.0014 0.0832 -0.0762 1.7604 ace = (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

14、 e1 e2 e3 e4;inputs = r;outputs = x; phi;sys_est_cl = ss(ace,bce,cce,dce,statename,states,inputname,inputs,outputname,outputs);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),ylabel),st

15、ring,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 rscale(a,b,c,d,k) % finds the scale facto

16、r 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; if (nargin1=2),% system form a