二级倒立摆课程设计.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——————
升级会员 