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

./倒立摆小车的建模系统参数：<M>小车质量0.5kg<m>倒立摆质量0.2kg<b>小车的阻尼系数0.1N/m/sec<l>倒立摆长度0.3m<I>倒立摆转动惯量0.006kg.m^2<F>施加在小车上的力<x>小车的位置坐标<theta>倒立摆与垂直方向的夹角〔从垂直向下开始计动力学建模：考虑水平方向的力矩平衡：以上两式合并得到：考虑垂直施加在倒立摆上的力及质心力矩平衡方程：消去PN,得到如下公式简化：简化后模型：传递函数模型：设：PID控制：或者如下表示：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;s=tf<'s'>;P_pend=<m*l*s/q>/<s^3+<b*<I+m*l^2>>*s^2/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<'ResponseofPendulumPositiontoanImpulseDisturbanceunderPIDControl: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<'ResponseofPendulumPositiontoanImpulseDisturbanceunderPIDControl:Kp=100,Ki=1,Kd=1'>;Kp=100;Ki=1;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<'ResponseofPendulumPositiontoanImpulseDisturbanceunderPIDControl:Kp=100,Ki=1,Kd=20'>;这时,小车的位置?传递函数：P_cart=<<<I+m*l^2>/q>*s^2-<m*g*l/q>>/<s^4+<b*<I+m*l^2>>*s^3/q-<<M+m>*m*g*l>*s^2/q-b*m*g*l*s/q>;T2=feedback<1,P_pend*C>*P_cart;t=0:0.01:5;impulse<T2,t>;title<'ResponseofCartPositiontoanImpulseDisturbanceunderPIDControl:Kp=100,Ki=1,Kd=20'>;基于状态空间方程的倒立摆小车的控制方法设计开环根<LQR>增加补偿基于观测器的控制1状态空间方程设计目标：倒立摆倒立状态小车向右移动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*l^2;%denominatorfortheAandBmatricesA=[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];states={'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.14285.5651线性二次型整定<LQR>检查可控性co=ctrb<sys_ss>;controllability=rank<co>controllability=4设R=1Q=C'*CQ=1000000000100000Q=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_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>,'Ylabel'>,'String','cartposition<m>'>set<get<AX<2>,'Ylabel'>,'String','pendulumangle<radians>'>title<'StepResponsewithLQRControl'>K=-1.0000-1.656718.68543.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''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>,'Ylabel'>,'String','cartposition<m>'>set<get<AX<2>,'Ylabel'>,'String','pendulumangle<radians>'>title<'StepResponsewithLQRControl'>Q=500000000000010000000K=-70.7107-37.8345105.529820.9238增加补偿系数：Cn=[1000];sys_ss=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'>,'String','cartposition<m>'>set<get<AX<2>,'Ylabel'>,'String','pendulumangle<radians>'>title<'StepResponsewithPrecompensationandLQRControl'>基于能观器的控制检查能观性：ob=obsv<sys_ss>;observability=rank<ob>observability=4能观器的根要比控制器的根快5到10倍的收敛控制器的根：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.00101.6992-0.0402-0.00140.0832-0.07621.7604Ace=[<A-B*K><B*K>;zeros<size<A>><A-L*C>];Bce=[B*Nbar;zeros<size<B>>];Cce=[Cczeros<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,'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','cartposition<m>'>set<get<AX<2>,'Ylabel'>,'String','pendulumangle<radians>'>title<'StepResponsewithObserver-BasedState-FeedbackControl'>Function[Nbar]=rscale<a,b,c,d,k>%Giventhesingle-inputlinearsystem:%.%x=Ax+Bu%y=Cx+Du%andthefeedbackmatrixK,%%thefunctionrscale<sys,K>orrscale<A,B,C,D,K>%findsthescalefactorNwhichwill%eliminatethesteady-stateerrortoastepreference%foracontinuous-time,single-inputsystem%withfull-statefeedbackusingtheschematicbelow:%%/\%R+u|.|%>N><>>|X=Ax+Bu|-->y=Cx>y%-|\/%||%|<K<|%%8/21/96YanjieSunoftheUniversityof