【基于Simulink+UG NX MCD 一级倒立摆控制系统仿真】建模和分析(一)

前言

倒立摆是比较典型的系统，可以看出火箭发射的简化模型，国内外学者常常通过在倒立摆上开发和测试控制算法。

对倒立摆的控制分为两大任务：

• 起摆
• 稳摆

所以本文想通过此项目对自动控制原理进行一个复习与学习的过程，具体目标设想建立单摆模型，通过simulink进行仿真，开发相关算法，同时，利用UG NX MCD建立单摆机电模型，可以进行更为直观的仿真，但利用MCD进行控制算法的开发还未看到有相关研究。

视频资料：【倒立摆合集】一、二、三阶倒立摆

建立数学模型

Step 0： 问题描述

如图所示，倒立摆一端通过关节连接水平移动的小车上，小车由电机轮驱动并且只能在$x$方向移动，电机水平驱动力为$F$，小车质量为$M$，摆杆质量为$m$，摆杆长度为$2l$，要研究怎样的外力$F$输入才能控制摆杆保持垂直。

Step 1： 采用牛顿力学定律进行建模

设小车移动记录为$x$，摆杆偏离垂直方向角度为$\theta$，小车对摆杆垂直方向和水平方向的反作用力分别为$P$和$N$

对于小车，根据牛顿定律可以写出水平方向的力平衡方程：
$$M\ddot{x}=F-B_p\dot{x}-N$$

对于摆杆，根据牛顿定律可以写出水平方向和垂直方向的力平衡方程：
$$N=m\frac{d^2{x}_1}{d{t}^2}=\frac{d^2}{d{t}^2}(x+lsin\theta )=m(\dot{x}+\dot{\theta }lcos\theta {)}^{\prime }$$$$=m\ddot{x}+ml\ddot{\theta }cos\theta -ml\dot{{\theta }^2}sin\theta$$

$$P-mg=\frac{d^2{y}_1}{d{t}^2}=\frac{d^2}{d{t}^2}(lcos\theta )=m(-\dot{\theta }lsin\theta {)}^{\prime }$$$$=-ml\ddot{\theta }sin\theta -ml\dot{\theta }cos\theta$$
其中：$x_1$为摆杆质心水平移动距离，$y_1$为垂直移动距离。

摆杆对质心转动惯量为$I$，则摆杆质心的力矩方程为：
$$I\ddot{\theta }=Plsin\theta -Nlcos\theta$$
联立上述四式，消去变量$H$和$N$，得:
$$I\ddot{\theta }=mglsin\theta -m{l}^2\ddot{\theta }-ml\ddot{x}cos\theta$$

$$M\ddot{x}=F-B_p\dot{x}-m(\ddot{x}+l\ddot{\theta }cos\theta -l\dot{{\theta }^2}sin\theta )$$

为使得倒单摆保持垂直状态，假定$\theta$很小，则有$cos\theta \approx 1，sin\theta =0，{\dot{\theta }}^2=0$，因此上面两式可化简为：
$$(I-m{l}^2)\ddot{\theta }+ml\ddot{x}=mgl\theta$$$$(M+m)\ddot{x}+B_p\dot{x}+ml\ddot{\theta }=F$$

设摆杆质量分布均匀,则$I=\frac{m(2l{)}^2}{12}$，同时忽略摩擦，$B_p=0$，因此上面两式可化简为数学模型方程：
$$\frac{4}{3}m{l}^2\ddot{\theta }+ml\ddot{x}=mgl\theta$$$$(M+m)\ddot{x}+ml\ddot{\theta }=F$$

Step 2： 传递函数

对上面两式做拉式变换，并设初始状态为$x(0)=0，\theta (0)=0$，得：
$$\frac{4}{3}m{l}^2\Theta (s)s^2+mlX(s)s^2=mgl\Theta (s)$$$$(M+m)X(s)s^2+ml\Theta (s)s^2=F(s)$$

设定系统输出用$\theta$角表示，则联立上面两式，意消去$X(s)$，得
$$X(s)=(\frac{g}{s^2}-\frac{4l}{3})\Theta (s)$$$$(M+m)(\frac{g}{s^2}-\frac{4l}{3})\Theta (s)s^2+ml\Theta (s)s^2=F(s)$$
整理得传递函数：
$$\frac{\Theta (s)}{F(s)}=\frac{1}{[(M+m)(\frac{g}{s^2}-\frac{4l}{3})+ml]s^2}$$
$$=\frac{-\frac{3}{(4M+m)l}}{s^2-\frac{3(M+m)g}{(4M+m)l}}$$

使用matlab构造传递函数

取$M=0.1kg，m=0.5kg，l=0.5m，g=9.81$，使用matlab计算，则有

%% 清理
clear;close;clc;

%% 模型数据
M=1;%kg，小车质量
m=0.5;%kg,摆杆质量
l=0.5;%m,摆杆长
g=9.81;%% 传递函数

num = [-3/((4*M+m)*l)];  %分子多项式系数行向量
den = [1 0 -(3*(M+m)*g)/((4*M+m)*l)];   %分母多项式系数行向量
GS = tf(num,den); %建立传递函数模型

得到传递函数为：

---

关于状态空间模型

状态空间方法是现代控制中的内容，适合于多输入多输出系统，时变系统等，并且可以知道内部变量的变化，而传递函数模型是经典控制中的内容，只适用于单输入单输出的线性定常系统。

状态空间方程标准形式和方框图作法如下

Step 3： 状态空间模型

选取系统状态变量：

$x_1=\theta ，x_2=\dot{\theta }，x_3=x，x_4=\dot{x}$

根据上述数学模型方程，于是得到

$\dot{x_1}=x_2$

$\dot{x_2}=\frac{3(M+m)g}{(4M+m)l}{x}_1-\frac{3}{(4M+m)l}F$

$\dot{x_3}=x_4$

$\dot{x_4}=-\frac{3mg}{4M+m}{x}_1+\frac{4}{4M+m}F$

则状态方程为：
$$\left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\\ \dot{x_3}\\ \dot{x_4}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ \frac{3(M+m)g}{(4M+m)l}& 0& 0& 0\\ 0& 0& 0& 1\\ -\frac{3mg}{4M+m}& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{3}{(4M+m)l}\\ 0\\ \frac{4}{4M+m}\end{array}\right]F$$

输出变量选择为

$y_1=x$

$y_2=\theta$

则输出方程为
$$y=\left[\begin{array}{c}x\\ \theta \end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& 0\\ 1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]$$

有：

$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ \frac{3(M+m)}{(4M+m)l}g& 0& 0& 0\\ 0& 0& 0& 1\\ -\frac{3mg}{4M+m}& 0& 0& 0\end{array}\right]$

$B=\left[\begin{array}{c}0\\ -\frac{3}{(4M+m)l}\\ 0\\ \frac{4}{4M+m}\end{array}\right]$

$C=\left[\begin{array}{cccc}0& 0& 1& 0\\ 1& 0& 0& 0\end{array}\right]$

$D=0$

传递矩阵

传递函数和状态空间方程之间可以进行转换。

由状态空间方程，根据公式，可以算得传递矩阵

可得
$$G(s)=\left[\begin{array}{cccc}0& 0& 1& 0\\ 1& 0& 0& 0\end{array}\right]{\left(\left[\begin{array}{cccc}s& 0& 0& 0\\ 0& s& 0& 0\\ 0& 0& s& 0\\ 0& 0& 0& s\end{array}\right]-\left[\begin{array}{cccc}0& 1& 0& 0\\ \frac{3(M+m)}{(4M+m)l}g& 0& 0& 0\\ 0& 0& 0& 1\\ -\frac{3mg}{4M+m}& 0& 0& 0\end{array}\right]\right)}^{-1}\left[\begin{array}{c}0\\ -\frac{3}{(4M+m)l}\\ 0\\ \frac{4}{4M+m}\end{array}\right]$$
使用matlab计算

%% 状态空间模型
syms M m l g;
A = [0 1 0 0 ;
    3*(M+m)/(4*M+m)/l*g 0 0 0;
    0 0 0 1;
    -3*m*g/(4*M+m) 0 0 0;]
B = [0;
   -3/(4*M+m)/l;
    0;
    4/(4*M+m);]
C = [0 0 1 0;
    1 0 0 0;];
D = 0;

syms s;
si = s*eye(4);
%det(si-A);%行列式
%inv(si-A)%逆矩阵
Gs_z = C * inv(si-A) * B+D %转递矩阵

结果为：

即小车位移和力的传递函数为第一行，摆杆摆角和驱动力的传递函数为第二行，我们关心摆角，则有
$$\frac{\Theta (s)}{F(s)}=\frac{-\frac{3}{(4M+m)l}}{s^2-\frac{3(M+m)g}{(4M+m)l}}$$

可见和通过拉氏变换得到的传递函数一样。

Step 4：模型分析

分析模型是否稳定，可不可控，以及能不能观测。

稳定性

a. 奈奎斯特判据
使用matlab做出开环传递函数的零极点图和nyquist图

%% 模型分析
figure() ;
subplot(121);
pzmap(GS)%零极点图
grid;

subplot(122);
nyquist(GS)%nyquist图 由开环传递函数判断闭环系统稳定性 是图解分析的方法
grid;

由经典控制理论奈奎斯特判据，稳定系统在$s$右半平面不能有闭环极点，公式为$Z=N+P=0$，从图中看到开环传递函数在$s$右半平面内的极点数为$P=1$，对$-1+j0$顺时针包围的次数$N=0$，所以$Z=1$，即闭环传递函数特征方程的零点在s右半平面的个数为1，因此系统不稳定。

b. 李雅普诺夫第一法（间接法）

参考文献：现代控制理论 6-1 概念 6-2 李雅普诺夫第一法(间接法)
由现代控制理论李雅普诺夫第一法，通过判断系统矩阵A的特征值来判断稳定性，令$|\lambda E-A|=0$，解得特征值$\lambda =[0,0,4.4294,-4.4294]$，有大于0的特征值，所以系统不稳定。matlab代码如下：

%李雅普诺夫第一法（间接法）
sys lm;%矩阵A特征值
da=det(lm*eye(4)-A) %A特征方程行列式
lm=solve(da)%求解da零点

%公式求解 和上面一样
eig(A) %公式求解A特征值

可控性

根据现代控制理论，完全可控的条件是$n\times n$维的可控性矩阵$[B,AB,...,A^{n-1}B]$的秩为$n$，秩就是矩阵的非零子式的最高阶次。
本系统中，可控性矩阵为
$$Q_c=[A，AB，A^{2}B，A^{3}B]$$

在Matlab中计算

%% 可控可观性分析
qc=[B A*B A^2*B A^3*B]
rank(qc)%求秩

所以说明系统是完全可控的。

可观性

根据现代控制理论，完全可观的条件为$n\times mn$维的可观测矩阵$[C,CA,...,C{A}^{n-1}{]}^T$的秩等于$n$，则本系统可观性矩阵为
$$\begin{gathered} G_c=\left[\begin{array}{c} \\ C\\ CA\\ C{A}^2\\ C{A}^3\end{array}\right] \end{gathered}$$
在Matlab中计算

gc=[C C*A C*A^2 C*A^3]';
rank(gc)%可观性矩阵求秩

所以说明系统是完全可观测的。

Step 5： Simulink模型

根据状态空间方程作图法建立模型

开环阶跃信号响应曲线

代码分享

%% 清理
clear;
close;
clc;
%% 模型数据
M=1;%kg，小车质量
m=0.5;%kg,摆杆质量
l=0.5;%m,摆杆长
g=9.81;

%% 传递函数模型
num = [-3/((4*M+m)*l)];  %分子多项式系数行向量
den = [1 0 -(3*(M+m)*g)/((4*M+m)*l)];   %分母多项式系数行向量
GS = tf(num,den); %建立传递函数模型

%% 状态空间模型
%syms M m l g; %算符号传递矩阵时用上
A = [0 1 0 0 ;
    3*(M+m)/(4*M+m)/l*g 0 0 0;
    0 0 0 1;
    -3*m*g/(4*M+m) 0 0 0;];
B = [0;
   -3/(4*M+m)/l;
    0;
    4/(4*M+m);];
C = [0 0 1 0;
    1 0 0 0;];
D = [0;0];

syms s;
si = s*eye(4);
%det(si-A);%行列式
%inv(si-A)%逆矩阵
Gs_z = C * inv(si-A) * B+D; %转递矩阵
%[num,den] = ss2tf(A,B,C,D,1)%也可使用此函数进行转换

GSS=ss(A,B,C,D)%由状态空间构造传递函数
%% 模型相关分析作图

sys1=feedback(GS,1);%闭环传递函数
figure()
rlocus(sys1)%根轨迹图 %由闭环传递函数特征方程的根随开环增益K从0-anf变化在S平面上
                    %的变化轨迹，优点在于不必求解特征方程
                    %全为复
%rlocus(A,B,C,D)%用户自定义K可以使用rlocus(sys1,K),rlocus(A,B,C,D,K)

figure();
bode(sys1);%伯德图
grid;

figure();
step(GS);%阶跃响应曲线
grid;
%% 稳定性分析
%nyquist判据方法
figure() ;
subplot(121);
pzmap(GS)%零极点图
grid;
subplot(122);
nyquist(GS)%nyquist图 由开环传递函数判断闭环系统稳定性 是图解分析的方法
grid;

%李雅普诺夫第一法（间接法）
syms lm;%矩阵A特征值
da=det(lm*eye(4)-A); %A特征方程行列式
lm=solve(da);%求解da零点

%公式求解 和上面一样
eigs(A) %公式求解A特征值 

%% 可控可观性分析
qc=[B A*B A^2*B A^3*B];
rank(qc)%可控性矩阵求秩

gc=[C C*A C*A^2 C*A^3]';
rank(gc)%可观性矩阵求秩;

%%

交流群

感兴趣同学可加机械&自动化攻城狮交流群（群号：1105076200）

分享一下控制理论结构图，来源知乎

参考文献

[1] 倒立摆状态反馈控制——分析、建模与仿真(matlab)
[2] 《现代控制工程》 第五版 卢伯英译
[3] 《Matlab/Simulink 机电系统建模与仿真》宋志安
[4]王强. 直线倒立摆的起摆和稳摆智能控制研究[D].天津理工大学,2013.