倒立摆分析

Catalogue
  1. 1. 1. 倒立摆案例
  2. 2. 2. 法一
    1. 2.1. 2.1. 已知参数
    2. 2.2. 2.2. 对摆进行水平受力分析
    3. 2.3. 2.3. 对车进行水平受力分析
    4. 2.4. 2.4. 对摆进行竖直方向受力分析
    5. 2.5. 2.5. 对摆进行转矩平衡分析
    6. 2.6. 2.6. 近似化简
  3. 3. 3. 法二
    1. 3.1. 3.1. 小车动能
    2. 3.2. 3.2. 杆动能
    3. 3.3. 3.3. 系统总动能
    4. 3.4. 3.4. 系统总势能
    5. 3.5. 3.5. 构造拉格朗日约束
    6. 3.6. 3.6. 系统状态空间表达
    7. 3.7. 3.7. 能观性能控性分析
      1. 3.7.1. 3.7.1. 构造能观性判别矩阵
      2. 3.7.2. 3.7.2. 构造能控性判别矩阵
    8. 3.8. 3.8. 稳定性分析

1. 倒立摆案例

2. 法一

2.1. 已知参数

假设:

  • 摆的重心坐标\((z+ l\sin \theta,l \cos \theta)\)
  • \(\theta\)从垂直向上,向右为正方向
  • \(u\)向右为正方向

2.2. 对摆进行水平受力分析

摆在水平方向,受到接合处向右的力\(H\)

根据牛顿第二定律,\(F=ma\),有

\[ \begin{aligned} H &=m \frac{d^2(z+l \sin \theta)}{dt^2} \\ &=m\frac{d( \dot{z}+l \cos \theta \dot{ \theta })}{dt} \\ &=m( \ddot{z}+l \cos\theta \ddot{ \theta}-\sin\theta \dot{ \theta}^2) \\ &=m\ddot{z}+ml\cos\theta\ddot{ \theta }-ml\sin\theta\dot{ \theta }^2 \end{aligned} \tag{1} \]

2.3. 对车进行水平受力分析

根据牛顿第二定律,\(F=ma\),有

\[ \begin{aligned} u-H=M\ddot{z} \tag{2} \end{aligned} \]

将(1)带入(2),得到

\[ \Rightarrow u=(M+m)\ddot{z}++ml\cos\theta\ddot{\theta}-ml\sin\theta\dot{\theta}^2 \tag{3} \]

2.4. 对摆进行竖直方向受力分析

根据牛顿第二定律,\(F=ma\),有

\[ \begin{aligned} v-mg&=m\frac{d^2(l\cos\theta)}{dt^2} \\ &=m\frac{d(l(-\sin \theta \dot{\theta}))}{dt} \\ &=ml(-\cos \theta \dot{\theta}^2 - \sin \theta \ddot{\theta}) \\ &=ml\sin \theta \ddot{\theta} - ml \cos \theta \dot{\theta}^2 \end{aligned} \tag{4} \]

2.5. 对摆进行转矩平衡分析

在这里,将摆视为绕着杆的重心旋转,那么,杆在端点(也就是接合处)受到的力沿杆方向进行分解,可以得到两个作用相反的力,分别为\(H\cos \theta\)\(v \sin \theta\),不难发现,\(H\cos \theta\)使的角\(\theta\)减小,而\(v \sin \theta\)使\(\theta\)增大

根据转矩平衡,和外力转矩 = 转动惯量 * 角加速度,即有\(M=J\alpha\),得:

\[ vl\sin \theta - Hl \cos \theta = J \ddot{\theta} \tag{5} \]

又根据(1)和(4),

\[ \left \{ \begin{aligned} H=m\ddot{z}+ml\cos\theta\ddot{\theta}-ml\sin\theta\dot{\theta}^2 \\ v=mg+ml\sin \theta \ddot{\theta} - ml \cos \theta \dot{\theta}^2 \end{aligned} \right. \]

代入式(5),得到

\[ \begin{aligned} -m\ddot{z}l\cos \theta - ml^2(\cos^2 \theta-\sin^2 \theta)\ddot{\theta}+mgl\theta = J \ddot{\theta} \end{aligned} \tag{6} \]

2.6. 近似化简

\(\theta\)很小的时候,近似有

\[ \begin{aligned} \cos \theta = 1 \\ \sin \theta =\theta \\ (\frac{d \theta}{dt})^2=\dot{\theta}^2 \approx0 \end{aligned} \]

因此,将式(3)和式(6)进行化简,有:

\[ \left \{ \begin{aligned} u=(M+m)\ddot{z}+ml \ddot{\theta} \\ (J+ml^2)\ddot{\theta}=-ml\ddot{z}+mgl\theta \end{aligned} \right . \tag{7} \]

求解方程组(7),最终,可以得到系统的状态空间描述:

\[ \begin{aligned} \dot{X}= \begin{bmatrix} \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & \frac{-m^2gl^2}{J(M+m)+Mml^2} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{mgl(M+m)}{J(M+m)+Mml^2} & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{J+ml^2}{J(M+m)+Mml^2} \\ 0 \\ \frac{-ml}{J(M+m)+Mml^2} \end{bmatrix} u \end{aligned} \tag{8} \]

进一步地,如果杆是均匀质杆,那么有转动惯量\(J=\frac{ml^2}{3}\)

代入到式(7)的第二个方程,有

\[ \begin{aligned} (\frac{ml^2}{3}+ml^2)\ddot{\theta}=-ml\ddot{z}+mgl\theta \end{aligned} \tag{9} \]

即可求出\(\ddot{\theta}\)的表达

\[ \ddot{\theta}=\frac{3g}{4l}\theta-\frac{3}{4l}\ddot{z} \]

如果令输入\(u=\ddot{z}\),那么,状态方程可以写成

\[ \begin{aligned} \dot{X}= \begin{bmatrix} \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{3g}{4l} & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ 0 \\ \frac{-3}{4l} \end{bmatrix} u \end{aligned} \tag{8} \]

3. 法二

拉格朗日约束法

因此,假设有某个广义坐标\(q\)以及拉格朗日算子\(L\),则拉格朗日方程可表示为:

\[ \frac{d}{dt}(\frac{d L}{d \dot{q}})-\frac{d L}{d q}=f \]

其中,f为沿着广义坐标轴方向上的外力。

对于上述系统,定义由\(z,\theta\)组成的广义坐标系,T为系统的动能,V为系统的是势能。

3.1. 小车动能

\[ T_M=\frac{1}{2}M\dot{z}^2 \tag{9} \]

3.2. 杆动能

\[ \begin{aligned} T_m&=\frac{1}{2}mv^2+\frac{1}{2}J\dot{\theta}^2 \\ &=\frac{1}{2}m\{\underbrace{[\frac{d(z+l\sin \theta)}{dt}]^2}_{v_x}+\underbrace{[\frac{d(l\cos \theta)}{dt}]^2}_{v_y}\}+\frac{1}{2}J\dot{\theta}^2 \end{aligned} \tag{10} \]

3.3. 系统总动能

\[ \begin{aligned} T=T_M+T_m &=\frac{1}{2}M\dot{z}^2+\frac{1}{2}mv^2+\frac{1}{2}J\dot{\theta}^2 \\ &=\frac{1}{2}M\dot{z}^2+\frac{1}{2}m\{\underbrace{[\frac{d(z+l\sin \theta)}{dt}]^2}_{v_x}+\underbrace{[\frac{d(l\cos \theta)}{dt}]^2}_{v_y}\}+\frac{1}{2}J\dot{\theta}^2 \\ &=\frac{1}{2}M\dot{z}^2+\frac{1}{2}m\dot{z}^2+m\dot{z}\cos \theta \dot{\theta}+\frac{2}{3}ml^2\theta^2 \end{aligned} \tag{11} \]

3.4. 系统总势能

\[ V=mgl\cos \theta \]

3.5. 构造拉格朗日约束

拉格朗日算子

\[ \begin{aligned} L=T-V= \frac{1}{2}M\dot{z}^2+\frac{1}{2}m\dot{z}^2+m\dot{z}\cos \theta \dot{\theta}+\frac{2}{3}ml^2\theta^2 - mgl\cos \theta \end{aligned} \tag{12} \]

由于在广义坐标轴\(\theta\)上没有外力作用,因此f=0,所以有s拉格朗日方程:

\[ \frac{d}{dt}(\frac{d L}{d \dot{\theta}})-\frac{d L}{d \theta}=0 \tag{13} \]

需计算如下

\[ \frac{d L}{d \theta}=-m\dot{z}l \sin \theta \dot{\theta}+ mgl \sin \theta \tag{14} \]

\[ \frac{d L}{d \dot{\theta}}=m\dot{z}l\cos\theta+\frac{4}{3}ml^2\dot{\theta} \tag{15} \]

\[ \frac{d}{dt}(\frac{d L}{d \dot{\theta}}) = m\ddot{z}\cos \theta - m \dot{z}l\sin \theta \dot{\theta}+\frac{4}{3}ml^2\ddot{\theta} \tag{16} \]

将式(14)(15)(16)代入式(13),并且使用近似\(\cos \theta=1 , \sin \theta=\theta\),因此有

\[ m\ddot{z}l+\frac{4}{3}ml^2\ddot{\theta}-mgl\theta=0 \tag{17} \]

3.6. 系统状态空间表达

选取系统状态变量\(X\)

\[ \begin{aligned} X= \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix}~~~ Y= \begin{bmatrix} z \\ \theta \end{bmatrix} \end{aligned} \]

如果令输入\(u=\ddot{z}\),那么,根据式(17),状态方程可以写成

\[ \begin{aligned} \dot{X}= \begin{bmatrix} \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{3g}{4l} & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ 0 \\ \frac{-3}{4l} \end{bmatrix} u \end{aligned} \tag{18} \]

\[ \begin{aligned} Y= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \end{bmatrix} u \end{aligned} \tag{19} \]

3.7. 能观性能控性分析

已知系统状态方程:

\[ \begin{aligned} \dot{X}= \begin{bmatrix} \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{3g}{4l} & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ 0 \\ \frac{-3}{4l} \end{bmatrix} u \end{aligned} \]

\[ \begin{aligned} Y= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \end{bmatrix} u \end{aligned} \]

3.7.1. 构造能观性判别矩阵

\[ \begin{aligned} \begin{bmatrix} C \\ CA \\ CA^2 \\ CA^3 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ - & - & - & - \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ - & - & - & - \\ 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{3g}{4l} & 0 \\ - & - & - & - \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{3g}{4l} \\ \end{bmatrix} \end{aligned} \]

可得,(实际上,仅需要算到CA的时候,就可以判断rank=4了)

\[ \begin{aligned} rank \begin{bmatrix} C \\ CA \\ CA^2 \\ CA^3 \end{bmatrix}=4 \end{aligned} \]

因此,系统完全能观

3.7.2. 构造能控性判别矩阵

\[ \begin{aligned} \begin{bmatrix} B & AB & A^2b & A^3B \end{bmatrix} = \begin{bmatrix} 0 & 1-\frac{3}{4l} & 0 & \frac{-9g}{16l^2} \\ 1 & 0 & 0 & 0 \\ 0 & -\frac{3}{4l} & 0 & \frac{-9g}{16l^2} \\ -\frac{3}{4l} & 0 & \frac{-9g}{16l^2} & 0 \end{bmatrix} \end{aligned} \]

\[ \begin{aligned} rank \begin{bmatrix} B & AB & A^2b & A^3B \end{bmatrix}=4 \end{aligned} \]

因此,系统完全能控

3.8. 稳定性分析

该一级倒立摆系统的状态空间方程为

\[ \left \{ \begin{aligned} \dot{x}(t)=Ax(t)+Bu(t) \\ y(t)=Cx(t)+Du(t) \end{aligned} \right . \]

可知,原系统是非线性系统,在平衡点处进行线性化,得到

\[ \begin{aligned} \dot{X}= \begin{bmatrix} \dot{z} \\ \ddot{z} \\ \dot{\theta} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{3g}{4l} & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \\ 0 \\ \frac{-3}{4l} \end{bmatrix} u \end{aligned} \]

\[ \begin{aligned} Y= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} z \\ \dot{z} \\ \theta \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \end{bmatrix} u \end{aligned} \]

取以下参数

  • \(g=9.8m/s^2\)
  • \(l=0.25m\)

可以得到系统矩阵A如下

\[ \begin{aligned} A= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 29.4 & 0 \end{bmatrix} \end{aligned} \]

利用特征方程\(det(\lambda I-A)=0\),可以计算出矩阵A的特征值:

\[ \lambda_{1,2}=0, \lambda_{3,4}=\pm 5.42 \]

因此,可以看到,系统有两个为0的特征根,另外两个特征根一个在复平面左侧,另外一个具有正的实部,根据Lyapunov第一法稳定判据,可以判断,原系统不稳定。