二级倒立摆课程设计.docx

1、二级倒立摆课程设计程序一：二级最优控制 M=1.32;m1=0.05;m2=0.132;m3=0.236;l1=0.075;l2=0.275;a=-(m1+2*m2+2*m3)*l1 (4/3*m1+4*m2+4*m3)*l12 2*m2*l1*l2;-m2*l2 2*m2*l1*l2 4/3*m2*l22; M+m1+m2+m3 -(m1+2*m2+2*m3)*l1 -m2*l2a = -0.0590 0.0087 0.0054 -0.0363 0.0054 0.0133 1.7380 -0.0590 -0.0363 inv(a)ans = 5.1340 -0.0595 0.7483 190

2、.8063 -64.0554 5.1340 -64.0554 101.1736 -0.0595 g=9.8; b=(m1+2*m2+2*m3)*g*l1 m2*g*l2 1b = 0.5777 0.3557 1.0000 c=diag(b)c = 0.5777 0 0 0 0.3557 0 0 0 1.0000 d=inv(a)*cd = 2.9659 -0.0212 0.7483 110.2307 -22.7871 5.1340 -37.0055 35.9915 -0.0595A=0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;2.9659 -0.0212 0.748

3、3 0 0 0;110.2307 -22.7871 5.1340 0 0 0;-37.0055 35.9915 -0.0595 0 0 0;B=0 0 0 0.5777 0.3557 1.0000;C=1 0 0 0 0 1;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0; 0 0 0 0 0 1;D=0 0 0 0 0 0;p = eig(A)% 求向量K%Q = diag(100 50 5000 10 5 10);R=1;%Q = diag(10 30 90 1 1 1);R=0.1;Q = diag(259.11343 455.25514

4、451.91667 0.01478 0.11355 1.58004);R =1; K = lqr(A,B,Q,R);% 计算LQR控制的阶跃响应并画出曲线Ac = (A-B*K);Bc = B;Cc = C;Dc = D;T = 0:0.005:5;U = 0.2*ones(size(T);% 求阶跃响应并显示，小车位置为虚线，摆杆角度为实线Y,X = lsim(Ac,Bc,Cc,Dc,U,T); plot(T,Y(:,1),:,T,Y(:,2),-)legend(Cart Position,Pendulum Angle)grid% end 程序二：摆杆角度控制% pid1.m % 摆杆角度P

5、ID控制% 输入倒立摆传递函数 G1(S)=num1/den1M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l2)-(m*l)2; num1 = m*l/q 0 0;den1 = 1 b*(I+m*l2)/q -(M+m)*m*g*l/q -b*m*g*l/q 0;% 输入控制器PID数学模型 Gc(s)=numPID/denPIDKp = 100;Ki = 1; Kd = 20;numPID = Kd Kp Ki;denPID = 1 0;% 计算闭环系统传递函数G(s)=num/den% 多项式相乘num

6、= conv(num1,denPID);% 多项式相加den=polyadd(conv(denPID,den1),conv(numPID,num1 );% 求整个系统传递函数的极点r,p,k = residue(num,den);% 显示极点s = p % 求取多项式传递函数的脉冲响应t=0:0.005:5;impulse(num,den,t)% 显示范围：横坐标0-5，纵坐标0-10，此条语句参数可根据仿真输出曲线调整axis(0 5 -0.2 0.2)grid% end % polyadd.m % 求两个多项式之和 functionpoly = polyadd(poly1,poly2)if

7、 length(poly1) 0 poly = zeros(1,mz),short + long;else poly = long + short;end% end 程序三：一级最优控制% lqr1.m % 最优控制% 确定开环极点的程序如下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;A = 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 ;

8、C = 1 0 0 0; 0 0 1 0;D = 0; 0;p = eig(A)% 求向量Kx = 5000;y = 100;Q = x 0 0 0; 0 0 0 0; 0 0 y 0 0 0 0 0;R = 1; K = lqr(A,B,Q,R)% 计算LQR控制的阶跃响应并画出曲线Ac = (A-B*K);Bc = B;Cc = C;Dc = D;T = 0:0.005:5;U = 0.2*ones(size(T);% 求阶跃响应并显示，小车位置为虚线，摆杆角度为实线Y,X = lsim(Ac,Bc,Cc,Dc,U,T); plot(T,Y(:,1),:,T,Y(:,2),-)legend

9、(Cart Position,Pendulum Angle)grid% end 程序四：带增益的最优控制% lqr2.m % 最优控制（量纲匹配） % 确定开环极点的程序如下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;A = 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; p = ei

10、g(A);% 求向量Kx = 5000;y = 100;Q = x 0 0 0;0 0 0 0;0 0 y 00 0 0 0;R = 1; K = lqr(A,B,Q,R)% 计算LQR控制矩阵Ac = (A-B*K);Bc = B;Cc = C;Dc = D;% 计算增益NbarCn = 1 0 0 0;Nbar = rscale(A,B,Cn,0,K);Bcn = Nbar*B;% 求阶跃响应并显示，小车位置为虚线，摆杆角度为实线T = 0:0.005:5;U = 0.2*ones(size(T);Y,X = Lsim(Ac,Bcn,Cc,Dc,U,T);plot(T,Y(:,1),:,T

11、,Y(:,2),-)legend(Cart Position,Pendulum Angle)grid% end % rscale.m % 求取输入输出匹配系数functionNbar = rscale(A,B,C,D,K) s = size(A,1);Z = zeros(1,s) 1;N = inv(A,B;C,D)*Z;Nx = N(1:s);Nu = N(1+s);Nbar = Nu + K*Nx;% end 程序五：小车位置PID CONTROL% pid2.m % 小车位置PID控制 % 输入倒立摆传递函数 G1(s)=num1/den1,G2(s)=num2/den2M = 0.5;

12、m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l2) -(m*l)2; num1 = m*l/q 0 0;den1 = 1 b*(I+m*l2)/q -(M+m)*m*g*l/q -b*m*g*l/q 0;num2 = -(I+m*l2)/q 0 m*g*l/q;den2 = den1;% 输入控制器PID数学模型 Gc(s)=numPID/denPIDKp =100;Ki =1.1760; Kd = 20;numPID = Kd Kp Ki;denPID = 1 0;% 计算闭环系统传递函数G(s)=num/den% 多项式

13、相乘num = conv(num2,denPID);% 多项式相加den = polyadd(conv(denPID,den2),conv(numPID,num1 );% 求闭环系统极点r,p,k = residue(num,den);% 显示闭环系统极点s = p% 求取多项式传函的脉冲响应t=0:0.005:5;impulse(num,den,t) % 显示范围：横坐标0-5，纵坐标0-10，此条语句参数可根据仿真输出曲线调整axis(0 5 -0.2 0.5)grid% end 程序六：Z-N法设计PID% trans.m % 倒立摆传递函数、开环极点及开环脉冲响应z-n% 输入倒立摆传

14、递函数 G(S)=num/den M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l2)-(m*l)2; % 计算并显示多项式形式的传递函数num = m*l/q 0 0den = 1 b*(I+m*l2)/q -(M+m)*m*g*l/q -b*m*g*l/q 0rlocus(num,den);axis(-5 2 -100 100);gridkm,pole=rlocfind(num,den)wm=imag(pole(2);kp=0.6*kmkd=kp*pi/(4*wm)ki=kp*wm/pink=kd kp k

15、i,dk=1 0nd=conv(nk,num),dd=conv(dk,den)n1,d1=feedback(num,den,1,1)n2,d2=feedback(nd,dd,1,1)g1m,p1m,wpc1,wgc1=margin(num,den)g2m,p2m,wpc2,wgc2=margin(nd,dd)w=logspace(-1,2,200);figurebode(num,den,w)gridhold onbode(nd,dd,w)hold off%t=0:0.005:5;figurestep(n1,d1,2)axis(0 1 0 2);%impulse(n1,d1,t)gridhold

16、 onstep(n2,d2,2)%impulse(n2,d2,t)hold off% end 程序七：解析法设计PID% pid1.m % 摆杆角度PID控制% 输入倒立摆传递函数 G1(S)=num1/den1M = 0.5;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l2)-(m*l)2; num1 = m*l/q 0 0;den1 = 1 b*(I+m*l2)/q -(M+m)*m*g*l/q -b*m*g*l/q 0;% 输入控制器PID数学模型 Gc(s)=numPID/denPIDki=0;wgc=80;pm=5

17、0;ngv=polyval(num,j*wgc);dgv=polyval(den,j*wgc);g=ngv/dgv;thetar=(pm-180)*pi/180;ejtheta=cos(thetar)+j*sin(thetar);eqn=(ejtheta/g)+j*(ki/wgc);x=imag(eqn);r=real(eqn);kp=rkd=x/wgcnumPID = kd kp ki;denPID = 1 0;% 计算闭环系统传递函数G(s)=num/den% 多项式相乘num = conv(num1,denPID);% 多项式相加den = polyadd(conv(denPID,den1),conv(numPID,num1 );% 求整个系统传递函数的极点r,p,k = residue(num,den);% 显示极点s = p % 求取多项式传递函数的脉冲响应t=0:0.005:5;impulse(num,den,t)% 显示范围：横坐标0-5，纵坐标0-10，此条语句参数可根据仿真输出曲线调整axis(0 1 -0.1 0.1)grid% end