小车倒立摆

小车倒立摆系统的控制问题一直是控制研究中的一个典型问题，下面先简单介绍一下这个系统。小车倒立摆系统由一个杆、一个导轨和一辆滑车组成，滑车可以沿导轨水平运动。在一定的初始条件下，通过在滑车质心处施加一个力μ(控制力)，使杆尽可能的平衡,如下图。

本次实验采用多种控制方法，并进行一下比较。 1.单级倒立摆的经典PID控制 建立系统的动力学方程:

假设小车质量为M,摆的质量是m，小车位置为x，摆的角度为θ,如上图。现假设摆杆偏离垂直线的角度为θ，同时规定摆杆重心的坐标为G(Xc,Yc），则有： Xc=x+lsinθ Yc=lcosθ

根据牛顿定律，可以建立摆杆水平和垂直运动状态方程。 摆杆围绕其重心的转动运动可用力矩方程来描述： IVlsinHlcos 式中，I为摆杆围绕其重心的转动惯量。 摆杆重心的水平运动由下式描述：

d2m2(xlsin)H dt

..摆杆重心的垂直运动由下式描述：

d2m2lcosVmg dt

小车的水平运动由下式描述：

d2xm2uH dt

假设θ很小，sinθ≈θ，cosθ≈1。则以上各式变为： IVlHl 1.1 m(xl)H 1.2 0Vmg 1.3 MxuH 1.4 由式1.2和1.4得：

(Mm)xmlu 1.5 由式1.2和1.3得：

(Iml)mlxmgl 1.6 由1.5和1.6可得单级倒立摆方程：

2................

....m(mM)glmlu22(mM)IMml(mM)IMml

m2gl2Iml2xu22(mM)IMml(mM)IMml

式中，

I1mL212， l=0.5L .

.. 控制指标共有四个，即单级倒立摆的摆角θ、摆速，小车位置x和小车速度x。将倒立摆运动方程的形式转化为状态方程的形式。令x(1)=θ,x(2)=，x(1)=x，x(1)=x，则有：

.. x=Ax+Bu

y=Cx+Du， 式中，

0tA10t21000000000t0B3010t4 ， ，

.

1000C0010 , D[0,0]

m2gl2m(mM)glt1t222(mM)IMml(mM)IMml ,

Iml2mlt3t422(mM)IMml(mM)IMml

为了得到更好的控制效果，采用最优控制中的LQR方法。通过状态方程

x=Ax+Bu 确定最佳控制量u(t)= -Kx(t)的矩阵K，使控制性能指标

.J(xTQxuTRu)dt0 达到极小。

其中，K=LQR(A,B,C,D)。 一级倒立摆的PID控制程序为： %声明全局变量 clear all; close all; global A B C D

%声明系统参数 g=9.8; M=1.0; m=0.1; L=1.0;

Fc=0.0005;

I=1/12*m*L^2; l=1/2*L;

t1=m*(M+m)*g*l/[(M+m)*I+M*m*l^2]; t2=-m^2*g*l^2/[(m+M)*I+M*m*l^2]; t3=-m*l/[(M+m)*I+M*m*l^2]; t4=(I+m*l^2)/[(m+M)*I+M*m*l^2];

A=[0,1,0,0; t1,0,0,0; 0,0,0,1; t2,0,0,0]; B=[0;t3;0;t4]; C=[1,0,0,0; 0,0,1,0]; D=[0;0];

Q=[50,0,0,0; 0,5,0,0; 0,0,50,0; 0,0,0,5]; R=[0.1];

K=LQR(A,B,Q,R);

e1_1=0;e2_1=0;e3_1=0;e4_1=0; u_1=0;

表明参量的重要性%采取LQR方法 %50,5,50,5 xk=[0.2,0,-0.5,0]; %初始状态, 小车摆角约为10度 ts=0.02; %采样周期 for k=1:1:500 time(k)=k*ts; Tspan=[0 ts];

para=u_1;

[t,x]=ode45('subsys',Tspan,xk,[],para); xk=x(length(x),:);

r1(k)=0.0; r2(k)=0.0; r3(k)=0.0;

r4(k)=0.0;

x1(k)=xk(1); x2(k)=xk(2); x3(k)=xk(3); x4(k)=xk(4);

e1(k)=r1(k)-x1(k); e2(k)=r2(k)-x2(k); e3(k)=r3(k)-x3(k);

e4(k)=r4(k)-x4(k); %调用子程序 %定义一个初始零矩阵%解算出误差

S=1;

if S==1 %LQR u(k)=K(1)*e1(k)+K(2)*e2(k)+K(3)*e3(k)+K(4)*e4(k); elseif S==2 %PD de1(k)=e1(k)-e1_1;

u1(k)=-50*e1(k)-10*de1(k); de2(k)=e2(k)-e2_1;

u2(k)=-10*e2(k)-10*de2(k); de3(k)=e3(k)-e3_1;

u3(k)=-10*e3(k)-10*de3(k); de4(k)=e4(k)-e4_1;

u4(k)=-10*e4(k)-10*de4(k); u(k)=u1(k)+u2(k)+u3(k)+u4(k); end

if u(k)>=10 u(k)=10; elseif u(k)<=-10 u(k)=-10; end

e1_1=e1(k); e2_1=e2(k); e3_1=e3(k); e4_1=e4(k); u_1=u(k); end

figure(1);

plot(time,r1,'k',time,x1,'k'); grid on;

xlabel('time(s)');ylabel('Angle'); figure(2);

plot(time,r2,'k',time,x2,'k'); grid on;

xlabel('time(s)');ylabel('Angle rate'); figure(3);

plot(time,r3,'k',time,x3,'k'); grid on;

xlabel('time(s)');ylabel('Cart position');figure(4);

plot(time,r4,'k',time,x4,'k'); grid on;

xlabel('time(s)');ylabel('Cart rate'); figure(5);

plot(time,u,'k'); grid on;

xlabel('time(s)');ylabel('Force');

调用的子程序subsys

function dx=dym(t,x,flag,para) global A B C D u=para;

dx=zeros(4,1);

dx=A*x+B*u; %显示结果%结束

结果分析:

初始角度为0.2rad，初始位置为-0.5m，此时施加的力最大，大约是14N, 小车从左侧向原点运动以保持平衡。摆角在3秒左右基本保持在零，而此时小车位移仍有小幅变化，6.5秒左右整个系统就已经达到所要求的平衡。可以看出用这个方法控制倒立摆系统，收敛性稳定性较好，小车也可以在比较短的时间内达到平衡。 2.

利用MATLAB自带的小车单级倒立摆模型进行仿真，调用slcp ,将小车模型初始参数改为（0.2，0，-0.5，0），模糊规则采用原程序自带的规则库。结果如下图

摆角随时间的变化

角加速度随时间的变化

小车位置随时间的变化

加速度随时间的变化

力随时间的变化

通过比较可以看出，两种控制均可以达到较好的控制效果，模糊控制的鲁棒性更好，而且更容易控制。

3.T-S模糊控制器

TaKagi和Sugeno提出的TS模糊模型，是非线形复杂系统模糊建模中一个典型的模糊动态模型。这类模型采用的模糊规则：其前件是依据系统输入、输出间是否存在局部线形关系来进行划分；而其后件是多项式线形方程来表达，从而构成各条规则间的线形组合，使非线形系统的全局输出具有良好的线性描述特性。TS模糊规则的一般形式如下：

R：ifx1isA1x2 isA2,…,xnisAn,then u= b0+b1x1+b2x2+…+bnxn

程序如下 close all; g=9.8; m=0.1; M=1; l=0.5;

a=1/(m+M);

a11=g/(4/3*l-a*m*l)

a12=-g*m*l/(M+m)*(4/3*l-a*m*l) A1=[0 1 0 0; a11 0 0 0; 0 0 0 1; a12 0 0 0]

b12=-a/(4/3*l-a*m*l)

b14=4/3*l/(M+m)*(4/3*l-a*m*l) B1=[0;b12;0;b14]

x2=200*pi/180;

a21=(g-a*m*l*x2^2)/(4/3*l-a*m*l)

a22=m*l*(4/3*l*x2*x2-g)/(M+m)*(4/3*l-a*m*l) A2=[0 1 0 0; a21 0 0 0; 0 0 0 1; a22 0 0 0]

b22=b12; b24=b14;

B2=[0;b22;0;b24]

x1=15*pi/180;

a31=g/(4/3*l-a*m*l*(cos(x1))^2)

a32=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2) A3=[0 1 0 0; a31 0 0 0; 0 0 0 1; a32 0 0 0]

b32=-a*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2)

b34=4/3*l/(M+m)*(4/3*l-a*m*l*(cos(x1))^2) B3=[0;b32;0;b34]

x1=15*pi/180; x2=200*pi/180;

a41=g/(4/3*l-a*m*l*(cos(x1))^2)

a42=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2) a43=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)

a44=m*l*(x2*sin(2*x1)*0.5+a*m*l*x2*sin(2*x1)*0.5*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2))/(M+m) A4=[0 1 0 0; a41 a42 0 0; 0 0 0 1;

a43 a44 0 0] b42=b32; b44=b34;

B4=[0;b42;0;b44]

x1=-15*pi/180; x2=200*pi/180;

a51=g/(4/3*l-a*m*l*(cos(x1))^2)

a52=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2) a53=-m*l*g*(cos(x1))/(M+m)*(4/3*l-a*m*l*(cos(x1))^2)

a=m*l*(x2*sin(2*x1)*0.5+a*m*l*x2*sin(2*x1)*0.5*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2))/(M+m) A5=[0 1 0 0; a51 a52 0 0; 0 0 0 1;

a53 a 0 0] b52=b32; b=b34;

B5=[0;b52;0;b]

P=[-5-5i;-5+5i;-1.5-1.5i;-1.5+1.5i]; %稳定极点

F1=place(A1,B1,P) F2=place(A2,B2,P) F3=place(A3,B3,P) F4=place(A4,B4,P)

F5=place(A5,B5,P)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% tc=newfis('tc','sugeno');

tc=addvar(tc,'input','jiaodu',[-15,15]*pi/180);

tc=addmf(tc,'input',1,'NG','gaussmf',[5,-15]*pi/180); tc=addmf(tc,'input',1,'ZR','gaussmf',[5,0]*pi/180); tc=addmf(tc,'input',1,'PO','gaussmf',[5,15]*pi/180);

tc=addvar(tc,'input','jiasudu',[-200,200]*pi/180);

tc=addmf(tc,'input',2,'NG','gaussmf',[50,-200]*pi/180); tc=addmf(tc,'input',2,'ZR','gaussmf',[50,0]*pi/180); tc=addmf(tc,'input',2,'PO','gaussmf',[50,200]*pi/180);

tc=addvar(tc,'input','weiyi',[-10,10]);

tc=addmf(tc,'input',3,'NG','gaussmf',[4,-10]); tc=addmf(tc,'input',3,'ZR','gaussmf',[4,0]); tc=addmf(tc,'input',3,'PO','gaussmf',[4,10]);

tc=addvar(tc,'input','sudu',[-10,10]);

tc=addmf(tc,'input',4,'NG','gaussmf',[2,-10]); tc=addmf(tc,'input',4,'ZR','gaussmf',[2,0]); tc=addmf(tc,'input',4,'PO','gaussmf',[2,10]);

tc=addvar(tc,'output','u',[-100,100]);

tc=addmf(tc,'output',1,'No.1','linear',[F1(1),F1(2),F1(3),F1(4) 0]); tc=addmf(tc,'output',1,'No.2','linear',[F2(1),F2(2),F2(3),F2(4) 0]); tc=addmf(tc,'output',1,'No.3','linear',[F3(1),F3(2),F3(3),F3(4) 0]); tc=addmf(tc,'output',1,'No.4','linear',[F4(1),F4(2),F4(3),F4(4) 0]); tc=addmf(tc,'output',1,'No.5','linear',[F5(1),F5(2),F5(3),F5(4) 0]);

rulelist=[1 1 1 1 4 1 1; 1 1 1 2 4 1 1; 1 1 1 3 4 1 1; 1 1 2 1 4 1 1; 1 1 2 2 4 1 1; 1 2 3 3 3 1 1; 1 3 2 3 5 1 1; 2 1 2 2 2 1 1; 2 2 1 1 1 1 1; 3 1 2 1 5 1 1;

3 2 3 3 3 1 1; 3 3 1 2 4 1 1]; tc=addrule(tc,rulelist);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% model=newfis('model','sugeno');

model=addvar(model,'input','jiaodu',[-15,15]*pi/180);

model=addmf(model,'input',1,'NG','gaussmf',[5,-15]*pi/180); model=addmf(model,'input',1,'ZR','gaussmf',[5,0]*pi/180); model=addmf(model,'input',1,'PO','gaussmf',[5,15]*pi/180);

model=addvar(model,'input','jiaosudu',[-200,200]*pi/180);

model=addmf(model,'input',2,'NG','gaussmf',[50,-200]*pi/180); model=addmf(model,'input',2,'ZR','gaussmf',[50,0]*pi/180); model=addmf(model,'input',2,'PO','gaussmf',[50,200]*pi/180);

model=addvar(model,'input','weiyi',[-10,10]);

model=addmf(model,'input',3,'NG','gaussmf',[5,-10]); model=addmf(model,'input',3,'ZR','gaussmf',[5,0]); model=addmf(model,'input',3,'PO','gaussmf',[5,10]);

model=addvar(model,'input','sudu',[-100,100]);

model=addmf(model,'input',4,'NG','gaussmf',[25,-100]); model=addmf(model,'input',4,'ZR','gaussmf',[25,0]); model=addmf(model,'input',4,'PO','gaussmf',[25,100]);

model=addvar(model,'input','u',[-5,5]);

model=addmf(model,'input',5,'Any','gaussmf',[1.5,-5]);

model=addvar(model,'output','d_jiaodu',[-200,200]*pi/180); model=addmf(model,'output',1,'No.1','linear',[0 1 0 0 0 0]); model=addmf(model,'output',1,'No.2','linear',[0 1 0 0 0 0]); model=addmf(model,'output',1,'No.3','linear',[0 1 0 0 0 0]); model=addmf(model,'output',1,'No.4','linear',[0 1 0 0 0 0]); model=addmf(model,'output',1,'No.5','linear',[0 1 0 0 0 0]);

model=addvar(model,'output','d_jiaosudu',[-200,200]*pi/180);

model=addmf(model,'output',2,'No.1','linear',[A1(2,1),0,0,0,B1(2),0]);

model=addmf(model,'output',2,'No.2','linear',[A2(2,1),0,0,0,B2(2),0]

);

model=addmf(model,'output',2,'No.3','linear',[A3(2,1),0,0,0,B3(2),0]);

model=addmf(model,'output',2,'No.4','linear',[A4(2,1),A4(2,2),0,0,B4(2),0]);

model=addmf(model,'output',2,'No.5','linear',[A5(2,1),A5(2,2),0,0,B5(2),0]);

model=addvar(model,'output','d_weiyi ',[-10,10]);

model=addmf(model,'output',3,'No.1','linear',[0 0 0 1 0 0]); model=addmf(model,'output',3,'No.2','linear',[0 0 0 1 0 0]); model=addmf(model,'output',3,'No.3','linear',[0 0 0 1 0 0]); model=addmf(model,'output',3,'No.4','linear',[0 0 0 1 0 0]); model=addmf(model,'output',3,'No.5','linear',[0 0 0 1 0 0]);

model=addvar(model,'output','d_sudu',[-100,100]);

model=addmf(model,'output',4,'No.1','linear',[0,A1(4,2),0,0,B1(4),0]);

model=addmf(model,'output',4,'No.2','linear',[A2(4,1),A2(4,1),0,0,B2(4),0]);

model=addmf(model,'output',4,'No.3','linear',[0,A3(4,2),0,0,B3(4),0]);

model=addmf(model,'output',4,'No.4','linear',[A4(4,1),A4(4,2),0,0,B4(4),0]);

model=addmf(model,'output',4,'No.5','linear',[A5(4,1),A5(4,2),0,0,B5(4),0]);

rulelist1=[1 1 1 1 0 4 4 4 4 1 1; 1 2 1 2 0 3 3 3 3 1 1; 131****5511; 2 1 2 1 0 2 2 2 2 1 1; 2 2 2 2 0 1 1 1 1 1 1; 2 3 2 3 0 2 2 2 2 1 1; 3 1 3 1 0 5 5 5 5 1 1; 3 2 3 2 0 3 3 3 3 1 1; 3 3 3 3 0 4 4 4 4 1 1]; model=addrule(model,rulelist1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ts=0.020; %步长

x=[0.20;0;-0.5;0]; %初始状态 for k=1:1:500 %循环 time(k)=k*ts;

u(k) =(-1)*evalfis([x(1),x(2),x(3),x(4)],tc); k0=evalfis([x(1),x(2),x(3),x(4),u(k)],model)'; x=x+ts*k0; y1(k)=x(1); y2(k)=x(2); y3(k)=x(3); y4(k)=x(4); end

figure(1);

plot(time,y1),grid on;

xlabel('time(s)'),ylabel('Angle'); figure(2);

plot(time,y2),grid on;

xlabel('time(s)'),ylabel('Angle rate'); figure(3);

plot(time,y3),grid on;

xlabel('time(s)'),ylabel('position'); figure(4);

plot(time,y4),grid on;

xlabel('time(s)'),ylabel('rate');

figure(5);

plot(time,u),grid on;

xlabel('time(s)'),ylabel('control force');

figure(6);

plotmf(tc,'input',1); ylabel('角度隶属度函数'); figure(7);

plotmf(tc,'input',2); ylabel('角速度隶属度函数'); figure(8);

plotmf(tc,'input',3); ylabel('位移隶属度函数'); figure(9);

plotmf(tc,'input',4); ylabel('速度隶属度函数);

showrule(tc);

showrule(model);

各个参数的隶属度函数图形：

从以上matlab仿真结果来看，对单级小车倒立摆进行TS模糊的控制是可行的，且有良好的动态特性及稳态特性。具有PID控制器具有结构简单、稳定性好、可靠性高以及易于工程实现等优点,同时具有模糊控制对于倒立摆这种时变、非线性的的复杂系统有较好的鲁棒性的优点。PID的控制的关键部分是参数的调节，一般由Ziegler-Nichols经验公式获得的参数需要进一步调节才能获得较好的控制效果，这一过程相对较难。TS 模糊控制器是基于PID 控制的,它利用PID 控制的方法进行参数设计，但由于它本身将PID 参数的一些调节经验转化为TS 规则,不需要参数的进一步调节。