单级倒立摆LQR和LQY控制

单级倒立摆的LQR与LQY控制1、建模在忽略了空气阻力，各种摩擦之后，可将直线一级倒立摆系统抽象成小车和匀质杆组成的系统，如下图所示。其中：M小车质量m摆杆质量b小车摩擦系数l摆杆转动轴心到杆质心的长度I摆杆惯量F加在小车上的力x小车位置(P摆杆与垂直向上方向的夹角0摆杆与垂直向下方向的夹角(考虑到摆杆初始位置为竖直向下)采用牛顿动力学方法可建立单级倒立摆系统的微分方程如下：(M+m)x+bx+mlB.cos0一ml。,sin0=F(I+ml2)0.+mglsin0=一mlxcos0倒立摆的平衡是使倒立摆的摆杆垂直于水平方向倒立，所以假设0=兀+8，8为足够小的角度，即可近似处理得：cos0=-1，sin0=-8，竺=0。dt2用u来代表被控对象的输入力「，线性化后两个方程如下：(I+ml2)8-mgl8=mlx(M+m)x-bx+ml8=u

取状态变量:X,_o_10.X3X-X4_取状态变量:X,_o_10.X3X-X4__x_X=即摆杆的角度和角速度以及小车的位移和速度四个状态变量。则系统的状态方程为:mgl(M+m)XX+tlI(M+m)+MmhI(M+m)+Mmh-ml•2土=、4I+mhX=X+u•I(M+m)+MmhI(M+m)+Mmh•4将上式写成向量和矩阵的形式，就成为线性系统的状态方程:x=Ax+Bu

"e"=Cx这里设:M=1.32Kg

m=0.07Kg

b=0.1N/m/s

I=0.20mI=0.0009Kg秫2将参数带入，有:0100380.3847000A=0

-2.80370

0.74772、LQR控制线性二次型是指系统的状态方程是线性的，指标函数是状态变量和控制变量的二次型。考虑线性系统的状态方程为：X(t)=Ax(t)+Bu(t)

y(t)=Cx(t)+Du(t)找一状态反馈控制律：u(t)=-Kx(t)，使得二次型性能指标最小化：1J=—xt(t)S(t)+—jf[xt(t)Q(t)x(t)+utR(t)u(t)]dt"210其中，x(t)为系统的状态变量；f、10为起始时间与终止时间；S为终态约束矩阵；Q(t)为运动约束矩阵；R(t)为约束控制矩阵。其中Q(t)、R(t)决定了系统误差与控制能量消耗之间的相对重要性。为使J最小，由最小值原理得到最优控制为：u*(t)=—R-1BtPQ)x(t)式中，矩阵p(t)为微分Riccatti方程：p(t)=—p(t)A—ATp(t)+p(t)BR-1BTp(t)—Q的解。如果令终止时间tf=8，P(t)为一个常数矩阵，且P(t)=0，因此以上的Riccatti方程简化为—P(t)A—AtP(t)+P(t)BR-iBtP()—Q=0。对于最优反馈系数矩阵K=R-iBTP(t)，使用Matlab中专门的求解工具lqr()来求取K。将LQR控制方法用于倒立摆控制的原理如下图所示。用Matlab求解lqr(A,B,Q,R)可以求出最优反馈系数矩阵K的值。lqr函数需要选择两个参数R和Q，这两个参数是用来平衡输入量和状态量的权重。其中，Q”代表摆杆角度的权重，而Q33是小车位置的权重。这里选择:TOC\o"1-5"\h\z250000°°°°00100L000。」R=0.1通过matlab求得：K=[-82.4246-10.7034-10.0000-11.8512]。3、LQY控制取性能指标为1J=—xt(t)S(t)+—jf[xt(t)Q(t)x(t)+utR(t)u(t)]dt9fx、f‘o210对应控制率为u*(t)=-R-1BtP()x(t),P满足如下黎卡提方程PA+AtP—PBR-1BtP+CtQC=0同LQR求解过程求解K=[-276.86563.8430-45.2606-8.6243]4、仿真通过matlab仿真，LQR控制与LQY倒立摆摆角和小车位移仿真结果如下图所示。LQR仿真图：123456Sec(sec)steprespondofinvertedpendulum123456Sec(sec)steprespondofinvertedpendulumsystem3x=yz5pop0.3LLLLLO0.2O0.1-A-T/\0\rrrrr-0.100.511.522.53Sec(sec)LQY仿真图上下两条曲线分别为倒立摆摆角和小车位移。4、结果分析从仿真图我们可以看出对于这两种控制方法LQY的控制响应效果更好，图中明显可以看出LQY控制器的控制下的倒立摆瞬时响应稳定时间更短，而LQR控制方法下的倒立摆的响应稳定时间几乎是LQY控制器的2倍，但是LQR控制下的倒立摆超调变化量更小，相对LQY响应过程更加稳定。5、程序LQR方法%Modellinga=[038.18250-0.3847;1-000;0000;00010];b=[0;-2.8037;0;0.7477];c=[1000;0010];d=[0;0];eig(a)Qc=ctrb(a,b);Rc=rank(Qc)if(Rc==4)disp('状态完全能控')elsedisp（'状态不完全能控'）endQo=obsv(a,c);Ro=rank(Qo)if(Ro==4)disp('状态完全能观测')elsedisp('状态不完全能观测')endQ=[250000;0000;00100;0000];R=1;K=lqr(a,b,Q,R);x0=[0.2,0,0,0];ac=[(a-b*K)];bc=;cc=[c];dc=[d];[K,P,e]=lqr(a,b,Q,R),t=0:0.005:7;figureinitial(ac,b,c,d,x0)title('steprespondofinvertedpendulumsystem')xlabel('Sec')ylabel('Outputy=x3')figure[y,x,t]=initial(ac,bc,cc,dc,x0,t);plot(t,x,'y')figure,x1=[1000]*x',plot(t,x1);grid;title('x1的响应曲线')figure,x2=[0100]*x',plot(t,x2);grid;title('x2的响应曲线')figure,x3=[0010]*x',plot(t,x3);grid;title('x3的响应曲线')figure,x4=[0001]*x',plot(t,x4);grid;title('x4的响应曲线')LQY方法%Modellinga=[038.18250-0.3847;1-000;0000;00010];b=[0;-2.8037;0;0.7477];c=[1000;0010];d=[0;0];sys=ss(a,b,c,d);eig(a)Qc=ctrb(a,b);Rc=rank(Qc)if(Rc==4)disp('状态完全能控')elsedisp('状态不完全能控')endQo=obsv(a,c);Ro=rank(Qo)if(Ro==4)disp('状态完全能观测')elsedisp('状态不完全能观测')endQ=[50000;0100]R=1;[K,P,e]=lqry(sys,Q,R)x0=[0.2,0,0,0];ac=[(a-b*K)];bc=;cc=[c];dc=[d];t=0:0.005:3;figureinitial(ac,b,c,d,x0,t)title('steprespondofinvertedpendulumsystem')xlabel('Sec')ylabel('Outputy=x3')figure[y,x,t]=initial(ac,bc,cc,dc,x0,t);plot(t,x,'y')figure,x1=[1000]*x',plot(t,x1);grid;ti