线性系统大作业——2.二阶倒立摆建模与控制系统设计（上）

Cc1924

已于 2022-04-16 11:10:30 修改

阅读量1.3w

点赞数 36

分类专栏：作业记录文章标签：几何学线性代数深度学习

于 2022-01-12 23:52:54 首次发布

本文链接：https://blog.csdn.net/qq_42731705/article/details/122464894

版权

作业记录专栏收录该内容

4 篇文章 23 订阅

订阅专栏

文章目录

0.简介
1.建立数学模型
- 1.1.牛顿运动定律分析
- - 欧拉-拉格朗日方程分析
2.Simulink仿真
3.使用SimMechancis仿真
4.在平衡点附近模型线性化
5.系统能控性、能观性和稳定性分析
6.基于极点配置方法的控制器设计

0.简介

本文是《线性系统理论》大作业的一部分，内容是一阶和二阶倒立摆的分析与控制，本文是线性系统大作业——0.一阶和二阶倒立摆建模与控制系统设计的一部分。

另外，由于本文字数过多，超过了CSDN单篇文章的字数限制，因此将本文分成了上下两部分。下半部分见：线性系统大作业——2.二阶倒立摆建模与控制系统设计（下）

最后，本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
Code: https://github.com/Cc19245/Inverted-pendulum.git

1.建立数学模型

二阶倒立摆物理模型
给定二阶倒立摆的物理模型如图所示，给出系统的参数如表所示，假设两杆的质量都是均匀分布的，并且不考虑系统的摩擦。

物理量	数值
小车质量( $M$ )	$2 k g$
下杆质量( $m_1$ )	$0.5 k g$
上杆质量( $m_2$ )	$0.5 k g$
下杆长度( $L$ )	$0.4 m$
上杆长度( $L$ )	$0.4 m$

1.1.牛顿运动定律分析

对整个系统、下摆杆和上摆杆进行受力分析分别如图所示。

系统整体受力

下摆杆的受力

上摆杆的受力

上摆杆和下摆杆的质心坐标为 $\begin{aligned} \begin{array}{cccc} &x_{1 g}=x+l_{1} \sin \theta_{1} \\ &y_{1 g}=l_{1} \cos \theta_{1} \\ &x_{2 g}=x+L \sin \theta_{1}+l_{2} \sin \theta_{2} \\ &y_{2 g}=L \cos \theta_{1}+l_{2} \cos \theta_{2} \end{array} \end{aligned}$

首先对系统进行整体的受力分析，在水平方向有 $\begin{aligned} F =M\ddot{x}+m_{1} \frac{\partial^{2}\left(x_{1 g}\right)}{\partial t^{2}}+m_{2} \frac{\partial ^{2}\left(x_{2 g}\right)}{\partial t^{2}} \end{aligned}$

对下摆杆在惯性系下受力分析，则下摆杆还受到自身加速度引起的惯性力，由动量矩定理，有
$\begin{aligned} &f_{2 y}^{\prime} L \sin \theta_{1}+m_{1} g l_{1} \sin \theta_{1}-f_{2 x}^{\prime} L \cos \theta_{1}-m_{1} \ddot{x}_{1 g} l_1 \cos \theta_{1}+m_{1} \ddot{y}_{1 g} l_1 \sin \theta_{1}=I_{1} \ddot{\theta}_{1} \end{aligned}$

同理对上摆杆，有 $\begin{aligned} m_{2} g l_{2} \sin \theta_{2}-m_{2} l_{2} \cos \theta_{2} \ddot{x}_{2 g}+m_{2} l_{2} \sin \theta_{2} \ddot{y}_{2 g}=I_{2} \ddot{\theta}_{2} \end{aligned}$

对上摆杆，在水平方向和竖直方向进行受力分析，由牛顿运动定律可得
$\begin{aligned} \begin{array}{c} f_{2 x}=m_{2} \ddot{x}_{2 g}=m_{2}\left(x+L \sin \theta_{1}+l_{2} \sin \theta_{2}\right)^{''} \\ f_{2 y} - m_2g=m_{2} \ddot{y}_{2 g}=m_{2}\left(L \cos \theta_{1}+l_{2} \cos \theta_{2}\right)^{''} \end{array} \end{aligned}$

将式带入式可得 $\begin{aligned} \begin{array}{cc} &\left(M+m_{1}+m_{2}\right) \ddot{x}+\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \cdot \ddot{\theta}_{1}+m_{2} l_{2} \cos \theta_{2} \cdot \ddot{\theta}_{2} \\ &-\left(M_{1} l_{1}+M_{2} L\right) \sin \theta_{1} \cdot \dot{\theta}_{1}^{2}-M_{2} l_{2} \sin \theta_{2} \cdot \dot{\theta}_{2}^{2}=F \end{array} \end{aligned}$

将式和式带入式，可得 $\begin{aligned} \begin{array}{cc} &\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \cdot \ddot{x}+\left(I_{1}+m_{1} l_{1}^{2}+m_{2} L^{2}\right) \ddot{\theta}_{1}+m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \cdot \ddot{\theta}_{2} \\ &-m_{2} L l_{2} \sin \left(\theta_{2}-\theta_{1}\right) \cdot \dot{\theta}_{2}^{2}=\left(m_{1} l_{1}+m_{2} L\right) g \sin \theta_{1} \end{array} \end{aligned}$

将式和式带入式，可得 $\begin{aligned} \begin{array}{cc} &m_{2} l_{2} \cos \theta_{2} \cdot \ddot{x}+m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \cdot \ddot{\theta}_{1}+\left(I_{2}+m_{2} l_{2}^{2}\right) \ddot{\theta}_{2} \\ &+m_{2} L l_{2} \sin \left(\theta_{2}-\theta_{1}\right) \cdot \dot{\theta}_{1}^{2}=m_{2} g l_{2} \sin \theta_{2} \end{array} \end{aligned}$

则由上述三式组成的方程组即为二阶倒立摆的动力学方程。

欧拉-拉格朗日方程分析

选择小车位移 $x$ 和两个摆杆的角度 $\theta_1$ 和 $\theta_2$ 作为广义坐标，小车推理 $F$ 作为广义力，则有
$\begin{aligned} \begin{aligned} T_{M} &=\frac{1}{2} M \dot{r}^{2} \\ T_{m_1} &=\frac{1}{2} I_{1} \dot{\theta}_{1}^{2}+\frac{1}{2} m_{1} \times\left\{\left[\frac{d}{d t}\left(r+l_{1} \sin \theta_{1}\right)\right]^{2}+\left[\frac{d}{d t}\left(l_{1} \cos \theta_{1}\right)\right]^{2}\right\} \\ &=\frac{1}{2} I_{1} \dot{\theta}_{1}^{2}+\frac{1}{2} m_{1} \times\left[\left(\dot{r}+l_{1} \cos \theta_{1} \cdot \dot{\theta}_{1}\right)^{2}+\left(l_{1} \sin \theta_{1} \cdot \dot{\theta}_{1}\right)^{2}\right] \\ T_{m_2} &=\frac{1}{2} I_{2} \dot{\theta}_{2}^{2}+\frac{1}{2} m_{2} \times\left\{\left[\frac{d}{d t}\left(r+L_{1} \sin \theta_{1}+l_{2} \sin \theta_{2}\right)\right]^{2}+\left[\frac{d}{d t}\left(L_{1} \cos \theta_{1}+l_{2} \cos \theta_{2}\right)\right]^{2}\right\} \\ &=\frac{1}{2} I_{2} \dot{\theta}_{2}^{2}+\frac{1}{2} m_{2} \times\left[\left(\dot{r}+L_{1} \cos \theta_{1} \cdot \dot{\theta}_{1}+l_{2} \cos \theta_{2} \cdot \dot{\theta}_{2}\right)^{2}+\left(L_{1} \sin \theta_{1} \cdot \dot{\theta}_{1}+l_{2} \sin \theta_{2} \cdot \dot{\theta}_{2}\right)^{2}\right] \\ V_{M} &=0 \\ V_{m_1} &=m_{1} g l_{1} \cos \theta_{1} \\ V_{m_2} &=m_{2} g \times\left(L_{1} \cos \theta_{1}+l_{2} \cos \theta_{2}\right) \end{aligned}\end{aligned}$

将上述变量带入欧拉-拉格朗日方程，化简可得系统数学模型为
$\begin{aligned} \begin{array}{l} {\left[\begin{array}{lll} M_{11} & M_{12} & M_{13} \\ M_{21} & M_{22} & M_{23} \\ M_{31} & M_{32} & M_{33} \end{array}\right]\left[\begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array}\right]+} {\left[\begin{array}{lll} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{array}\right]\left[\begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array}\right]+\left[\begin{array}{c} G_{1} \\ G_{2} \\ G_{3} \end{array}\right]=\left[\begin{array}{c} u \\ 0 \\ 0 \end{array}\right] } \end{array}\end{aligned}$

其中， $\begin{aligned} \begin{array}{l} M_{11}=M+m_{1}+m_{2} \\ M_{12}=\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \\ M_{13}=m_{2} l_{2} \cos \theta_{2} \\ M_{21}=M_{12} \\ M_{22}= I_1 + m_{1} l_{1}^{2} + m_{2} L^{2} \\ M_{23}=m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \\ M_{31}=M_{13} \\ M_{32}=M_{23} \\ M_{33} = I_{2} + m_{2} l_{2}^{2} \\ C_{11}=0, C_{12}=-\left(m_{1} l_{1}+m_{2} L\right) \dot{\theta}_{1} \sin \theta_{1}, C_{13}=-m_{2} l_{2} \dot{\theta}_{2} \sin \theta_{2}\\ C_{21}=0, C_{22}=0, C_{23}=-m_{2} L l_{2} \dot{\theta}_{2} \sin \left(\theta_{2}-\theta_{1}\right) \\ C_{31}=0, C_{32}=m_{2} L l_{2} \dot{\theta}_{1} \sin \left(\theta_{2}-\theta_{1}\right), C_{33}=0 \\ G_{1}=0, G_{2}=-\left(m_{1} l_{1}+m_{2} L\right) g \sin \theta_{1}, G_{3}=-m_{2} g l_{2} \sin \theta_{2} \\ u=F \end{array} \end{aligned}$

由此可见，使用欧拉-拉格朗日方程建立的系统动力学方程和使用牛顿运动定律建立的动力学方程是完全相同的，因此初步验证了所建立的动力学方程的正确性。

2.Simulink仿真

由二阶倒立摆系统的动力学方程可以发现，系统存在非常严重的耦合现象，因此直接使用Simulink搭建仿真模型较为麻烦，因此这里使用S-Fuction来建立系统的仿真模型。编写MATLAB代码实现如下：

% 二阶倒立摆系统的动力学方程s-function建模
function [sys,x0,str,ts,simStateCompliance] =
order2_sfun(t,x,u,flag, x_0, theta1_0, theta2_0)
switch flag,
    case 0,
        [sys,x0,str,ts,simStateCompliance]=
        mdlInitializeSizes(t,x,u, x_0, theta1_0, theta2_0);
    case 1,
        sys=mdlDerivatives(t,x,u);
    case 2,
        sys=mdlUpdate(t,x,u);
    case 3,
        sys=mdlOutputs(t,x,u);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,x,u);
    case 9,
        sys=mdlTerminate(t,x,u);
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag', 
        num2str(flag));
end
% 主函数结束

% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u, x_0, theta1_0, theta2_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构
sizes.NumContStates  = 6;% 连续状态数, 分别是x', theta1', theta2', x, theta1, theta2
sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
sizes.NumOutputs     = 6;% 输出量个数,缺省为 0, 
sizes.NumInputs      = 1;% 输入量个数，缺省为 0
sizes.DirFeedthrough = 1;%是否存在直接馈通。1：存在；0：不存在，缺省为 1 
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);
x0  = [0; 0; 0; x_0; theta1_0; theta2_0];% 设置初始状态
str = [];% 保留变量置空
ts  = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes

% ---------------------------------------------
function sys=mdlDerivatives(t,x,u)
%  计算导数例程子函数
M=2; m1=0.5; m2=0.5; l1=0.2; l2=0.2; L=0.4; g=9.8
I1 = 1/12*m1*(2*l1)^2; I2 = 1/12*m2*(2*l2)^2;
dx_ = x(1); dth1_ = x(2); dth2_ = x(3);
x_ = x(4); th1_ = x(5); th2_ = x(6); 
M11 = M + m1 + m2;
M12 = (m1*l1+m2*L)*cos(th1_);
M13 = m2*l2*cos(th2_);
M21 = M12;
M22 = I1 + m1*l1^2 + m2*L^2;
M23 = m2*L*l2*cos(th2_ - th1_);
M31 = M13;
M32 = M23;
M33 = I2 + m2*l2^2;
C11 = 0; C12 = -(m1*l1+m2*L)*sin(th1_)*dth1_;
C13 = -m2*l2*sin(th2_)*dth2_;
C21 = 0; C22 = 0; C23 = -m2*L*l2*dth2_*sin(th2_ - th1_);
C31 = 0; C32 = m2*L*l2*dth1_*sin(th2_ - th1_); C33 = 0;
G1 = 0; G2 = -(m1*l1+m2*L)*g*sin(th1_); 
G3 = -m2*g*l2*sin(th2_);
A = [M11 M12 M13 C11 C12 C13;
     M21 M22 M23 C21 C22 C23;
     M31 M32 M33 C31 C32 C33;
      0   0   0   1   0   0 ;
      0   0   0   0   1   0 ;
      0   0   0   0   0   1 ];
B = [u-G1;
     -G2;
     -G3;
     dx_;
     dth1_;
     dth2_];
sys = A\B;

% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];

% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
sys=[x(1);x(2);x(3);x(4);x(5);x(6)];

% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
 % 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
 % 6. 仿真结束时要调用的例程函数
sys = [];

利用s-function在Simulink中搭建系统的仿真模型如图所示。

二阶倒立摆的Simulink仿真模型

由于二阶倒立摆很不稳定，所以为了看系统初始状态下存在小扰动时系统的动态响应，假设系统的初始状态偏离渐进稳定点，即 $x=0,\theta_1=\pi,\theta_2=\frac{5}{6}\pi$ ，且系统无输入，则此后小车位置 $x$ 和倒立摆的摆角 $\theta_1$ 、 $\theta_2$ 的变化如图所示。

使用Simulink建模的二阶倒立摆初始响应

3.使用SimMechancis仿真

如图所示，是使用SimMechanicalcs建立二阶倒立摆的物理模型。其中增益模块的增益值-1是由于SimMechanicalcs中的 $\theta$ 方向和上面推导的方向相反，另外值得注意的是，在SimMechanicalcs中的 $\theta_2$ 角度定义的参考系和上面的推导也不相同，它的角度是以下杆的方向为参考进行角度定义，也就是下杆和上杆之间的角度。因此为了和上面的公式推导结果相对比，在接入示波器之前将 $\theta_1$ 和 $\theta_2$ 加起来。如图所示，是使用SimMechanicalcs的仿真结果。

二阶倒立摆SimMechancis模型

二阶倒立摆SimMechancis模型的动态响应

由图可见这个结果和上面动力学建模并使用Simulink仿真的结果完全相同，从而证明了之前动力学建模的正确性。

4.在平衡点附近模型线性化

系统在平衡点附近时， $\theta_1$ 和 $\theta_2$ 都很小，并且假设其角速度 $\dot{\theta_1}$ 和 $\dot{\theta_2}$ 也很小，则可进行近似处理： $\cos{\theta} \approx 1, \sin\theta \approx \theta, \sin \theta \dot{\theta} \approx0$ 。从而得到二阶倒立摆系统在平衡点附近的动力学方程为
$\begin{aligned} \begin{array}{c} &\left[ \begin{array}{ccc} M+m_1+m_2 & m_1l_1+m_2L & m_2l_2 \\ m_1l_1+m_2L & I_1+m_1l_1^2+m_2L^2 & m_2Ll_2 \\ m_2l_2 & m_2Ll_2 & I_2+m_2l_2^2 \end{array}\right]\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array}\right] &=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & (m_1l_1+m_2L)g & 0 \\ 0 & 0 & m_2gl_2 \end{array}\right]\left[ \begin{array}{c} x \\ \theta_{1} \\ \theta_{2} \end{array}\right]+\left[ \begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right]u \end{array}\end{aligned}$

使用MATLAB符号函数求逆的功能求解可得 $\begin{aligned} \begin{array}{c} {\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array} \right]= \left[ \begin{array}{ccc} a_{11} & a_{12} & a_{13}\\ a_{21} & a_{22} & a_{23}\\ a_{31} & a_{32} & a_{33}\\ \end{array} \right] \left[ \begin{array}{c} x \\ \theta_{1} \\ \theta_{2} \end{array} \right]+ \left[ \begin{array}{c} b_{1} \\ b_{2} \\ b_{3} \end{array} \right]u} \end{array}\end{aligned}$

其中， $\begin{aligned} \begin{array}{l} a_{11}=0 \\ a_{12}= -\frac{I_2gL^2m_2^2 + gLl_1l_2^2m_1m_2^2 + 2I_2gLl_1m_1m_2 + gl_1^2l_2^2m_1^2m_2 + I_2gl_1^2m_1^2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \\ a_{13}=-\frac{gm_1l_1^2l_2^2m_2^2 - Lgm_1l_1l_2^2m_2^2 + I_1gl_2^2m_2^2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{21}=0 \\ a_{22}=\frac{g(I_2Lm_2^2 + I_2l_1m_1^2 + LMl_2^2m_2^2 + I_2LMm_2 + Ll_2^2m_1m_2^2 + I_2Ml_1m_1 + I_2Lm_1m_2 + l_1l_2^2m_1^2m_2 + I_2l_1m_1m_2 + Ml_1l_2^2m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{23}=-\frac{gl_2^2m_2^2(Lm_1 - l_1m_1 + LM)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{31}=0 \\ a_{32}=-\frac{gl_2m_2(L^2Mm_2 - l_1^2m_1^2 + Ll_1m_1^2 + L^2m_1m_2 + LMl_1m_1 - Ll_1m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{33}=\frac{gl_2m_2(I_1m_1 + I_1m_2 + I_1M + l_1^2m_1m_2 + L^2Mm_2 + Ml_1^2m_1 + L^2m_1m_2 - 2Ll_1m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \\ b_{1}=\frac{I_2m_2L^2 + m_1m_2l_1^2l_2^2 + I_2m_1l_1^2 + I_1m_2l_2^2 + I_1I_2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ b_{2}=-\frac{ l_1m_1m_2l_2^2 + I_2Lm_2 + I_2l_1m_1}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ b_{3}=-\frac{l_2m_2(m_1l_1^2 - Lm_1l_1 + I_1)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \end{array} \end{aligned}$

定义系统的状态变量为 $(x_1,x_2,x_3,x_4,x_5,x_6)=(\dot{x},\dot{\theta_1}, \dot{\theta_2},x, \theta_1,\theta_2)$ ，系统的输入量为小车外力 $u$ ，系统输出为小车的位移 $x$ ，两个摆杆的角度 $\theta_1,\theta_2$ ，则可得二阶倒立摆系统的状态空间方程为
$\begin{aligned} \begin{array}{c} {\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \\ \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array} \right]= \left[ \begin{array}{cccccc} 0 & 0 & 0 & a_{11} & a_{12} & a_{13} \\ 0 & 0 & 0 & a_{21} & a_{22} & a_{23} \\ 0 & 0 & 0 & a_{31} & a_{32} & a_{33} \\ 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{array} \right] \left[ \begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ x \\ \theta_{1} \\ \theta_{2} \end{array} \right]+ \left[ \begin{array}{c} b_{1} \\ b_{2} \\ b_{3} \\ 0 \\ 0 \\ 0 \end{array} \right]u} \\ y=x=\left[\begin{array}{llllll} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ x \\ \theta_{1} \\ \theta_{2} \end{array}\right] \end{array}\end{aligned}$

其中， $a_{ij}$ 和 $b_{i}$ 的定义如式所示。则将系统的各变量参数代入，可得具体的系统状态空间方程为
$\begin{aligned} \begin{array}{c} {\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \\ \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array} \right]= \left[ \begin{array}{cccccc} 0 & 0 & 0 & 0 & -4.41 & 0.49 \\ 0 & 0 & 0 & 0 & 77.175 & -33.075 \\ 0 & 0 & 0 & 0 & -99.225 & 84.525\\ 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{array} \right] \left[ \begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ x \\ \theta_{1} \\ \theta_{2} \end{array} \right]+ \left[ \begin{array}{c} 0.4667 \\ -1.5 \\ 0.5 \\ 0 \\ 0 \\ 0 \end{array} \right]u} \\ y=x=\left[\begin{array}{llllll} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \\ x \\ \theta_{1} \\ \theta_{2} \end{array}\right] \end{array}\end{aligned}$

5.系统能控性、能观性和稳定性分析

5.1.能控性分析

对系统进行能控性分析，编写MATLAB代码如下：

M=2; m_1=0.5; m_2=0.5; l_1=0.2; l_2=0.2; L=0.4; g=9.8;
I_1 = 1/12*m_1*(2*l_1)^2; I_2 = 1/12*m_2*(2*l_2)^2;

M_11 = M+m_1+m_2; M_12 = m_1*l_1+m_2*L;  M_13 = m_2*l_2;
M_21 = M_12; M_22 = I_1+m_1*l_1*l_1+m_2*L*L;  M_23 = m_2*L*l_2;   
M_31 = M_13; M_32 = M_23; M_33 = I_2+m_2*l_2*l_2;
M = [M_11 M_12 M_13; M_21 M_22 M_23; M_31 M_32 M_33];
G = [0 0 0; 0 (m_1*l_1+m_2*L)*g 0; 0 0 m_2*g*l_2];
U = [1; 0; 0];
A_a = M\G;
B_b = M\U;

A = zeros(6,6);
B = zeros(6,1);
A(1:3, 4:end) = A_a;
A(4:end, 1:3) = eye(3);
B(1:3) = B_b;
C = [0 0 0 1 0 0;
     0 0 0 0 1 0;
     0 0 0 0 0 1];

S_c = [B A*B A^2*B A^3*B A^4*B A^5*B];
fprintf('rank(S_c) = %d\n', rank(S_c));

程序运行结果为： $\begin{aligned} rank(S_c)=6\end{aligned}$

因此系统是状态完全能控的。

5.2.能观性分析

对系统进行能控性分析，继续编写MATLAB代码判断系统能观性

Q_o = [C; C*A; C*A^2; C*A^3; C*A^4; C*A^5];
fprintf('rank(Q_o) = %d\n', rank(Q_o));

程序运行结果为： $\begin{aligned} rank(Q_o)=6\end{aligned}$

因此系统是能观的。

5.3.稳定性分析

5.3.1.Routh-Hurwitz判据

使用Matlab计算矩阵A的特征值：

disp('eig(A)=');
eig(A)

程序输出结果为 $\lambda = \left[0,0,-11.7582,-4.8420,4.8420,11.7582\right]$ ，存在两个复平面右半平面的特征根，因此系统是不稳定的。

Lyapunov函数

使用MATLAB进行Lyapunov方程的求解，程序如下：

P = lyap(A',eye(6))

程序运行报错为The solution of this Lyapunov equation does not exist or is not unique，因此系统不稳定。

6.基于极点配置方法的控制器设计

根据上述分析可知系统存在复平面右半平面的极点导致系统不稳定，因此需要设计一个状态反馈控制器 $u = - K x$ ，配置系统闭环极点全部到复平面左半平面。

假设希望将系统的闭环极点配置到 $\mu_1=-2+j*2, \mu_2=-2-j*2, \mu_3=-6,\mu_4=-7, \mu_5=-8,\mu_6=-9$ 。则可以通过调用MATLAB中的acker和place命令来方便地计算反馈矩阵K，代码如下：

% 调节器的极点配置问题
J = [-2+j*2 -2-j*2 -6 -7 -8 -9];
K1 = acker(A,B,J); 
K2 = place(A,B,J); 
disp('K1 = ');
disp(K1);
disp('K2 = ');
disp(K2);
K = K2;

程序输出结果为 $K 1 = K 2 = (23.4125, - 0.5709, 44.4357, 22.3907, - 283.0923, 379.2252)$ ，可见对于单输入系统来说使用acker和place命令得到的结果是一样的。

为了检验上述极点配置的闭环系统的性能，使用在平衡点线性化的系统配置的极点K对非线性系统进行极点配置，并在Simulink中搭建如图所示。

二阶倒立摆的状态反馈控制仿真模型

假设系统初始状态为 $\left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0,0,0,0,0,5^{\circ}\right]$ ，运行Simulink仿真模型，得到系统的闭环响应应如图所示，由图可见系统在微小扰动的情况下仍然能够保持稳定，说明所设计的状态反馈控制器是有效的。