二级倒立摆课程设计.docx
- 文档编号:2339558
- 上传时间:2023-05-03
- 格式:DOCX
- 页数:20
- 大小:190.38KB
二级倒立摆课程设计.docx
《二级倒立摆课程设计.docx》由会员分享,可在线阅读,更多相关《二级倒立摆课程设计.docx(20页珍藏版)》请在冰点文库上搜索。
二级倒立摆课程设计
程序一:
二级最优控制
>>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)*l1^22*m2*l1*l2;
-m2*l22*m2*l1*l24/3*m2*l2^2;
M+m1+m2+m3-(m1+2*m2+2*m3)*l1-m2*l2]
a=-0.05900.00870.0054
-0.03630.00540.0133
1.7380-0.0590-0.0363
>>inv(a)
ans=5.1340-0.05950.7483
190.8063-64.05545.1340
-64.0554101.1736-0.0595
>>g=9.8;
>>b=[(m1+2*m2+2*m3)*g*l1m2*g*l21]
b=0.57770.35571.0000
>>c=diag(b)
c=0.577700
00.35570
001.0000
>>d=inv(a)*c
d=2.9659-0.02120.7483
110.2307-22.78715.1340
-37.005535.9915-0.0595
A=[000100;
000010;
000001;
2.9659-0.02120.7483000;
110.2307-22.78715.1340000;
-37.005535.9915-0.0595000];
B=[0000.57770.35571.0000]';
C=[100001;
010000;
001000;
000100;
000010;
000001];
D=[000000]';
p=eig(A)
%求向量K
%Q=diag([10050500010510]);R=1;
%Q=diag([103090111]);R=0.1;
Q=diag([259.11343455.25514451.916670.014780.113551.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('CartPosition','PendulumAngle')
grid
%——————end——————
程序二:
摆杆角度控制
%——————pid1.m——————
%摆杆角度PID控制
%输入倒立摆传递函数G1(S)=num1/den1
M=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;
num1=[m*l/q00];
den1=[1b*(I+m*l^2)/q-(M+m)*m*g*l/q-b*m*g*l/q0];
%输入控制器PID数学模型Gc(s)=numPID/denPID
Kp=100;
Ki=1;
Kd=20;
numPID=[KdKpKi];
denPID=[10];
%计算闭环系统传递函数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([05-0.20.2])
grid
%——————end——————
%——————polyadd.m——————
%求两个多项式之和
function[poly]=polyadd(poly1,poly2)
iflength(poly1) short=poly1; long=poly2; else short=poly2; long=poly1; end mz=length(long)-length(short); ifmz>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*l^2; A=[0100; 0-(I+m*l^2)*b/p(m^2*g*l^2)/p0; 0001; 0-(m*l*b)/pm*g*l*(M+m)/p0]; B=[0; (I+m*l^2)/p; 0; m*l/p]; C=[1000; 0010]; D=[0; 0]; p=eig(A) %求向量K x=5000; y=100; Q=[x000; 0000; 00y0 0000]; 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('CartPosition','PendulumAngle') 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*l^2; A=[0100; 0-(I+m*l^2)*b/p(m^2*g*l^2)/p0; 0001; 0-(m*l*b)/pm*g*l*(M+m)/p0]; B=[0; (I+m*l^2)/p; 0; m*l/p]; C=[1000; 0010]; D=[0; 0]; p=eig(A); %求向量K x=5000;y=100; Q=[x000; 0000; 00y0 0000]; R=1; K=lqr(A,B,Q,R) %计算LQR控制矩阵 Ac=[(A-B*K)]; Bc=[B]; Cc=[C]; Dc=[D]; %计算增益Nbar Cn=[1000]; 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),'-') legend('CartPosition','PendulumAngle') grid %——————end—————— %————rscale.m———— %求取输入输出匹配系数 function[Nbar]=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—————— 程序五: 小车位置PIDCONTROL %——————pid2.m—————— %小车位置PID控制 %输入倒立摆传递函数G1(s)=num1/den1,G2(s)=num2/den2 M=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; num1=[m*l/q00]; den1=[1b*(I+m*l^2)/q-(M+m)*m*g*l/q-b*m*g*l/q0]; num2=[-(I+m*l^2)/q0m*g*l/q]; den2=den1; %输入控制器PID数学模型Gc(s)=numPID/denPID Kp=100; Ki=1.1760; Kd=20; numPID=[KdKpKi]; denPID=[10]; %计算闭环系统传递函数G(s)=num/den %多项式相乘 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([05-0.20.5]) grid %——————end—————— 程序六: Z-N法设计PID %——————trans.m—————— %倒立摆传递函数、开环极点及开环脉冲响应z-n %输入倒立摆传递函数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*l^2)-(m*l)^2; %计算并显示多项式形式的传递函数 num=[m*l/q00] den=[1b*(I+m*l^2)/q-(M+m)*m*g*l/q-b*m*g*l/q0] rlocus(num,den); axis([-52-100100]); grid [km,pole]=rlocfind(num,den) wm=imag(pole (2)); kp=0.6*km kd=kp*pi/(4*wm) ki=kp*wm/pi nk=[kdkpki],dk=[10] nd=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); figure bode(num,den,w) grid holdon bode(nd,dd,w) holdoff %t=0: 0.005: 5; figure step(n1,d1,2) axis([0102]); %impulse(n1,d1,t) grid holdon step(n2,d2,2) %impulse(n2,d2,t) holdoff %——————end—————— 程序七: 解析法设计PID %——————pid1.m—————— %摆杆角度PID控制 %输入倒立摆传递函数G1(S)=num1/den1 M=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; num1=[m*l/q00]; den1=[1b*(I+m*l^2)/q-(M+m)*m*g*l/q-b*m*g*l/q0]; %输入控制器PID数学模型Gc(s)=numPID/denPID ki=0; wgc=80;pm=50; 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=r kd=x/wgc numPID=[kdkpki]; denPID=[10]; %计算闭环系统传递函数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([01-0.10.1]) grid %——————end——————
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 二级 倒立 课程设计