直线二级倒立摆建模与仿真
1、直线二级倒立摆建模
为进行性线控制器的设计,首先需要对被控制系统进行建模.二级倒立摆系统数学模型的建立基于以下假设：
1)每一级摆杆都是刚体；
2)在实验过程中同步带长保持不变；
3)驱动力与放大器输入成正比，没有延迟直接拖加于小车；
4)在实验过程中动摩擦、库仑摩擦等所有摩擦力足够小，可以忽略不计。
图1 二级摆物理模型
二级倒立摆的参数定义如下：
M 小车质量
m1摆杆1的质量
m2摆杆2的质量
m3质量块的质量
l1摆杆1到转动中心的距离
l2摆杆2到转动中心的距离
θ1摆杆1到转动与竖直方向的夹角
θ2摆杆2到转动与竖直方向的夹角
F 作用在系统上的外力
利用拉格朗日方程推导运动学方程
拉格朗日方程为：
其中L 为拉格朗日算子，q 为系统的广义坐标，T 为系统的动能，V 为系统的势能
其中错误!未找到引用源。
,错误!未找到引用源。
为系统在第i 个广义坐标上的外力，在二级倒立摆系统中，系统有三个广义坐标，分别为x,θ1,θ2,θ3。
首先计算系统的动能：
其中错误!未找到引用源。
,错误!未找到引用源。
,错误!未找到引用源。
,错误!未找到引用源。
分别为小车的动能，摆杆1的动能，摆杆2的动能和质量块的动能。
小车的动能：
错误!未找到引用源。
,其中错误!未找到引用源。
，错误!未找到引用源。
分别为摆杆1的平动动能和转动动能。
错误!未找到引用源。
，其中错误!未找到引用源。
，错误!未找到引用源。
分别为摆杆2的平动动能和转动动能。
对于系统，设以下变量： xpend1摆杆1质心横坐标 xpend2摆杆2质心横坐标 yangle1摆杆1质心纵坐标 yangle2摆杆2质心纵坐标 xmass 质量块质心横坐标 ymass 质量块质心纵坐标 又有：
(,)(,)(,)
L q q T q q V q q =-
则有：
系统总动能：系统总势能：则有：
求解状态方程：可解得：
使用MATLAB对得到的系统进行阶跃响应分析，执行命令：
A=[0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 1 01;
0 0 0 0 0 0;
0 86.69 -21.62 0 0 0;
0 -40.31 39.45 0 0 0];
B=[0;0;0;1;6.64;-0.808];
C=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0];
D=[0;0;0];
sys=ss(A,B,C,D);
t=0:0.001:5;
step(sys,t)
求取系统的单位阶跃响应曲线：
图2 二级摆阶跃响应曲线
由图示可知系统小车位置、摆杆1角度和摆杆2角度均发散，需要设计控制器以满足期望要求。
2.直线二级倒立摆极点配置控制
通过引入状态反馈的方法进行极点配置，使系统满足要求。
我们借用LQR的模块，只是把LQR中K值的计算方式变了。
图3 二级摆极点配置演示图
对于控制系统
=+
X AX Bu
式中
X 为状态向量（n维）、u控制向量（纯量）、A为n×n维常数矩阵、B为n×1维常数矩阵、选择控制信号为：错误!未找到引用源。
求解上式，得到
()()()x t A BK x t =-
方程的解为：
()()(0)A BK t x t e x -=
可以看出，如果系统状态完全可控，K 选择适当，对于任意的初始状态，当t 趋于无穷时，都可以使x (t )趋于0。
极点配置的设计步骤：
1) 检验系统的可控性条件。
2) 从矩阵 A 的特征多项式
111n sn n n
sI A s a a s a ---=++
++
来确定
1a ,
2
a ,…
n
a 的值。
3) 确定使状态方程变为可控标准型的变换矩阵 T:
T MW =
其中 M 为可控性矩阵,
1n M B AB A B -⎡⎤=⎣⎦
1211231
001
1001
000an an a an an W a --⎡⎤
⎢⎥--⎢⎥
⎢
⎥=⎢
⎥⎢⎥⎢⎥⎣
⎦
4) 利用所期望的特征值，写出期望的多项式
11211()()()n n n n n
s s s s s s μμμααα-----=++++
并确定错误!未找到引用源。
的值。
5) 需要的状态反馈增益矩阵 K 由以下方程确定：
111
2211[]n n n n K a a a a T αααα---=----
上述方法给出所要求的状态反馈增益矩阵K 。
前面我们已经得到了直线一级倒立摆的状态空间模型，以小车加速度作为输入的系统状态方程为：
于是可以确定出相应的A 、B 、C 、D 。
按极点配置步骤进行计算：
1) 检验系统可控性，由系统可控性分析可以得到，系统的状态完全可控性矩阵的秩等于系统的状态维数6，系统的输出完全可控性矩阵的秩等于系统输出向量y 的维数3，所以系统可控。
2)计算特征值，根据要求，并留有一定的裕量（设调整时间为2.5秒），我们选取期望的闭环极点：
P=[-10-0.0001*j,-10+0.0001*j,-1.5-2*sqrt(3)*j,-1.5+2*sqrt(3)*j,-5+0.1*j,-5-0.1*j]
因此期望的特征方程为：
3) 确定使状态方程变为可控标准型的变换矩阵 T: 4) 于是有状态反馈增益矩阵K 为：
K=13.9849、79.9262、-121.6517、11.3329、3.0025、-19.6633
状态反馈增益矩阵按上述的方法确定，即可使误差（由扰动所引起的）以足够快的速度
降到零。
对于一个给定的系统，矩阵并不是唯一的，而是取决于所期望的闭环极点位置的选择。
选择期望的闭环极点或期望的特征方程是在误差矢量响应的快速性与对扰动和测量噪声敏感型之间的一个折衷方案。
也就是说，如果我们使误差响应的速度提高，那么扰动和测量噪声的有害影响往往也会增强。
在确定给定系统的状态反馈增益矩阵K时，通常是通过比较按不同的期望闭环极点或期望特征方程得到的矩阵K。
下面对以上的计算结果在MATLAB Simulink中进行仿真：
图4 二级摆极点配置仿真图
运行仿真，得到以下结果：
图5 二级摆极点配置仿真结果
可以看出，在给定系统干扰后，倒立摆可以在2秒内很好的回到平衡位置，满足设计要求。
3.实验数据记录与结果分析
将LQR的模块进行改动
按照仿真结果参数进行调节，得到结果如下：
图6 二级摆极点配置实际控制仿真图
示波器窗口中，从上至下一次表示小车位置，二级摆角度、一级摆角度。
由图可知，这三个量都呈现出微小的简谐振动，但系统基本上可以稳定，满足实验期望。