基于MATLAB的单级倒立摆仿真
作者：互联网
有关代码及word文档请关注公众号“浮光倾云”，后台回复A010.02即可获取
一、单级倒立摆概述
倒立摆是处于倒置不稳定状态，人为控制使其处于动态平衡的一种摆，是一类典型的快速、多变量、非线性、强耦合、自然不稳定系统。由于在实际中存在很多类似的系统，因此对它的研究在理论上和方法上均有重要意义。
单级倒立摆系统（Simple Inverted Pendulum System）是由倒立摆和小车两部分组成。小车依靠直流电动机施加控制力，可以在导轨上左右移动，其控制目标是在有限长导轨上使倒立摆能够稳定竖立在小车上而不倒，达到动态平衡[1]
图1 倒立摆小车系统
Fig1. Inverted Pendulum on a Cart
图中F为施加于小车的水平方向的作用力， 是摆杆的倾斜角。若不给小车施加控制力，摆杆会左右倾斜，控制的过程即当摆杆出现偏角，在水平方向给小车以作用力，通过小车的水平运动，使摆杆保持在垂直位置，意即控制系统的状态参数，以保持倒立摆的倒立稳定。
倒立摆系统由6大部分组成，并构成一个闭环结构，包括计算机、数据采集卡、电源及功率放大器、直流伺服电机、倒立摆本体和两个光电编码器等模块，如图2所示。
图2 倒立摆系统结构组成示意图
Fig2 Structure of the Single Inverted Pendulum System
二、单级倒立摆数学模型
建立控制系统的数学模型有两种基本方法，其一，机理建模法，即对系统各部分的运动机理进行分析，根据它们所依据的物理规律或化学规律分别列写相应的运动方程，合在一起便成为描述整个系统的方程；其二，实验建模方法，即人为地给系统施加某种测试信号，记录其输出响应，并用适当的数学模型去逼近。主要用于系统运动机理复杂因而不便分析或不可能分析的情况[2]。
倒立摆的形状较为规则，而且是一个绝对不稳定系统，无法通过测量频率特性方法获取其数学模型，故适合用数学工具进行理论推导。此处可以用牛顿力学法和拉格朗日方程法两种方式进行推导。
2.1模型假设
为了建立倒立摆系统的数学模型，先作出如下假设：
（1） 倒立摆与摆杆均为匀质刚体；
（2） 各部分的摩擦力（力矩）与相对速度（角速度）成正比；
（3） 施加在小车上的驱动力与加在功率放大器上的输入电压成正比；
（4） 皮带轮与传送带之间无滑动，传送带无伸长状态；
（5） 信号与力的传递无延时；
（6） 忽略空气阻力。
2.2模型符号
（1）牛顿法模型符号
表1 牛顿力学法符号含义
|
符号 |
物理量 |
数值 |
|
M |
小车质量 |
0.5kg |
|
m |
摆杆质量 |
0.2kg |
|
l |
摆杆转动轴心到摆杆质心的长度 |
0.3m |
|
I |
摆杆绕质心的转动惯量 |
0.006 |
|
b |
小车滑动摩擦系数 |
0.1N/m/sec |
|
c |
摆杆转动摩擦系数 |
0.1 |
|
F |
加在小车上的力 |
—— |
|
x |
小车位移 |
—— |
|
Φ |
摆杆与法向夹角 |
—— |
（2）拉格朗日方程法模型符号
表2 拉格朗日方程法符号含义
|
符号 |
物理量 |
数值 |
|
M |
小车质量 |
0.5kg |
|
m |
摆杆质量 |
0.2kg |
|
l |
摆杆转动轴心到摆杆质心的长度 |
0.3m |
|
I |
摆杆绕质心的转动惯量 |
0.006 |
|
b |
小车滑动摩擦系数 |
0.1N/m/sec |
|
c |
摆杆转动摩擦系数 |
0.1 |
|
F |
加在小车上的力 |
—— |
|
x |
小车位移 |
—— |
|
摆杆与法线方向的夹角 |
—— |
|
|
q |
系统的广义坐标 |
—— |
|
T |
倒立摆系统的动能 |
—— |
|
D |
倒立摆系统的耗散能 |
—— |
|
V |
倒立摆系统的势能 |
—— |
|
T0 |
倒立摆系统中小车的动能 |
—— |
|
D0 |
倒立摆系统中小车的耗散能 |
—— |
|
V0 |
倒立摆系统中小车的势能 |
—— |
|
T1 |
倒立摆系统中摆杆的动能 |
—— |
|
D1 |
倒立摆系统中摆杆的耗散能 |
—— |
|
V1 |
倒立摆系统中摆杆的势能 |
—— |
2.3数学模型
2.3.1 Newton-Euler法
对系统中的摆杆和小车分别进行受力分析，如图3。其中，N和P为小车与摆杆相互作用力的水平和垂直方向的分量。 以顺时针方向为矢量方向，且角速度、角加速度都按同一方向为矢量方向。
图3 倒立摆模型受力分析
分析小车水平方向所受的力，可以得到以下方程：
(1)
由摆杆水平方向的受力进行分析，摆杆水平方向的合外力为N，则可以得到运动微分方程：
(2)
由式（2），得：
(3)
把式(3)代入式(1)中，得系统的第一个运动方程：
(4)
对摆杆垂直方向上的受力进行分析，可以得到下面方程：
(5)
即
(6)
以摆杆质心为基点，得摆杆的力矩平衡方程如下：
(7)
合并方程（3）、(6)和(7)，约去 P和N ，
得到系统的第二个运动方程：
（8）
进行近似处理，线性化后两个运动方程如下：
（9）
因为摆体绕支点的转动惯量I与摆体绕质心转动惯量 关系为
（10）
将式（20）代入到式（19）中，得到：
（11）
2.3.2 Newton力学法
对系统中的摆杆和小车分别进行受力分析，如图4。其中，N和P为小车与摆杆相互作用力的水平和垂直方向的分量。
图4 倒立摆模型受力分析
分析小车水平方向所受的力，可以得到以下方程：
(1)
由摆杆水平方向的受力进行分析，摆杆水平方向需要保持平衡，故合外力为0，则可以得到平衡方程：
(2)
式中,
则
最后得第一个运动微分方程：
(4)
对摆杆垂直方向上的受力进行分析，可以得到下面方程：
(5)
式中，
得:
(6)
以摆杆质心为基点，得摆杆的力矩平衡方程如下：
(7)
合并方程（3）、(6)和(7)，约去 和 ，
得到系统的第二个运动方程：
（8）
则可以进行近似处理，线性化后两个运动方程如下：
（9）
因为摆体绕支点的转动惯量I与摆体绕质心转动惯量 关系为
（10）
将式（20）代入到式（19）中，得到：
（11）
2.3.3 Lagrange方程法
Lagrange 方程是以能量观点建立的运动方程式，需要从两个方面分析，一个是表征系统运动的动力学量——系统的动能和势能，另一个是表征主动力作用的动力学量——广义力。
由n个关节部件组成的机械系统，其Lagrange方程应为：
（12）
其中，q为系统的广义坐标，表示系统中线位移和角度的变量；T为倒立摆系统的动能，V为倒立摆系统的势能，D为倒立摆系统中的耗散能。
由图4分析，可以得出，小车和摆杆的各部分能量表达式为：
图5单级倒立摆分析图
Fig 5. Simple Inverted Pendulum analysis chart
对于小车：
（13）
对于摆杆：
（14）
对于倒立摆系统，由式（13）和式（14），得：
， （15）
对小车而言，由式（15），得：
由式（12）则有：
（16）
而当 的时候，即对摆杆而言，由式（15），得：
由式（12）则有：
（17）
根据上面各式综合，可以得到单级倒立摆系统动力学方程为：
（18）
进行线性化处理，根据分析可知，在接近平面位置时
因此，将式（18）简化后得到动力学方程：
（19）
2.4模型分析
（1）微分方程
由上述分析，可以得到系统的微分方程为：
（11）
即
(20)
（2）传递函数
令初始条件为零，对于输入为作用力F，输出为摆杆角度时，对式（20）进行拉普拉斯变换，得到：
(21)
由于力F为输入，角度 为输出，求解以上方程组，得
（3）状态空间方程
由现代控制理论，控制系统的状态空间方程
令
则
故状态空间方程为：
则有
三、单级倒立摆开环系统分析
3.1稳定性分析
（1）系统模型
由上节分析，系统的传递函数为：
利用MATLAB工具，求系统传递函数，程序代码见附录1-CDHS.m，运行结果如下：
传递函数的多项式表达式为：
G =
0.06 s^2
-----------------------------------------------
0.0204 s^4 + 0.0724 s^3 - 0.4016 s^2 + 0.0588 s
由上节分析，可知系统的状态空间方程为：
则有
利用MATLAB工具，求系统状态方程，程序代码见附录1-ZTFC.m，运行结果如下：
系统状态方程各参数值为：
A =
x1 x2 x3 x4
x1 0 1 0 0
x2 0 -0.02837 0.417 -0.07092
x3 0 0 0 1
x4 0 -0.07092 25.54 -4.344
B =
u1
x1 0
x2 0.2837
x3 0
x4 0.7092
C =
x1 x2 x3 x4
y1 1 0 0 0
y2 0 0 0 0
y3 0 0 1 0
y4 0 0 0 0
D =
u1
y1 0
y2 0
y3 0
y4 0
（2）稳定性分析
由MATLAB代码分析系统的极点、并判断稳定性，代码见附录2-WDXFX.m：
系统极点：
0
-6.5986
2.8989
0.1507
有两个极点位于右半平面：系统不稳定
3.2开环响应分析
（2）脉冲响应
MATLAB仿真的开环脉冲响应（即给系统加一个脉冲推力）曲线如图6，程序代码见附录2-MCXY.m
图6 开环脉冲响应曲线
（3）阶跃响应
MATLAB仿真的开环阶跃响应曲线如图7，程序代码见附录2-JYXY.m，
图7 开环阶跃响应曲线
（5） Simulink仿真平台分析
利用MATLAB中的simulink仿真平台进行仿真，系统开环输入为作用力F，输出为摆杆角度，则将数值代入传递函数中，可得系统的传递函数值：
G =
0.06 s^2
-----------------------------------------------
0.0132 s^4 + 0.0724 s^3 - 0.4016 s^2 + 0.0588 s
在Simulink中搭建仿真平台，如图8所示：
图8 Simulink仿真平台
仿真运行，得到开环系统的阶跃响应（a）和脉冲响应（b）如图9：
（a）阶跃响应曲线 （b）脉冲响应曲线
图9 开环系统响应曲线
由开环响应得知，系统不稳定
四、单级倒立摆系统控制器设计及闭环系统分析
4.1 单级倒立摆系统的控制
倒立摆有多种控制方法，当前的控制方法可分为：
（1）线性理论控制方法
将倒立摆系统的非线性模型进行近似线性化处理，获得系统在平衡点附近的线性化模型，然后再利用各种线性系统控制器设计方法，如PID控制、状态反馈控制、LQR控制等，得到期望的控制器。
（2）非线性理论控制方法
由于线性控制理论与倒立摆系统多变量、非线性之间的矛盾，人们意识到针对多变量、非线性对象，采用具有非线性特性的多变量控制是解决多变量、非线性系统的必由之路。因此人们先后开展了倒立摆系统的非线性控制方法研究[3]。
4.2 PID控制器的设计
4.2.1 PID控制的基本原理
PID控制电路的主要原理是将偏差的比例（P）、积分（I）和微分（D）通过线性组合构成控制量，对被控对象进行控制。
图10 PID控制框图
PID控制器是一种线性控制器，它根据给定值与实际输出值构成控制偏差
将偏差的比例（P）、积分（I）、微分（D）通过线性组合构成控制量，对被控对象进行控制，故称为PID控制器，其控制规律为：
或写成传递函数的形式：
简单来说，PID控制器各校正环节作用如下[4]：
表3 各个控制环节的特点及作用
|
环节名称 |
特点及作用 |
|
比例控制（P） |
减少了系统稳态误差，提高了系统精度，但是降低了系统的相对稳定性。 |
|
积分控制（I） |
在系统前向通道加入积分环节，提高系统的类型，改善系统的稳定性能，但是，同样会降低系统的相对稳定性，对系统的动态分析不利。 |
|
比例+微分控制（PD） |
比例加微分控制引入了开环零点，从而增加了系统的相角裕度，提高了系统的相对稳定性，改善了动态性能。 |
|
比例+积分+微分（PID） |
在比例+积分+微分的控制中，系统被引入了两个零点，使系统的相角裕度大大提高，特别适用于大惯性环节。 |
4.2.2 参数设计
（1）系统分析
在开环系统中添加PID控制器构成闭环控制系统，系统框图如图11：
图11 PID控制系统框图
系统r=0，则可以将框图变换为图12，
图12 变换后的系统框图
构成闭环系统的传递函数为：
式中，赋值后得到传递函数的多项式表达式为：
G =
0.06 s^2
-----------------------------------------------
0.0204 s^4 + 0.0724 s^3 - 0.4016 s^2 + 0.0588 s
（2）参数选择
筛选合适的PID参数，利用MATLAB中的PID Tuner仿真，当：参数为表4时达到较好的性能，相应的性能指标见表5
表4 较佳参数值
表5 性能指标
（3）代码分析
PID控制器传递函数为：
KD =
3.007 s^2 + 23.16 s + 36.42
---------------------------
s
构成的闭环系统传递函数为：
Gx =
0.1804 s^4 + 1.39 s^3 + 2.185 s^2
------------------------------------------------
0.0204 s^5 + 0.2528 s^4 + 0.9881 s^3 + 2.244 s^2
4.2.3闭环系统分析
（1）稳定性分析
MATLAB代码见附录3-WDXFX.m，运行结果如下：
系统极点
0.0000 + 0.0000i
0.0000 + 0.0000i
-8.0836 + 0.0000i
-2.1543 + 2.9945i
-2.1543 - 2.9945i
系统稳定
（2）时域分析
用MATLAB代码（见附录3—XNFX.m）分析系统的阶跃响应，并求得性能指标如下：
终值C = 0.9737
峰值时间peak_time = 0.4558s
最大超调量max_overshoot = 51.1121%
上升时间rise_time = 0.1253s
调整时间settling_time = 1.9256s
阶跃响应曲线如图13
图13 阶跃响应曲线
（3）频域分析
由MATLAB代码分析频域特征，绘制频谱图，如图14所示，代码见附录3-PYFX.m
图14 频域伯德图
4.3 LQR控制器设计
现代控制理论的最突出特点就是将控制对象用状态空间表达式的形式表示出来，这样便于对多输入多输出系统进行分析和设计。线性二次型最优控制算法（LQR）的目的是在一定的性能指标下，使系统的控制效果最佳，即利用最少的控制能量，来达到最小的状态误差，以下将利用最优控制算法实现对一阶倒立摆系统的摆杆角度和小车位置的同时控制。
此前的输入是脉冲量，并且在设计控制器时，只对摆杆角度进行控制，而不考虑小车的位移。然而，对一个倒立摆系统来说，把它作为单输出系统是不符合实际的，如果把系统当作多输出系统的话，用状态空间法分析要相对简单一些。
4.3.1状态空间系统分析
阶跃响应曲线如图15，分别是小车速度、位移和标杆角速度、角加速度，与外力F阶跃输入后的响应，程序代码见附录4-ZTKJYXY.m
图15 开环阶跃响应曲线
可见，系统处于不稳定状态。
4.3.2最优控制器的设计
最优控制的前提条件是系统是能控的，下面来判断一下系统的能控能观性。
图16 状态反馈原理图
（1）线性系统的可控性
如果一个系统在有限的时间内，在某种控制向量u(t)的作用下，系统状态向量x(t)可由一种初始状态达到任意一种目标状态时，则称此系统是可控的。也就是说，一个系统是可控的，就是在理论上存在这样的控制向量u(t)，使系统从当前状态转变为所希望的状态。
规定初始时间
,初始状态是任意的
,终端时间为
，终端状态即目标状态设在状态空间的原点上，即 =0,可控性判据分为两种形式：
方式一：设系统的状态方程为：
式中，B暂时设为 列阵，u为输入信号
若 n×n矩阵
满秩，即
则这样的系统是可控的，否则不可控。
方式二：设系统的状态方程为：
式中，B暂时设为 列阵，u为输入信号
如果方程式无重特征根，那么它可变成
式中， （i=1,2，···，n）是系统的相互不等的特征根。
如果系统的输入矩阵B中没有一行是全为0时，则系统是可控的，否则不可控。
由MATLAB代码分析，知系统r=4，系统是可控的，程序见附录4-KKKGFX.m。
Col =
0 0.2837 -0.0583 0.5173
0.2837 -0.0583 0.5173 -3.5483
0 0.7092 -3.1010 31.5899
0.7092 -3.1010 31.5899 -216.4684
r =
4
（2）线性系统的可观测性
一般来说，用系统状态变量表示的系统状态不一定都能直接测出，而系统的输出是必须能测出的变量。如何根据有限的输出了解系统的状态是系统可测性问题。系统的可测性的定义为：系统中如果每一个初始状态 都可通过在一个有限时间间隔内由输出量 的观测量确定，则称此系统是可观测的。系统的可观测性同样有两种方式判别：
方式一：设系统的状态方程为：
y=Cx
式中，B暂时设为 列阵，u为输入信号
若矩阵
满秩，即
则这样的系统是可观测的。
方式二：设系统的状态方程为：
y=Cx
式中，B暂时设为 列阵，u为输入信号
如果方程式无重特征根，那么它可变成
y=Cx
式中， （i=1,2，···，n）是系统的相互不等的特征根。
如果系统的输入矩阵B中没有一行是全为0时，则系统是可观测的。
4.3.3 LQR控制器
用MATLAB中的lqr函数，可以得到最优控制器对应的k，要求用LQR控制算法控制倒立摆摆动至竖直状态，并可以控制倒立摆左移和右移。
G3 = -1.0000 -3.4148 89.8052 11.7563
小车受扰动后能恢复到平衡状态，故符合要求，如图
图17 LQR控制器
五、仿真界面GUI动态模拟
GUI即图形用户界面（Graphical User Interface）的简称，又称图形用户接口，是指采用图形方式显示的计算机操作用户界面。GUI是借鉴并结合了计算机科学、美学、心理学、行为动作学及商业领域分析学的人机系统工程，将人—机—环境三者作为一个整体系统总体设计。与以往的计算机命令界面相比，GUI更容易让用户在视觉上得到接受，并获得很好的用户体验。此次演示采用了GUI技术，实现了倒立摆控制的动画演示。
5.1程序设计
采用最优LQR控制算法来实现倒立摆小车的控制，主程序dlb.m包括以下几个部分：
（1）模型参数的设定：采用 mc_CreateFcn()、mc_Ca33back()实现小车质量的设定，采用同样的方式可以实现对摆杆长度和质量的设定。
（2）LQR参数设定：采用 _CreateFcn()和 _Callback()实现 ,i=4,3,2,1。采用 r_CreateFcn()和 r_Callback()实现 R。
（3）利用LQR计算K：由 lqrok_Callback(）完成。
（4）K 的设定：采用 _CreateFcn()和 _Callbak()实现 ,i=4,3,2,1。 。
（5）摆杆角度和小车位置初始值设定：实现小车水平位置设定与回调 、小车水平拖动条设定与回调，摆杆角度设定与回调、摆杆角度拖动条的创建与回调。
（6）干扰的输入：干扰主要包括有冲击、阶跃和正弦等三种。
（7）仿真时间和步长的设定：主要利用Tedit__CreateFcn()和 Step__CreateFcn()来实现。
（8）仿真启动的设定。
（9）重置按钮：利用 reset_Callback(),实现倒立摆模型参数和 LQR 参数的重置。
（10）退出按钮：exit_callback()。
5.2演示界面的设计
首先在 MATLAB 环境命令框内输入“guide”便可进入 GUI 设计界面。此次设计的 GUI 界面文件为 dlb.fig，创建并保存该文件的同时，系统会自动生成主程序框架 dlb.m。 在主程序 dlb.m 框架下，通过相应的 GUI 组建回调函数中描述模型和编写控制算法。通过 MATLAB 环境下执行“guideDLB.fig”即可打开 GUI 编辑界面。 此次演示设计中，采用了 GUI 开发环境的触控按钮、静态文本、可编辑文本、滑动条、 坐标轴等系统组件。以小车的水平位置的界面设计为例，首先建立小车水平位置的 Edittext， 将其属性 tag 标签定义“in_po”。基于 GUI 的倒立摆 LQR 控制演示主界面如图 18 所示。
图18 GUI演示界面
5.3演示过程
通过以下4个步骤即可实现对倒立摆的动画演示：
（1）输入倒立摆的参数：包括倒立摆摆杆质量 m 和长度 L，还有小车质量 M。
（2）通过“初始值”下的“水平位置”和“摆杆角度”可设定倒立摆和小车的初始角度及位置；选择一种干扰输入（正弦、冲击、阶跃），并设定干扰输入的幅值。
（3）根据控制系统要求的性能输入控制器设计参数 和 R，点击“确定”即可得到控制器增益“K”。
（4）点击“启动仿真”便可以实现倒立摆的动态仿真演示。
5.4仿真结果
（1）根据之前的计算结果取 M=0.5，m=0.2，l=0.3 即下图 L=0.6，随意设置小车初始时刻的水平位置和摆杆的角度。控制器的参数设定为 和R=1，利用 LQR 方法求长，计算可得控制器增益
（2）K=[-92.3048 -11.5154 -10 -13.2052].
（3）设定仿真时间为 10 秒，步长 0.1。
（4）点击“启动仿真”图标，可得到如下仿真结果如图 19 所示。
图19 仿真结果显示
六、结论
在此次仿真中，对单级倒立摆小车系统分别采用牛顿力学法和拉格朗日方程法进行数学建模，并得到了相应的状态方程，二者相比，拉格朗日方程从能量的角度进行建模更加简便，便于处理。
对单级倒立摆小车系统的控制分别采用经典控制理论设计控制器（PID控制器）和现代控制理论设计控制器（最优控制LQR控制器），并用 MATLAB R2017b 软件仿真和调试控制系统。用MATLAB对不同控制方式下的倒立摆系统进行仿真，最后利用图形界面GUI工具设计倒立摆的仿真效果动画演示。小车位置跟踪输 入信号，控制摆杆超调足够小，稳态误差满足要求，上升时间和稳定时间也符合应用指标。
参考文献
[1]郝丽娜,刘兴刚.计算机仿真技术及CAD.[M].高等教育出版社.2009.
[2]吕庆莉.单级倒立摆系统分析与设计.陕西科技大学学报.2009,27(6):121-128.
[3]李洪兴,苗志宏,王加银.四级倒立摆系统的变论域自适应模糊控制.中国科学E辑.2002,32(1):65-75.
[4]黄国盛.单级倒立摆小车的控制系统.[M].2017.09
标签：仿真,方程,小车,系统,MATLAB,摆杆,单级,倒立 来源： https://www.cnblogs.com/Sonny-xby/p/12350454.html