三级倒立摆建模与控制器设计仿真

三级倒立摆摘要：从刚体运动学角度分析了三级倒立摆系统的运动，运用状态空间法对三级倒立摆系统进行数学模型的建立;应用二次型最优控理论实现三级倒立摆控制；最后根据计算结果，通过Matlab程序仿真，得出上中下摆的角度和小车的位移曲线图。关键词：倒立摆；最优控；仿真；MATLAB三级倒立摆系统模型一个典型的三级直线倒立摆系统主要由机电装置和控制装置两部分组成,机电装置的结构见图!其主要由摆杆、电机、小车、导轨、传动齿轮、齿条及各种传感器等构成假设系统中的每一根摆杆都是匀质刚体，驱动力与放大器的输入成正比且无延迟地直接作用于小车上，并且可以在忽略实验中的库仑摩擦和动摩擦的前提下，图1所示的倒立摆系统的物理分析：三级倒立摆受力分析图：小车受力分析图下摆受力分析中摆受力分析小车图1系统分析示意图𝐹11𝐹12𝐹11𝐹12𝜃1𝐹22𝐹21yxY𝐹31𝐹33X𝐹21𝐹22𝐹33𝐹31XY𝜃2𝜃3上摆受力分析下摆牛顿定律及动力矩方程：𝐹12−𝐹22=𝑚1𝑑2𝑑𝑡2(𝑟+𝑙1sin𝜃1)𝑚1g−𝐹11+𝐹21=𝑚1𝑑2𝑑𝑡2(𝑙1cos𝜃1)J𝜃1̈=𝐹11𝑙1sin𝜃1+𝐹21(𝐿1−𝑙1)sin𝜃1−𝐹12𝑙1cos𝜃1−𝐹22(𝐿1−𝑙1)cos𝜃1中摆受力分析：𝐹22−𝐹31=𝑚2𝑑2𝑑𝑡2(𝑟+𝐿1sin𝜃1+𝑙2sin𝜃2)𝑚2g−𝐹21+𝐹33=𝑚2𝑑2𝑑𝑡2(𝐿1cos𝜃1+𝑙2cos𝜃2)J𝜃2̈=𝐹21𝐿2sin𝜃2+𝐹33(𝐿2−𝑙2)sin𝜃2−𝐹22𝑙2cos𝜃2−𝐹31(𝐿2−𝑙2)cos𝜃2丄摆受力分析：𝐹31=𝑚3𝑑2𝑑𝑡2(𝑟+𝐿1sin𝜃1+𝐿2sin𝜃2+𝑙3sin𝜃3)𝑚3g−𝐹33=𝑚3𝑑2𝑑𝑡2(𝐿1cos𝜃1+𝐿2cos𝜃2+𝑙3cos𝜃3)J𝜃3̈=𝐹33𝑙3sin𝜃3−𝐹31𝑙3cos𝜃3注：𝑙1𝑙2𝑙3—下，中，上摆质心到铰链处距离；𝐿1𝐿2–下，中摆的长度；𝜃1𝜃2𝜃3−−上，中，下摆偏离竖直方向的角度；𝑚1𝑚2𝑚3−−下，中，上摆质量;其动力学方程如下⑴：M(𝜃1,𝜃2,𝜃3)[𝑟̈𝜃1̈𝜃2̈𝜃3̈]=F(𝜃1,𝜃2,𝜃3,𝜃1̇,𝜃2,̇𝜃3̇)[𝑟̇𝜃1̇𝜃2̇𝜃3̇]+𝑁(𝜃1,𝜃2,𝜃3)[𝑟𝜃1𝜃2𝜃3]+𝐺𝑈⑴M(𝜃1,𝜃2,𝜃3)=[𝑚0+𝑚1+𝑚2+𝑚3𝑚1𝑑1cos𝜃1+𝑚2𝑑4cos𝜃1+𝑚3𝑑4cos𝜃1𝑚2𝑑2cos𝜃2+𝑚3𝑑5cos𝜃2𝑚3𝑑3cos𝜃3𝑚1𝑑1cos𝜃1+𝑚2𝑑4cos𝜃1+𝑚3𝑑4cos𝜃1𝐽1+𝑚1𝑑12+𝑚2𝑑42+𝑚3𝑑42𝑚2𝑑2𝑑4cos(𝜃1−𝜃1)+𝑚3𝑑4𝑑5cos(𝜃2−𝜃1)𝑚3𝑑3𝑑4cos(𝜃3−𝜃1)𝑚2𝑑2cos𝜃2+𝑚3𝑑5cos𝜃2𝑚2𝑑2𝑑4cos(𝜃1−𝜃1)+𝑚3𝑑4𝑑5cos(𝜃2−𝜃1)𝐽2+𝑚2𝑑22+𝑚3𝑑52𝑚3𝑑3𝑑5cos(𝜃3−𝜃2)𝑚3𝑑3cos𝜃3𝑚3𝑑3𝑑4cos(𝜃3−𝜃1)𝑚3𝑑3𝑑5cos(𝜃3−𝜃2)𝐽3+𝑚3𝑑32]F(𝜃1,𝜃2,𝜃3,𝜃1̇,𝜃2,̇𝜃3̇)=[−𝑓0000(𝑚1𝑑1sin𝜃1+𝑚2𝑑4sin𝜃1+𝑚3𝑑4sin𝜃1)𝜃1−(𝑓1+𝑓2)𝑓2−𝑚2𝑑2𝑑4sin(𝜃2−𝜃1)+𝑚3𝑑4𝑑5sin(𝜃2−𝜃1)−𝑚3𝑑3𝑑4sin(𝜃3−𝜃1)(𝑚2𝑑2sin𝜃2+𝑚3𝑑5sin𝜃2)𝜃2𝑓2+(𝑚2𝑑2𝑑4+𝑚3𝑑4𝑑5)𝜃2sin(𝜃2−𝜃1)−(𝑓3+𝑓2)𝑓3−𝑚3𝑑3𝑑5𝜃2sin(𝜃3−𝜃2)𝑚3𝑑3sin𝜃3∙𝜃3𝑚3𝑑3𝑑4sin(𝜃3−𝜃1)𝑓3+𝑚3𝑑3𝑑5𝜃3sin(𝜃3−𝜃2)−𝑓3]N(𝜃1,𝜃2,𝜃3)=[0(𝑚1𝑑1+𝑚2𝑑4+𝑚3𝑑4)sin𝜃1(𝑚2𝑑2+𝑚3𝑑5)𝑔sin𝜃2𝑚3𝑑3𝑔sin𝜃3]G=[𝐺0000]𝑇将此非线性模型在系统的平衡点r=𝜃1=𝜃2=𝜃3=0,𝑟=̇𝜃1̇=𝜃2̇=𝜃3=0̇处线性化得到系统的状态方程:{𝑥̇=𝐴𝑥+𝐵𝑢𝑦=𝐶𝑥+Du⑵A=|04∗4I4∗4A21A22|,B=|04∗1𝐵2|其中：将参数带入方程计算：A=[00000000000000−3.655145.74−49.782−5.159000.12698−80.216169.9−114.49010001000.44281−8.3647−4.78172.26900−16.80957.098−62.1436.4401000.016897−0.562720.98145−0.53453000010−0.00564470.32956−0.659640.5167301−0.0032595−0.0721790.32957−0.76437]B=[00008.72−29.62132.238−3.341]𝑇C=[10000100001000010000000000000000]D=[0000]𝑇控制器设计根据由图⑴所示的倒立摆系统模型的推导可知，该系统在一定的前提下，可以转变为线性系统#对于线性系统的控制，需要经过系统的稳定性、能控性和能观性判断后才能选择其最优的控制策略#图!显示了式（$）所示的线性系统外控制器的设计过程状态方程的重构过程，实质上就是本文从式⑴式转化为式⑵式的过程，在MATLAB中，可以用如下代码表示:％系统状态方程的重构％状态变量(𝑋1,𝑋2,𝑋3,𝑋4,𝑋5,𝑋6,𝑋7,𝑋8)=(r,zeta1,zeta2,zeta3,𝑟,,𝑧𝑒𝑡𝑎1,,zeta2,,𝑧𝑒𝑡𝑎3,)％输出变量Y=XA=zeros(8,8);A(1:4,5:8)=eye(4,4);A(5:8,1:4)=T0M(000)−1𝑁(000)𝑇0−1;A(5:8,5:8)=T0M(000)−1[−𝐹(000000)]𝑇0−1;％不用invese而用blackslash是由于blackslash有更好的错误检查机制并且少用时间和内存。B=zeros(8,1);B(5:8)=T0M(000)−1[𝐺0000]𝑇;C=zeros(4,8)C(1:4,1:4)=eye(4,4);D=zeros(4,1);系统的稳定性、能控性及能惯性判断对于如式（2）式所示的线性定常系统而言，其稳定性可通过计算系统矩阵A的特征值来确定(李雅普诺夫线性系统稳定判据)，如果特征值均处于[S]复平面左半平面，则系统稳定。MATLAB中，用函数eig（A）来计算系统矩阵的特征值，经过计算，式⑵式所示的线性定常系统的特征值为：r1=[013.27158.39193.1656-18.2950-2.9940-8.5547-13.7810]因此，该系统为不稳定系统，需加外部控制器形如式（2）所示的线性定常系统能控的充分必要条件是：rank[BAB…An−1B]=n;MATLAB中，用“ctrb(A,B)”来求系统能控矩阵Pc=[BAB…An−1B]的秩，经过计算rank(Pc)=8,系统能控。形如式（2）所示的线性定常系统能观的充分必要条件为：rank[CCA⋮CAn−1]=nMATLAB中，用“obsv(A,C)”来求系统能观阵Qo=[CCA⋮CAn−1]的秩，计算的，rank(Qo)=8,系统能观。综上所述，式（2）所示的线性定常系统为不稳定的能控、能观系统，可加外控制器实现系统的稳定。线性最有调节器的设计定义状态方程⑵的二次性能指标函数为：J=12∫(XTQX+uTRu)dt∞0⑶正定R阵的作用在于限制控制器的输出不会太大而导致难于实现，所以不失一般性，可以将其设为1，LQR方法中，Q阵可以选为非负的对角阵，Q=diag（q11，q22，q33，q44，q55，q66，q77，q88）.各主对角系数的相对大小对系统的整体控制性能有很大的影响，目前普遍采用试探实验的方法来确定加权矩阵Q的系数大小。观察知：q11大小的变化对小车的位移影响很大，因为小车位移大小受直线导轨长度的物理限制，所以，q11取值可以固定为一个适当大小的数，使小车位移不至过大；q22对系统控制的影响最为明显，q33次之，q44的影响较小。取值时可以去q22q33q44,取Q=diag(500,500,300,100,0,0,0,0),R=1.求解Riccati代数方程PA+ATP-PBR−1BTP+Q=0⑷可以得到状态反馈阵KT=R−1BTP=[−22.3607−80.103528.5577−408.4518−30.1003−53.7813−44.7459−53.7370]此时各摆角的动态和稳态指标较为理想。初始条件为r0=0，θ10=0.050，θ20=−0.050，θ30=0.050。仿真结果程序：A=[0,0,0,0,1,0,0,0;0,0,0,0,0,1,0,0;0,0,0,0,0,0,1,0;0,0,0,0,0,0,0,1;0,-3.6551,0.12698,0.44281,-16.809,0.016897,-0.0056447,-0.0032595;0,45.74,-80.216,-8.3647,57.098,-0.56272,0.32956,-0.072179;0,-49.782,169.9,-4.781,-62.143,0.98145,-0.65964,0.32957;0,-5.159,-114.49,72.269,6.4401,-0.53453,0.51673,-0.76437;];B=[0;0;0;0;8.72;-29.621;32.238;-3.341];C=[1,0,0,0,0,0,0,0;0,1,0,0,0,0,0,0;0,0,1,0,0,0,0,0;0,0,0,1,0,0,0,0];D=[0;0;0;0];r1=eig(A)q1=500;q2=500;q3=300;q4=100;q5=0;q6=0;q7=0;q8=0;Q=[q10000000;0q2000000;00q300000;000q40000;0000q5000;00000q600;000000q70;0000000q8];R=1;[K,P,r2]=lqr(A,B,Q,R)Ac=[(A-B*K)];Bc=[B];Cc=[C];Dc=[D];T=0:0.02:20;U=zeros(size(T));x0=[0;0.05;-0.05;0.05;0;0;0;0];[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T,x0);figure(1);plot(T,Y(:,1));xlabel('Time/sec');ylabel('x(小车)/m');title('小车位移');grid;figure(2);plot(T,Y(:,2));xlabel('Time/sec');ylabel('01/rad');title('下摆角度变化曲线');grid;figure(3);plot(T,Y(:,3));xlabel('Time/sec');ylabel('02/rad');title('中摆角度变化曲线');grid;figure(4);plot(T,Y(:,4));xlabel('Tim