《二级倒立摆课程设计.docx》由会员分享，可在线阅读，更多相关《二级倒立摆课程设计.docx（27页珍藏版）》请在三一办公上搜索。
1、程序一：二级最优控制 M=1.32;m1=0.05;m2=0.132;m3=0.236;11=0.075;12=0.275;a=-(m1+2*m2+2*m3)*11 (4/3*m1+4*m2+4*m3)*11人22*m2*11*12;-m2*l2 2*m2*11*12 4/3*m2*12人2;M+m1+m2+m3 -(m1+2*m2+2*m3)*11 -m2*12a =-0.05900.00870.0054-0.03630.00540.01331.7380-0.0590-0.0363 inv(a)ans =5.1340-0.05950.7483190.8063 -64.05545.1340-6
2、4.0554 101.1736-0.0595 g=9.8; b=(m1+2*m2+2*m3)*g*11m2*g*121b =0.57770.35571.0000 c=diag(b)c =0.57770000.35570001.0000 d=inv(a)*cd =2.9659-0.02120.7483110.2307 -22.78715.1340-37.005535.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.7483 0 0 0;110.2307 -22.7871 5.1340 0 0 0; -37.005
3、5 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 451.91667 0.01478 0.11355 1.58004);R =1;K = lq
4、r(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),y)legend(Cart Position,Pendulum Angle) grid%end程序二：摆杆角度控制%pidl.m%摆杆角度PID控制% 输入倒立摆传递函数 G1(S)=num1/den1 M = 0.5;m = 0.2;b = 0
5、.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l人2)-(m*l)人2;numl = m*l/q 0 0;denl = 1b*(I+m*lA2)/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 = conv(num1,denPID);%多项式相加den=polyadd(conv(denPID,den1),co
6、nv(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 length(poly1) 0poly = zeros(1,mz),short + long;elsepoly = long + sh
7、ort;endend61 -1 F-tdiE-Itle er t Tools Window HelpaQ芽DImpulse ResponseTime (sec)程序三：一级最优控制%Iqrl.m%最优控制%确定开环极点的程序如下M = 0.5;m = 0.2;b = 0.1;g = 9.8;l = 0.3;p = I*(M+m)+M*m*l人2;A = 0100;0 -(I+m*l人2)*b/p(mA2*g*lA2)/p0;0001;0 -(m*l*b)/pm*g*l*(M+m)/p0;B = 0；(I+m*l人2)/p;0;m*l/p ;C = 1 0 0 0;0 0 1 0;D = 0;0
8、;p = eig(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;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),y)legend(Cart Position,Pendulum Angle)grid%e
9、nd程序四：带增益的最优控制%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*l;A = 0100;0 -(I+m*l人2)*b/p (mA2*g*lA2)/p 0;0001;0 -(m*l*b)/pm*g*l*(M+m)/p 0;B = 0；(I+m*l人2)/p;0;m*l/p ;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;00y00
10、 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,Y(:,2),y)legend(Cart Position,Pendulum Angle)grid%end%rsca
11、le.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;m = 0.2;b = 0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l人2) -(m*l)人2;n
12、uml = m*l/q 0 0;denl = 1b*(I+m*lA2)/q-(M+m)*m*g*l/q-b*m*g*l/q 0;num2 = -(I+m*lA2)/q 0 m*g*l/q;den2 = denl;%输入控制器PID数学模型Gc(s)=numPID/denPIDKp =100;Ki =1.1760;Kd = 20;numPID = Kd Kp Ki;denPID = 1 0;%计算闭环系统传递函数G(s)=num/den%多项式相乘num = conv(num2,denPID);%多项式相加den=polyadd(conv(denPID,den2),conv(numPID,num
13、1 );%求闭环系统极点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%endFile Edi t Vi ew Ins er t Tools Window Help QS * A / /如务程序六：Z-N法设计PID%trans.m%倒立摆传递函数、开环极点及开环脉冲响应z-n%输入倒立摆传递函数G(S)=num/denM = 0.5;m = 0.2;b =
14、0.1;I = 0.006;g = 9.8;l = 0.3;q = (M+m)*(I+m*l人2)-(m*l)人2;%计算并显示多顼式形式的传递函数 num = m*l/q 0 0den = 1b*(I+m*lA2)/q-(M+m)*m*g*l/q-b*m*g*l/q 0 rlocus(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 ki,dk=1 0nd=conv(nk,num),dd=conv(dk,d
15、en)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);figure bode(num,den,w) gridhold on bode(nd,dd,w) hold off %t=0:0.005:5; figure step(n1,d1,2) axis(0 1 0 2); %impulse(n1,d1,t) gridhold onstep(n2,d2,2) %impulse
16、(n2,d2,t)hold offendFile Edit Vi ew Insert Tools Window Helpjo含耐鲁|A/j用QO：0-x A / / 国 Q OBode DiagramFrequency (rad/sec)3S回区iews V-Insert Tools Window Help A / / 国 Q OStep ResponseTime (sec)IDpmQ.身File Edi t Vi ew Itleert Tools Wiridow Help00.511.522.533.544.55Time (sec)Impulse ResponseILIPIm-d 与程序七：
17、解析法设计PID%pidl.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*l人2)-(m*l)人2;numl = m*l/q 0 0;denl = 1b*(I+m*lA2)/q-(M+m)*m*g*l/q-b*m*g*l/q 0;%输入控制器PID数学模型Gc(s)=numPID/denPID ki=0;wgc=80;pm=50;ngv=polyval(num,j*wgc);dgv=polyval(den,j*wgc);g=ngv/dg
18、v;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/wgc numPID = 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% endInsert Tools nindow Help A / / 国 Q OImpulse Response0.10.20.30.40.50.60.70.80.91Time (sec)
