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

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 u

3、nder PID Control: 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

4、; Kd = 20; 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 = 20'); 这时，小车的位置?传递函数： P_cart = (I+m*l2)/q)*s2 - (m*g*l/q)/(s4 + (b*(I + m*l2)*s3/q

5、- (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('Response of Cart Position to an Impulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd = 20');基于状态空间方程的倒立摆小车的控制方法设计§ 开环根§ (LQR)§ 增加补偿§ 基于观测器的控制1 状态空间方程 设计目标：1

6、 倒立摆倒立状态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 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;state

7、s = 'x' 'x_dot' 'phi' 'phi_dot'inputs = 'u'outputs = 'x' 'phi'sys_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);poles = eig(A)poles = 0 -5.6041 -0.1428 5.5651线性二次型整定 (LQR)1 检查可控性co = ctrb(sy

8、s_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 = 'x' 'x_dot' 'phi' 'phi_dot'inputs = 'r'outputs = 'x' 'phi'sys_c

9、l = 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

10、),'Ylabel'),'String','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' &

11、#39;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)

12、,'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 = -70.7107 -37.8345 105.5298 20.9238增加补偿系数：Cn = 1 0 0 0;sys_ss

13、= 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(AX(1),'Ylabel&

14、#39;),'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倍的收敛控制

15、器的根：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.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(si

16、ze(B);Cce = Cc zeros(size(Cc);Dce = 0;0;states = 'x' 'x_dot' 'phi' 'phi_dot' 'e1' 'e2' 'e3' 'e4'inputs = 'r'outputs = 'x' 'phi'sys_est_cl = ss(Ace,Bce,Cce,Dce,'statename',states,'inputname',inputs

17、,'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'),'String','pendulum angle (radians)&

18、#39;)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 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 | . |