利用拉格朗日方程推导运动学方程
拉格朗日方程为：
其中L为拉格朗日算子，q为系统的广义坐标，T为系统的动能，V为系统的势能。
其中f_1为系统在第i个广义坐标上的外力，在二级倒立摆系统中，系统有三个广义坐标，分别为x,θ_1，θ_2。
首先计算系统的动能：
T=T_M+T_m1+T_m2+T_m3
其中T_M，T_m1，T_m2，T_m3分别为小车的动能，摆杆1的动能，摆杆2的动能和质量块的动能。
小车的动能：
对于系统，设以下变量：
Xpend1摆杆1质心横坐标
Xpend2摆杆2质心横坐标
Yangle1摆杆1质心纵坐标
Yangle2摆杆2质心纵坐标
Xmass 质量块质心横坐标
Ymass 质量块质心横坐标
又有:
则有：
系统总动能：
T=T_M+T_m1+T_m2+T_m3
系统总势能：
V=V_m1+V_m2+V_m3=m_1 ypend1+m_2 ypend2+m_3 ymass
则有：
V=m_1 l_1 cosθ_1+m_2 (2l_1 cosθ_1+l_2 cosθ_2)+2m_3 l_1 cosθ_1
求解状态方程：
可解得：
使用matlab进行编程，A=[0 0 0 1 0 0;0 0 0 0 1 0;
0 0 0 0 0 1;
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.088];
C=[ 1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
];
D=[0;0;0];
M=ss(A,B,C,D);
t=0:0.001:5;
step(M,t);
得到图线为：
由图像可知，系统小车位置、摆杆A角度和摆杆二角度发散，需要增设控制器。
二、直线二级倒立摆使用极点配置方法控制
二级倒立摆系统是一个不稳定的系统。设计控制器的目的是使倒立摆系统动态稳定，即使小车在外力作用下其位移以较小的误差跟随输入的变化。由于系统的动态响应主要是由他的极点位置决定的 ,同时容易证明二级倒立摆系统是一个能控而且能观的系统。因此本文通过极点配置状态反馈控制器来使系统保持稳定。建立simulink模型，如图所示：
首先设配置的极点为-2+i2√3，-2-i2√3，-10，-10,-10，-10的矩阵
并求出对应的特征方程，再利用A，B计算出能控标准型M，由特征方程和A计算出特征值，然后利用matlab编辑器使用函数计算出K值（公式：K=[0 0 0 0 0 1]*(inv(M))*Phi），[ 62.7840 122.6947 -280.2655 40.8096 -0.1389 -46.7317].
显示波形图：
然后增设扰动，simulink仿真模型如图：
扰动数据如下图所示：每4 S突加一个值为1的脉冲，持续时间为2 S，
显示波形如下图，在脉冲初次来到之前，系统正常运行，而当脉冲到来时，系统出现微弱波动，然后进行反馈调节，逐渐稳定，持续2 S，然后周期进行。
三、心得体会
本文以二级倒立摆为研究对象 ,讨论了将极点配置 在期望的区域内的状态反馈控制方法。从结果可以看出 ,该方法可以保证系统具有一定的动态和稳态性能 ,不仅满足闭环系统的内部动态特性要求 , 也兼顾了抑制外部扰动对系统的影响。由此可知 , 极点配置控制方法可以实现二级摆的倒立平衡控制。
