单级倒立摆从建模到仿真

把很久以前的草稿整理一下搬到博客上

1.动力学模型

如图1所示，单级倒立摆由系统由水平导轨，平移支座和摆杆构成。平移支座与摆杆无阻尼铰接（摆杆可自由摆动）。平移支座可以在导轨上受控平移。摆杆的质量是 $m$ ，质心到铰链轴心的距离是 $l$ ，过质心的 $z$ 轴方向的转动惯量是 $I_z$

根据拉格朗日力学对系统进行动力学建模。首先，根据系统的自由度确定描述系统运动的广义坐标。系统的自由度是2。因此，广义坐标可以取平移支座的水平位移 $x_c$ 和摆杆的摆角θ。系统拉格朗日方程（以下 $x_c$ 不再带角标）为

$\left\{\begin{matrix} \frac{\mathrm{d} }{\mathrm{d} t}\! \! \left ( \frac{\partial L}{\partial \dot{x}} \right )-\frac{\partial L}{\partial x}=F_x \\ \\ \frac{\mathrm{d} }{\mathrm{d} t}\! \! \left ( \frac{\partial L}{\partial \dot{\theta}} \right )-\frac{\partial L}{\partial \theta}=0 \end{matrix}\right.$ (1)

其中 $L=T-V$ ，是拉格朗日函数。 $F_x$ 是平移支座的驱动力。

动能：

$T=\frac{1}{2}m\overrightarrow{\! v}\! \! \cdot\! \overrightarrow{\! v}+\frac{1}{2}I_z\dot{\theta}^2$ (2)

$\overrightarrow{\! v}=\begin{bmatrix} \dot{x}\! -\! l\dot{\theta} \sin\theta\\ l\dot{\theta} \cos\theta \end{bmatrix}$ (3)

重力势能：

$V=mg\sin\theta$ (4)

这里研究位移控制，即输入量是位移 $x$ ，而不是驱动力矩 $F_x$ 。因此，只对方程组(1)中的第二个展开。得到动力学方程

$\left ( ml^2+I_z \right )\ddot{\theta}+mgl\cos\theta=ml\ddot{x}\sin\theta$ (5)

动力学模型（被控系统）Simulink框图见图2，初始摆角是 $5\pi/12$ 。控制系统框图如图3所示，支座的初始位移是0.1m 。

2.控制器设计

2.1 摆杆倒立控制

倒立摆控制的首要任务是摆杆的稳定倒立。方程(5)是个非线性微分方程，为了能用线性系统理论对它进行分析，需要先进行线性化。从方程(5)的形式可以看出，使用反馈线性化方法比较合适。令

$\ddot{x}=\frac{u}{\sin\theta}+g\cot\theta$ (6)

于是得到线性的动力学方程

$\left ( ml^2+I_z \right )\ddot{\theta}=u$ (7)

摆杆的相对竖直位置的偏差 $e_\theta\! =\! \pi/2\! -\! \theta$ , 于是有状态方程

$\begin{bmatrix} \dot{e_\theta}\\ \ddot{e_\theta} \end{bmatrix} =\frac{ml}{ml^2+I_z}\begin{bmatrix} 0 & 1\\ 0 & 0 \end{bmatrix} \begin{bmatrix} e_\theta\\ \dot{e_\theta} \end{bmatrix}+\begin{bmatrix} 0\\1 \end{bmatrix}u$ (8)

引入状态反馈

$u=k_1e_\theta+k_2\dot{e_\theta}$ (9)

得到

$\begin{bmatrix} \dot{e_\theta}\\ \ddot{e_\theta} \end{bmatrix} =\frac{ml}{ml^2+I_z}\begin{bmatrix} 0 & 1\\ -k_1 & -k_2 \end{bmatrix} \begin{bmatrix} e_\theta\\ \dot{e_\theta} \end{bmatrix}$ (10)

当方程(10)中 $k_1$ 和 $k_2$ 都大于零时状态向量 $\begin{bmatrix} e_\theta &\! \! \! \dot{e_\theta} \end{bmatrix}^T$ 即可收敛，例如k1=2，k2=3，仿真结果见4。

2.2 支撑点位置控制

从图4可见，当摆杆倒立稳定后，平移支座的水平位移一直在以恒定速率朝一个方向变化。这显然没有达到控制要求。因此，在系统的状态变量中再添加上平移支座的位移和速率。将摆杆角度偏差 $e_\theta\! =\! \pi/2\! -\! \theta$ 代入方程(5)，再在 $e_\theta\! =\! 0$ 处线性化

$\ddot{e}_\theta\approx \frac{g}{ml^2+I_z}e_\theta-\frac{1}{ml^2+I_z}\ddot{x}$ (11)

于是有状态方程

$\begin{bmatrix} \dot{e}_\theta\\ \ddot{e}_\theta\\\dot{x}\\ \ddot{x} \end{bmatrix} =\begin{bmatrix} 0 &1 &0 &0 \\ \frac{g}{ml^2+I_z}& 0& 0& 0\\ 0 & 0 & 0 &1 \\ 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} e_\theta\\ \dot{e}_\theta\\x\\ \dot{x} \end{bmatrix} +\begin{bmatrix} 0\\ -\frac{1}{ml^2+I_z}\\ 0\\ 1 \end{bmatrix} \ddot{x}$ (12)

系统完全可控，可以任意配置极点，取状态反馈

$\ddot{x}=K\begin{bmatrix} x & \dot{x} & \theta & \dot{\theta} \end{bmatrix}^T$ (13)

将极点配置在[-3 -2 -9 -6]处，解得K=[212 68 50 47 ]。MATLAB求解代码见附录，输出结果见图5。

4.串级PD控制

PD参数整定的基本思路是先确定控制的首要任务，优先确定与之相关的参数。在这里首要任务是摆杆的稳定倒立，因此先对把杆角度偏差的PD响应参数Kp和Kd进行整定。在整定这两个参数时，在摆杆稳定的前提下，应尽可能取较大的Kp和Kd。摆杆倒立的PD参数整定好后，再进行平移支座的位移稳定PD参数整定。在这里根据控制任务的重要性，摆杆倒立稳定控制优先于平移支座的位移稳定控制。虽然支座位移是控制输入，但是根据任务优先顺序，支座平移稳定只属于间接的控制任务。由于支座位移稳定属于间接任务，于是在位移控制中：欲使支座往右移，得先使摆杆向右倾，支座就得先向左加速。这就是位移控制的参数正负号的确定思路。由于在摆杆角度偏差较大时，角度偏差PD反馈主导控制器输出；角度偏差较小时，支座水平位移PD反馈起才起作用。因此其增益参数Kp和Kd的值应比角度偏差反馈的PD参数小得多。位移PD反馈的增益参数整定顺序是先Kd后Kp，且Kd要大于Kp。这与偏角反馈的正好相反，例如[120 70 30 40]，输出结果见图6。

5.总结

在以上的分析中，状态反馈的和串级PD控制的本质是等效的，都是根据四个被测变量做反馈控制。状态反馈的极点配置需要提前知道动力学模型的参数，而串级PD反馈是通过实验来整定增益参数的。根据图5和图6的曲线来看，使用极点配置状态反馈的曲线比串级PD控制的更加光滑。这有利于防止电机损坏，所以在实践中可以将两种方法结合起来使用，各取所长。

附录：

syms theta a g
a=1;g=9.8;    %a=ml^2+Iz
A=[0 1 0 0;0 0 0 0;0 0 0 1;0 0 g/a 0];  %状态矩阵
b=[0;1;0;-1/a];       %输出矩阵
Q=[b A*b A*A*b A*A*A*b];     %能控性判别矩阵
s=[-3-0i -2+0i -9-0i -6+0i]; %期望极点
S=diag(s);
Ke=poly(S);                  %期望特征多项式
Ka=poly(double(subs(A)));    %被控对象特征多项式
K_=[Ka(5)-Ke(5) Ka(4)-Ke(4) Ka(3)-Ke(3) Ka(2)-Ke(2)];
Tc=[A*A*A*b A*A*b A*b b]*[Ka(1) 0 0 0;Ka(2) Ka(1) 0 0;Ka(3) Ka(2) Ka(1) 0;Ka(4) Ka(3) Ka(2) Ka(1)];
K=double(subs(K_/Tc))