Matlab 仿真——单自由度倒立摆（1）系统建模

1. 受控对象与设计要求

  该例的系统包含一个装有一个倒立摆的小车。我们的控制目标是通过给小车作用一个力，使顶部的倒立摆不落下来。下图标示出了各个变量的含义。  
  对于这个例子，我们有以下参数：   (M) 小车质量 0.5 kg (m) 倒立摆质量 0.2 kg (b) 小车的摩擦系数 0.1 N/m/sec (l) 转动点到倒立摆质心的长度 0.3 m (I) 倒立摆的转动惯量 0.006 kg.m^2 (F) 小车受到的力 (x) 小车的位置坐标 (theta) 倒立摆与垂线的角度   我们会用不同的方法设计该系统的控制器：PID，根轨迹，频域分析，状态空间。由于PID，根轨迹以及频域分析法最适合SISO系统的分析研究，因此用这三种方法的时候我们不考虑小车的位置，仅仅考虑如何控制好倒立摆的角度。我们会设计一款控制器，使得小车收到一个冲击（1Nsec）时仍能保持倒立摆垂直，设计要求是5s内稳定倒立摆，且倒立摆角度与稳定状态下的角度相比不超过0.05 弧度。倒立摆的初始位置垂直于地面。因此，我们可以总结出我们的设计需求：  

1. θ的稳定时间 < 5s
2. |θ-θ0| < 0.05 radians

  而对于状态空间设计方法，我们得以能够处理多输出系统。因此，在用状态空间设计法的时候我们尝试同时控制倒立摆的角度以及小车的位置。以下是我们的设计需求：  

1. x 与 θ 的稳定时间 < 5s
2. x 的上升时间 < 0.5s
3. |θ-θ0| < 0.05 radians （也就是20°）
4. 对于x和θ来说，稳态误差 < 2%

2. 力分析与系统方程

  下图是该系统的受力分析图：
  水平方向对小车进行受力分析得到（1）式：  
  你也可以在竖直方向对小车进行受力分析，但没什么卵用。接着在水平方向对倒立摆进行受力分析，得到（2）式 (这一步推导有点快，请参考这个文档)：  
  接着把（2）式代入到（1）式得到该系统的第一个动力学公式：   第一个动力学方程：  
  为了得到第二个动力学公式，我们在垂直于倒立摆的方向上列出力平衡方程：  
  左边的三项不难理解，右边的第一项为引起转动加速度的力，第二项为引起平动加速度的力。为了消去P和N，我们再列出关于倒立摆重心的转动惯量方程：  
  稍微整合一下上述两个方程得到我们要用的第二个动力学方程：   第二个动力学方程：  
  上述两个动力学方程是非线性的，为了采用线性系统的方法去设计控制器，还需要把上述两个动力学方程线性化一下。因此我们假设倒立摆的平衡位置 θ  = π ，且倒立摆只在平衡位置下小范围内摆动。我们用$\varphi$ 代表倒立摆与平衡位置的角度偏差，那么倒立摆在任意时刻的位置 θ = π  + ϕ 。有了上述线性化假设之后，我们可以得到：  

  将上述结果带入到我们的动力学方程组中得到：   第一个动力学方程：  
  第二个动力学方程：  

2.1 转换方程

  对动力学方程组分别做拉氏变换  
  要得到输入$U(s)$与输出 $\Phi (s)$ 的关系,我们需要消掉$X(s)$,于是经过一系列的化简代入我们最终得到：  
  其中：  

2.2 状态空间

  有了线性化之后的动力学方程组，一顿化简得到：  
  该系统的输出有两个，一是小车的位置$x$ ，二是倒立摆的角度$\theta$ ,因此$y$有两项

3. Matlab表达

  现在我们就可以在Matlab里面表达出我们的系统了  

3.1 转换方程

<code class="prism language-csharp has-numbering">M <span class="token operator">=</span> <span class="token number">0.5</span><span class="token punctuation">;</span>
m <span class="token operator">=</span> <span class="token number">0.2</span><span class="token punctuation">;</span>
b <span class="token operator">=</span> <span class="token number">0.1</span><span class="token punctuation">;</span>
I <span class="token operator">=</span> <span class="token number">0.006</span><span class="token punctuation">;</span>
g <span class="token operator">=</span> <span class="token number">9.8</span><span class="token punctuation">;</span>
l <span class="token operator">=</span> <span class="token number">0.3</span><span class="token punctuation">;</span>
q <span class="token operator">=</span> <span class="token punctuation">(</span>M<span class="token operator">+</span>m<span class="token punctuation">)</span><span class="token operator">*</span><span class="token punctuation">(</span>I<span class="token operator">+</span>m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token operator">-</span><span class="token punctuation">(</span>m<span class="token operator">*</span>l<span class="token punctuation">)</span><span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">;</span>
s <span class="token operator">=</span> <span class="token function">tf</span><span class="token punctuation">(</span><span class="token string">'s'</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

P_cart <span class="token operator">=</span> <span class="token punctuation">(</span><span class="token punctuation">(</span><span class="token punctuation">(</span>I<span class="token operator">+</span>m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token operator">/</span>q<span class="token punctuation">)</span><span class="token operator">*</span>s<span class="token operator">^</span><span class="token number">2</span> <span class="token operator">-</span> <span class="token punctuation">(</span>m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token operator">/</span>q<span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token operator">/</span><span class="token punctuation">(</span>s<span class="token operator">^</span><span class="token number">4</span> <span class="token operator">+</span> <span class="token punctuation">(</span>b<span class="token operator">*</span><span class="token punctuation">(</span>I <span class="token operator">+</span> m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token operator">*</span>s<span class="token operator">^</span><span class="token number">3</span><span class="token operator">/</span>q <span class="token operator">-</span> <span class="token punctuation">(</span><span class="token punctuation">(</span>M <span class="token operator">+</span> m<span class="token punctuation">)</span><span class="token operator">*</span>m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token punctuation">)</span><span class="token operator">*</span>s<span class="token operator">^</span><span class="token number">2</span><span class="token operator">/</span>q <span class="token operator">-</span> b<span class="token operator">*</span>m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token operator">*</span>s<span class="token operator">/</span>q<span class="token punctuation">)</span><span class="token punctuation">;</span>

P_pend <span class="token operator">=</span> <span class="token punctuation">(</span>m<span class="token operator">*</span>l<span class="token operator">*</span>s<span class="token operator">/</span>q<span class="token punctuation">)</span><span class="token operator">/</span><span class="token punctuation">(</span>s<span class="token operator">^</span><span class="token number">3</span> <span class="token operator">+</span> <span class="token punctuation">(</span>b<span class="token operator">*</span><span class="token punctuation">(</span>I <span class="token operator">+</span> m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token operator">*</span>s<span class="token operator">^</span><span class="token number">2</span><span class="token operator">/</span>q <span class="token operator">-</span> <span class="token punctuation">(</span><span class="token punctuation">(</span>M <span class="token operator">+</span> m<span class="token punctuation">)</span><span class="token operator">*</span>m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token punctuation">)</span><span class="token operator">*</span>s<span class="token operator">/</span>q <span class="token operator">-</span> b<span class="token operator">*</span>m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token operator">/</span>q<span class="token punctuation">)</span><span class="token punctuation">;</span>

sys_tf <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token class-name">P_cart</span> <span class="token punctuation">;</span> P_pend<span class="token punctuation">]</span><span class="token punctuation">;</span>

inputs <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">'u'</span><span class="token punctuation">}</span><span class="token punctuation">;</span>
outputs <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">'x'</span><span class="token punctuation">;</span> <span class="token string">'phi'</span><span class="token punctuation">}</span><span class="token punctuation">;</span>

<span class="token keyword">set</span><span class="token punctuation">(</span>sys_tf<span class="token punctuation">,</span><span class="token string">'InputName'</span><span class="token punctuation">,</span>inputs<span class="token punctuation">)</span>
<span class="token keyword">set</span><span class="token punctuation">(</span>sys_tf<span class="token punctuation">,</span><span class="token string">'OutputName'</span><span class="token punctuation">,</span>outputs<span class="token punctuation">)</span>

sys_tf
</code>

  输出：  

3.2 状态空间

<code class="prism language-csharp has-numbering">M <span class="token operator">=</span> <span class="token number">.5</span><span class="token punctuation">;</span>
m <span class="token operator">=</span> <span class="token number">0.2</span><span class="token punctuation">;</span>
b <span class="token operator">=</span> <span class="token number">0.1</span><span class="token punctuation">;</span>
I <span class="token operator">=</span> <span class="token number">0.006</span><span class="token punctuation">;</span>
g <span class="token operator">=</span> <span class="token number">9.8</span><span class="token punctuation">;</span>
l <span class="token operator">=</span> <span class="token number">0.3</span><span class="token punctuation">;</span>

p <span class="token operator">=</span> I<span class="token operator">*</span><span class="token punctuation">(</span>M<span class="token operator">+</span>m<span class="token punctuation">)</span><span class="token operator">+</span>M<span class="token operator">*</span>m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">;</span> <span class="token operator">%</span>denominator <span class="token keyword">for</span> the <span class="token class-name">A</span> and <span class="token class-name">B</span> matrices

A <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">0</span>      <span class="token number">1</span>              <span class="token number">0</span>           <span class="token number">0</span><span class="token punctuation">;</span>
     <span class="token number">0</span> <span class="token operator">-</span><span class="token punctuation">(</span>I<span class="token operator">+</span>m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token operator">*</span>b<span class="token operator">/</span>p  <span class="token punctuation">(</span>m<span class="token operator">^</span><span class="token number">2</span><span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token operator">/</span>p   <span class="token number">0</span><span class="token punctuation">;</span>
     <span class="token number">0</span>      <span class="token number">0</span>              <span class="token number">0</span>           <span class="token number">1</span><span class="token punctuation">;</span>
     <span class="token number">0</span> <span class="token operator">-</span><span class="token punctuation">(</span>m<span class="token operator">*</span>l<span class="token operator">*</span>b<span class="token punctuation">)</span><span class="token operator">/</span>p       m<span class="token operator">*</span>g<span class="token operator">*</span>l<span class="token operator">*</span><span class="token punctuation">(</span>M<span class="token operator">+</span>m<span class="token punctuation">)</span><span class="token operator">/</span>p  <span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">;</span>
B <span class="token operator">=</span> <span class="token punctuation">[</span>     <span class="token number">0</span><span class="token punctuation">;</span>
     <span class="token punctuation">(</span>I<span class="token operator">+</span>m<span class="token operator">*</span>l<span class="token operator">^</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token operator">/</span>p<span class="token punctuation">;</span>
          <span class="token number">0</span><span class="token punctuation">;</span>
        m<span class="token operator">*</span>l<span class="token operator">/</span>p<span class="token punctuation">]</span><span class="token punctuation">;</span>
C <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">1</span> <span class="token number">0</span> <span class="token number">0</span> <span class="token number">0</span><span class="token punctuation">;</span>
     <span class="token number">0</span> <span class="token number">0</span> <span class="token number">1</span> <span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">;</span>
D <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">;</span>
     <span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">;</span>

states <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">'x'</span> <span class="token string">'x_dot'</span> <span class="token string">'phi'</span> <span class="token string">'phi_dot'</span><span class="token punctuation">}</span><span class="token punctuation">;</span>
inputs <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">'u'</span><span class="token punctuation">}</span><span class="token punctuation">;</span>
outputs <span class="token operator">=</span> <span class="token punctuation">{</span><span class="token string">'x'</span><span class="token punctuation">;</span> <span class="token string">'phi'</span><span class="token punctuation">}</span><span class="token punctuation">;</span>

sys_ss <span class="token operator">=</span> <span class="token function">ss</span><span class="token punctuation">(</span>A<span class="token punctuation">,</span>B<span class="token punctuation">,</span>C<span class="token punctuation">,</span>D<span class="token punctuation">,</span><span class="token string">'statename'</span><span class="token punctuation">,</span>states<span class="token punctuation">,</span><span class="token string">'inputname'</span><span class="token punctuation">,</span>inputs<span class="token punctuation">,</span><span class="token string">'outputname'</span><span class="token punctuation">,</span>outputs<span class="token punctuation">)</span>
</code>

  输出  
我们可以通过把状态空间表达转换为转换方程：  

<code class="prism language-csharp has-numbering">sys_tf <span class="token operator">=</span> <span class="token function">tf</span><span class="token punctuation">(</span>sys_ss<span class="token punctuation">)
</span>
</code>

  输出  
  我们发现上述转换方程里面有系数非常接近于零，这是由于Matlab在四舍五入的时候带来的小误差。我们完全可以手动把这些系数改为0。完了之后我们这里转换得到的转换方程和之前我们直接得到转换方程表征的应该是完全一样的系统  

4. 引用

https://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=SystemModeling