基于LQR的二级倒立摆建模与MATLAB仿真

1、系统建模

二级倒立摆系统的机械部分主要是由小车、摆杆1、摆杆2、导轨、皮带轮、传送皮带等组成，电气部分由电机、功率放大器、PWM、传感器、驱动电路以及保护电路组成。系统结构如下图所示。

中间的推导过程不再赘述
取$x_1=x$,$x_2=\dot{x}$,$x_3={\theta }_1$,$x_4={\dot{\theta }}_1$,$x_5={\theta }_2$,$x_6={\dot{\theta }}_2$,系统状态方程为：

即得到形式如下的状态方程：
$$\dot{x}=AX+BU$$
$$Y=CX+DU$$

2、MATLAB程序

A=[0 1 0 0 0 0;
0 -16.6601 -1.2973 0 0.0857 0;
0 0 0 1 0 0;
0 39.0555 18.0514 0 -7.8603 0;
0 0 0 0 0 1;
0 -68.5120 -14.4458 0 25.9635 0];
B=[0;0.7270;0;-1.7044;0;0.2069];
C=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1];
D=[0;0;0;0;0;0];  %这里的ABCD矩阵都可以根据具体的数值进行修改
p=eig(A)  %求取A矩阵的特征值
[num,den]=ss2tf(A,B,C,D,1);  %将状态空间模型转变成传递函数形式
printsys(num,den)  %输出以num为分子多项式，以den为分母多项式的系统传函
Q=[1000 0 0 0 0 0;
0 0 0 0 0 0;
0 0 10 0 0 0;
0 0 0 0 0 0;
0 0 0 0 10 0;
0 0 0 0 0 0];  %根据具体要求进行修改
Tc=ctrb(A,B);  %可控性
rank(Tc)  %Tc矩阵的秩
To=obsv(A,C);  %可观性
rank(To)  %To矩阵的秩
R=1;
K=lqr(A,B,Q,R)  %求取K矩阵
Ac=[(A-B*K)];  
Bc=[B];
Cc=[C];
Dc=[D];
T=0:0.005:20;  %时间，第一个和第三个是时间的起始和末尾，中间是采样间隔
U=0.2*ones(size(T));  %系统输入
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T);  %计算系统对输入向量u的响应
plot(T,Y(:,1),':',T,Y(:,2),'-',T,Y(:,3),'*')  %绘制系统的响应曲线
legend('cart position ', 'pendulum angle1' ,'angle2')  %曲线的参数
grid  %保留图像的栅格