直线二级倒立摆的建模和控制

西南科技大学

自动化专业方向设计报告

设计名称：直线二级倒立摆的建模和镇定控制

姓名：

学号:

班级：

指导教师：

起止日期：

方向设计任务书

学生班级：学生姓名：学号：

设计名称：

起止日期：指导教师：

方向设计学生日志

直线二级倒立摆的建模与镇定控制

摘要（150-250字）

倒立摆是一个典型的多变量、非线性、强耦合、欠驱动的自然不稳定系统，对倒立摆系统的控制研究，能反映控制过程中的镇定、非线性和随动等问题，因此常用于各种控制算法的研究。而且对倒立摆系统的研究还有重要的工程背景，对机器人行走、火箭的姿态调整等都有重要的现实意义。

本文以直线二级倒立摆系统为模型，阐释了直线二级倒立摆的建模方法和镇定控制算法。其次介绍了直线二级倒立摆系统的结构和参数，应用拉格朗日方程建模方法详细推导了二级倒立摆的数学模型，并对系统的性能进行分析。接下来，本文重点研究了最优控制算法在直线二级倒立摆镇定控制中的应用；在介绍倒立摆系统的最优控制算法的基础上，设计了系统的最优控制器，分析得出控制参数的选择规律；并且在Simulink上完成仿真实验,观察控制系统性能。

关键词：倒立摆；建模；LQR；镇定控制

Modeling and Balance Control of the Linear Double Inverted

Pendulum

Abstract：Inverted pendulum is a typical multivariable, nonliner, closed coupled and quick movement natural instable process of control research can reflect many key problems in control theory, such as the problem of tranquilization, non linearity, following and so on. So the inverted pendulum is commonly used for the study of many kinds of control theory. The research of inverted pendulum also has important background of engineering, and has practical significance for the Robot walk and Rocket-profile adjustment.

In this paper, taking the linear double inverted pendulum system as the control model, reaching of the control system based on lagrange equation and optimal control algorithm. First of all, giving out the research significance and situation of the inverted pendulum system,and introducing the linear double inverted pendulum modeling methods and stabilization control theory. Secondly, introducing the structure and parameters of the inverted pendulum system. Researching of the inverted pendulum mathematical model based on lagrange equation, and giving a detailed derivation, then having stability analysis of the system. Next, this paper studied the inverted pendulum system’s optimal control algorithm,and designed the LQR controller based on it, then coming to the law of selection of control parameters. Finishing the simulation in the Simulink software,observing the performance of the control system.

Key words: inverted pendulum, modeling, LQR, balance control

一、设计目的和意义

二、控制要求

对直线二级倒立摆模型的物理特性做分析，然后利用拉格朗日方程建模方法建立倒立摆的数学模型。利用线性二次最优控制理论设计倒立摆的LQR镇定控制器，在Matlab和Simulink 中完成倒立摆的镇定控制仿真，得出倒立摆LQR控制器设计规律。

三、设计方案论证

牛顿力学建模方法：根据传统的牛顿力学分析，建立二级摆动力学方程，最后经过求解方程建立系统的数学模型。

特点：分析通俗易懂，但需要求解大量微分方程，计算复杂；对于二级以上倒立摆建模很少使用。

拉格朗日建模方法：基于广义力和广义坐标的系统能量法，需要求出系统的动能和势能，建立Lagrange方程。

特点：理论较难，但建模简单，可以编程求解方程。

四、系统设计

倒立摆系统是由导轨、小车和各级摆杆组成，本文研究的直线二级倒立摆的物理结构如图1-1所示。小车依靠直流电机施加的控制力，可以在导轨上左右移动，其位移和摆杆角度信息由传感器测得，目标是使倒立摆在有限长的导轨上竖立稳定，达到动态平衡。

图1-1直线二级倒立摆的物理结构

其中，双摆系统由摆杆1、摆杆2、质量块1、小车和基座组成。摆杆1与摆杆2由质量块1连接，摆杆1与基座都由带滚动轴承的旋转轴自由相连。小车装在滑动轴上，通过电机带动皮带使小车移动。为了测量摆杆的转角，转轴上安装有增量式角度编码器，内杆与小车连接处的角度编码器的质量可直接考虑为小车质量的一部分，故图中只表注了内外杆连接处的编码器质量块1。两个摆杆不带动力源，两摆杆的运动控制只能通过小车的移动来实现。计算机从运动控制卡中读取实时数据，确定控制决策，并由运动控制卡来实现该控制决策，产生相应的控制量，使电机转动，带动小车运动，保持摆杆的平衡。当把摆杆提起到平衡位置附近后放开，

若小车不动，摆杆会由于重力倒下来。当小车在水平方向上移动时，摆杆受到一个转动力矩，这个力矩使摆杆朝与小车运动方向相反的方向运行，通过规律性的改变小车的受力方向使得摆杆在竖直方向上左右摆动，从而实现摆杆在竖直方向上的动态平衡。

直线二级倒立摆系统的物理参数如表所示。

表直线二级倒立摆的物理参数

1.基于Lagrange 方程方法建立二级摆的数学模型

为了对二级摆的性能做具体的研究，需要建立倒立摆的数学模型。系统建模方法可以分为两种：机理建模和实验建模。实验建模就是通过在研究对象上加上一系列的研究者事先确定的输入信号，激励研究对象并通过传感器检测其可观测的输出，应用数学手段建立起系统的输入－输出关系。机理建模就是在了解研究对象的运动规律基础上，通过物理、化学的知识和数学手段建立起系统内部的输入、状态关系。

对于倒立摆系统，由于其本身是自不稳定的系统，实验建模存在一定的困难。但是忽略掉一些次要的因素后，倒立摆系统就是一个典型的运动的刚体系统，可以在惯性坐标系内进行机理建模。对于小车系统的机理建模，一般有牛顿力学建模方法和分析力学中的拉格朗日方程建模方法两种途径。牛顿力学建模法分析复杂，且要计算大量的微分方程组，而拉格朗日建模方法分析简单，可以编程求解方程。因此本文采用拉格朗日方程建模方法建立倒立摆的数学模型，应用mathematica 软件编程求解拉格朗日方程，简化对微分方程的求解。 方程建模的基本原理

Lagrange 方程属于分析力学的范畴，是基于广义坐标和广义能量的方法。Lagrange 方程是由虚功原理推导出来的，指受理想约束的完整力学系中广义力做功为零。在分析力学中，质点系的虚功可以表示如下：

1

N

Qi i i W F q δδ==?∑

其中，Qi F 称为对应于第i 个广义坐标i q 的广义主动力，i q δ是广义坐标i q 的广义虚位移。因为i q δ可以是虚线位移，也可以是虚角位移，因此Qi F 可以有力或力矩的量纲，这体现了广义力的广义性。广义力一般分为保守力和非保守力，保守力是做功与路径无关的力，非保守力是

做功与路径有关的力。

虚功原理解决的是受理想约束的完整力学系统处于静平衡状态的问题，当系统在运动时，通过达朗伯定理引入惯性力，将系统运动时的问题转化为静力学问题。此时系统受主动力、约束力和惯性力处于静平衡状态，应用虚功原理，得到达朗伯-拉格朗日方程。

应用达朗伯-拉格朗日方程可以推导出基于广义坐标和广义能量的Lagrange

方程，本节中我们只对Lagrange方程进行应用，因此舍去其详细的推导过程。下面对Lagrange方程的三种基本形式做介绍。

（1）基本的第二类拉格朗日方程为：

（2）主动力是保守力的拉格朗日方程为：

（3）主动力包括保守力和非保守力的拉格朗日方程为：

其中为拉格朗日算子，q 为系统的广义坐标，T 为系统的动能，V 为

系统的势能，

i

Q为广义坐标

i

q对应的广义力。

对一个系统进行Lagrange方程建模时的步骤为：首先明确对象，一般取整个系统为研究对象。对整个系统进行受力分析，分析主动力，当出现非理想约束力时，将其视为主动力；选择恰当形式的拉格朗日方程。然后对系统的运动性能进行分析，恰当系统中独立的广义坐标。进而求取系统的动能和势能，得到系统的拉格朗日函数，求偏导和全导，代入拉格朗日方程，获N个运动微分方程。最后对N个微分方程进行线性化，求出系统的数学模型。

3.二级倒立摆系统建模

应用Lagrange方程对二级摆系统进行建模时，对二级摆系统进行受力分析。由于二级摆系统受推力和摩擦力作用，其中推力属于保守力，摩擦力属于非保守力。因此选用主动力包括保守力和非保守力的拉格朗日方程。

1,2,

i

i i

d L L

Q i n

dt q q

??

-==

??

???

对二级摆系统的运动特性进行分析，确定广义坐标。在二级倒立摆系统中，系统有三个广

义坐标，分别为

12

,,

xθθ。当

i

q x

=时，

i

Q F

=；当

12

i

qθθ

=、时，对应的

i

Q为零。然后求出系

统的动能和势能，系统的动能由小车、摆杆1、摆杆2和质量块1的动能组成，系统的势能由摆杆1、摆杆2和质量块1的势能组成。由于系统在12,θθ广义坐标下没有外力作用，可以列出系统在广义坐标中所受外力为零的方程。然后求解方程组，就可以得到倒立摆状态量的表达式，进而建立倒立摆的状态空间方程。

由于倒立摆系统是一个非线性和不确定的系统，因此应用机理建模法对二次级倒立摆系统数学模型时作如下假设： 1)每一级摆杆都是刚体。

2)在实验过程中同步带长度保持不变。

3)驱动力与放大器输入成正比并无延迟的直接施加于小车。

4)实验过程中的库仑摩擦、动摩擦等所有摩擦力足够小，在建模过程中可忽略不计。

使用Lagrange 方程对直线二级倒立摆系统的建模过程为： (1)求出倒立摆的动能和势能

X

0f x

图1-2 直线二级倒立摆受力分析

先对倒立摆的物理模型进行分析，如图2-3所示，然后建立摆杆和质量块的质心的坐标表达式。这里规定摆杆1的质心坐标为()11,g g x y ，摆杆2的质心坐标为()22,g g x y ，质量块的质心坐标为()33,g g x y 。质心坐标如下：

111

111

sin cos g g x x l y l θθ=-???

=??

21122

211222sin sin 2cos cos g g

x x l l y l l θθθθ=--???

=+??

311

311

2sin 2cos g g x x l y l θθ=-???

=?? 在直线二级倒立摆系统中，广义坐标为x ，1θ和2θ。根据倒立摆的物理模型分析图，列写出系统的动

能为：

123M m m m T T T T T =+++ 其中，M

T 、

1m T 、2m T 和3m T 分别为小车、摆杆1、摆杆2和质量块1的动能，它们分别为：

111m m

m T T T '''=+ '''222m m m T T T =+

其中，1m

T '和2m T '分别为摆杆1和摆杆2质心平动动能，1m T ''和2m T ''分别为摆杆1和摆杆2绕质心转动动能。然后，应用摆杆和质量块质心表达式求出系统的动能。 带入公式（）摆杆1的动能为：

所以得到

同理，求出摆杆2的动能为：

'''222m m m T T T =+

质量块1的动能为：

22111133(2sin )(2cos )12m d x l d l T m dt dt θθ??-????=+ ? ? ? ???????

因此，可以得到系统动能为

123M m m m T T T T T =+++

另一方面，系统的势能为

()

12311131121122

cos2cos2cos cos

m m m

V V V V m gl m gl m g l l

θθθθ

=++=+++

从而得到Lagrange函数为

L T V

=-

()

11131121

122

cos2cos2cos cos

m

gl m gl m g l

l

θθ

θθ

--

-+

（2）列出Lagrange方程

由于在广义坐标

2

1

,θ

θ

上均无外力作用，故列写Lagrange方程得到

将式代入式和得到

上述方程为倒立摆的动力学方程，求解微分方程可以得到倒立摆状态量的表达式，然后可以建立倒立摆的数学模型。

4.倒立摆运动方程的线性化处理

针对已建立的拉格朗日方程，求解方程可以得到倒立摆状态量的表达式。由于是关于系统的状态变量和输入控制量u的方程，小车施加的加速度信号作为控制量，有u x

=；因此设方程的解为：

对倒立摆模型进行线性化处理，这里采用在平衡点附近将函数进行泰勒级数展开。上面的方程为七元函数，因此采用对多元函数展开的方法展开，这里对二元函数的泰勒级数展开方法进行介绍。

二元函数的形式为

12

(,)

y f x x

=，在其平衡点

10200

(,,)

x x y附近进行泰勒级数展开。在平衡

点附近，由于偏差1110x x x ?

=-及2220x x x ?=-的绝对值很小，可以省略函数高次项得：

12121210200,01100,022012(,)(,)[()()()()]x x x x f f y f x x f x x x x x x x x ====??==+∣-+∣-??

将上式化简，得到一次线性方程：

1122y k x k x ?=?+? 1210,01x x f k x ==?=∣?，1220,02

x x f k x ==?=∣? 这样，y 与1x 和2x 之间的非线性关系，转化为y ?与1x ?和2x ?之间的线性关系。当系统的平衡点处于原点时，即10200x x y ===，可以对方程化简为：

1122y k x k x =+

按照二元函数的泰勒级数展开方法对公式（）进行展开，由于直线二级倒立摆系统的平衡点为：

；因此线性化后得到的表达式为：

其中

由于求解微分方程比较繁琐，因此对线性化处理后的方程采用mathematica 软件编写程序，求解倒立摆状态量的表达式。根据得到的参数，建立倒立摆的数学模型。

根据倒立摆系统的动能和势能，求得拉格朗日算子，然后建立Lagrang 方程，并按线性化后的关系求解方程。根据倒立摆的拉格朗日建模方法编写mathematica 程序，求解Lagrang 方程。直线二级倒立摆拉格朗日方程求解的mathematica 实现代码见附录1。

运行程序求出倒立摆状态量的表达参数ij k ，其中k11、k14、k15、k16、k21、k24、k25、k26的值为0，其余各参数的表达式如下：

1

3g(m1+2(m2+m3))k12=(4m1+3(m2+4m3))l =； 1

9gm2k13=-2(4m1+3(m2+4m3))l =； 1

3(2m1+m2+4m3)k17=-2(4m1+3(m2+4m3))l =； 29g(m1+2(m2+m3))k22=-

2(4m1+3(m2+4m3))l =；

2

3g(m1+3(m2+m3))k23=(4m1+3(m2+4m3))l =

； 2

3m1k27=2(4m1+3(m2+4m3))l =； 对二级倒立摆系统，取系统状态变量为

，然后建立连续状态空间方程为：

根据系统状态变量直接的关系，写出系统的状态空间方程为：

在倒立摆的镇定控制中,采用LQR 控制器实现镇定控制。当倒立摆系统受到干扰偏离了平衡状态时,能在不消耗过多能量的情况下,保持系统最快的回到平衡状态。设计最优控制器时，选择合适的正定矩阵()Q t 和()R t 使系统的性能指标J 取最小，然后利用Matlab 提供的函数(,,,)lqr A B Q R ，按照给定的加权阵求出控制参数K 。

在最优控制器的设计中，最优控制信号u 和加权阵()Q t 、()R t 的选择相关。()Q t 为状态变量加权矩阵，表示对误差向量的重视程度；()R t 为控制加权阵，表示在对控制能量的要求。实际上，控制系统的误差与控制消耗的能量是相互制约的，如果仅要求控制误差尽量小，则可能造成求得的控制向量u 过大，控制能量消耗过大；反之，为了节省控制能量，就不得不降低对控制性能的要求。因此选择加权矩阵时，要满足控制性能和控制能量消耗的要求。为了方便对每个控制量进行控制，通常将加权阵()Q t 、()R t 选择为对称矩阵，对角线上的加权系数表示对每个控制量的重视程度。在实际选择加权阵时，都是通过试凑法来实现，选择一组加权阵，然后仿真观察其控制性能是否满足要求，直到寻找到满足其性能要求的加权阵为止。加权阵()Q t 、()R t 是相对的，因此在实际选择中，先令()R t =1，然后改变()Q t 对角线上的值，直到满足性能要求为止。

在二级倒立摆的镇定控制中，要求系统最快的回到平衡位置，按照控制要求选择加权阵的

值。因为二级倒立摆控制器只有一个输入控制量，()R t 为标量，直接选择()R t =1。

在平衡点附近,小车的速度和各摆杆的角速度很小,与这些速度变量相对应的项对指标函数贡献很小,为简化控制,我们只选择状态向量K 中的位移量作为受控量,因而各速度项对应的权系数取为0。因为二级倒立摆是一个高阶、绝对不稳定的系统, 上下摆的稳定性是最重要的，所以其对应的加权系数取值要大一些。又因为上、下摆稳定是通过小车的移动来实现的, 所以对小车位置的变化范围不要控制的过严, 以免在扰动过大时失去调节作用, 所以位置对应的加权系数取值要尽量小些。总之在考虑各个状态向量时，上摆偏角比下摆偏角重要，下摆偏角比小车位移重要，按照这个规律选择加权矩阵()Q t 、()R t 能较快的满足控制性能要求。

控制器设计

利用线性二次最优控制规律设计LQR 控制器时，就是求取控制器的反馈增益K 的问题。根据期望性能指标选取加权矩阵()Q t 、()R t ，利用Matlab 命令lqr 就可以得到反馈增益K 的值。然后利用求得的K 值进行仿真实验，观察系统性能是否满足要求。若不满足要求，则改变加权矩阵()Q t 的值，直到符合系统的性能要求。【给出公式】

在设计LQR 控制器时，根据LQR 控制规律，编写直线二级倒立摆的LQR 控制Matlab 计算程序，程序见附录2。程序中利用lqr 指令计算出控制器的反馈增益K ，然后进行LQR 控制仿真，观察系统在扰动信号下的响应。系统中通过引入反馈增益K 来消除稳态误差，控制信号为输入量与输出信号乘以反馈增益之后的差。这样反馈信号与输入信号的量纲不同，需要对输入信号做标度变换，给输入信号乘以增益Nbar 。程序中用函数rscale 求出增益Nbar ，即Nbar=rscale(A,B,Cn,0,K),由于Matlab 工具箱中不带rscale 函数，在运行程序时要将函数添加到工具箱中。实际上输入信号与反馈信号相减时，对应的是输入量与小车位移x 对应的项相减，因此输入量的增益Nbar 与K 向量中x 对于的项相等，即Nbar=Kx 。

在程序中选择加权矩阵()Q t ，求出控制器的最优反馈增益K ，并对控制系统在输入扰动作用下的性能进行分析。在最优控制器的设计时，控制器的加权阵由选择者选择，然后验证得到的控制参数是否满足控制性能。这里首先将加权阵()R t 选择为1，然后选择()Q t 中的11Q 、22Q 和33Q 参数，获得最优反馈增益K 。这里选择11Q =1、22Q =1和33Q =1，求取LQR 控制器的反馈矩阵K ，然后进行系统的干扰响应实验，得到仿真如图3-2所示。

控制器的控制参数优化

按照前面的控制器设计程序，重新选择二级倒立摆的加权阵，直到获得最佳的控制参数K 。选取二级倒立摆的加权阵()Q t 的参数时，按照上摆偏角比下摆偏角重要，下摆偏角比小车位移重要的规律选择。改变小车位移、摆杆1角位移和摆杆2角位移对应的加权系数11Q 、22Q 和33Q ，得出二级倒立摆系统的QR 参数与反馈控制矩阵K 以及系统控制性能的关系，在固定R 为1的情况下，加权阵Q 的参数在0-100时，控制器的控制能力很弱，不能明显的改善控制性能；加权阵Q 的参数在100-1000内时，控制器能明显的提高控制性能；加权阵Q 的参数大于1000时，系统的的控制量加大，但是性能提高缓慢，而且参数选择过大时，系统会丧失稳定性。因此选择加权阵Q 的参数时，在100-1000的范围内合理选择，能获得最优的控制性能。

在加权阵Q 中，将小车位移加权系数从100增加到500后，系统的性能指标提高明显；继续加大小车

位移加权系数，不能明显减小稳定时间；因此将小车的位移加权系数选为500时，系统性能最优。固定小车位移加权系数11Q 为500时，相应增加摆杆1和摆杆2的加权系数，则反馈控制矩阵K 中摆杆1的角速度反馈项减小，其余项增大，小车位移和稳态时间减小；当摆杆1和摆杆2 的加权系数过大时，摆杆的灵敏度较高，系统的稳定时间增加。

所以，要在较小的控制量下得到较好的控制性能，应当选取Q=100(5,7,9,0,0,0)diag ?，R = 1时，反馈控制矩阵K=[]22.36 171.55 -295.08 24.66 -0.85 -43.64，二级倒立摆系统的镇定控制性能较佳。

五、设计结果及分析

要在较小的控制量下得到较好的控制性能，应当选取Q=100(5,7,9,0,0,0)diag ?，R = 1时，反馈控制矩阵K=[]22.36 171.55 -295.08 24.66 -0.85 -43.64，二级倒立摆系统的镇定控制性能较佳。此时倒立摆干扰响应实验的仿真如图1-3所示。【给出搭建出来的simulink 仿真环境图】【要对仿真结果进行分析】

图1-3 LQR 控制仿真结果

图中倒立摆的稳定时间为左右，为最优结果。

结束语

参考文献（递增引用，引用相关内容）

[1]毕效辉,自动控制理论[M].北京：中国轻工业出版社，2007.

[2]郭圣权,毕效辉.现代控制理论[M].北京：中国轻工业出版社，2007.

[3]郑恩让,聂诗良.控制系统仿真[M].北京：北京大学出版社，2006.

[4]荆海英.最优控制理论与方法[EB].超星数字图书馆，2002.

[6]倒立摆与自动控制原理实验[Z].固高科技（深圳）有限公司，2005.

[7]梁艳阳.时变不确定机电运动系统的非线性自适应控制[D].中国科技大学博士学位论文,2008.

[8]宋君烈,肖军,徐心和.倒立摆系统Lagrange方程建模与模糊控制[J]. 东北大学学报, 2002,23(4)

[9]吴文进,葛锁良.基于LQR 最优调节器的二级倒立摆控制系统[J]. 安庆师范学院学报(自然科学版)，

2007,13(2)

[10]张宏立.基于MATLAB/RTW的实时控制系统开发[J].制造业自动化，2004，362

附录（程序、电路图等）

附录1 直线二级倒立摆拉格朗日方程求解的

mathematica 实现代码

其中M 为小车质量，m1为摆杆1的质量，m2为摆杆2的质量，m3为质量块的质量，1l 为摆杆1的长度，2l 为摆杆2的长度；

'2TM=1/2*M*x [t];小车的动能

1g 11x =x[t]-l *Sin[θ[t]];

1g 11y =l *Cos[θ[t]];摆杆1的质心

^2^2t 1g t 1g T1=1/2*m1*((x )+(y ))??^2'^211

+1/2*(1/3*m1*l )*θ[t];摆杆1的动能 2g 1122x =x[t]-2*l *Sin[θ[t]]-l *Sin[θ[t]];

2g 1122y =2*l *Cos[θ[t]]+l *Cos[θ[t]];摆杆2的质心

^2^2t 2g t 2g T2=1/2*m2*((x )+(y ))??^2'

^222+1/2*(1/3*m2*l )*θ[t];摆杆2的动能

3g 11x =x[t]-2*l *Sin[θ[t]];

3g 11y =2*l *Cos[θ[t]];质量块的质心

^2^2t 3g t 3g T3=1/2*m3*((x )+(y ));??质量块的动能

1g 2g 3g V = m1*g*y + m2*g*y + m3*g*y ;整个倒立摆系统的势能

Simplify[V];

L = TM + T1 + T2 + T3 - V;拉格朗日算子

Simplify[L];

'θ1[t]L1=L;?

Simplify[L1];

θ1[t]f1=L1-L;t ??

Simplify[f1];倒立摆系统在广义坐标1θ上的Lagrang 方程 'θ2[t]L2=L;?

Simplify[L2];

θ2[t]f2=L2-L;t ??

Simplify[f2];倒立摆系统在广义坐标2θ上的Lagrang 方程 ''''12Solve[{f10,f20},{θ[t],θ[t]}];====求解Lagrang 方程得到''''12θ,θ的表达式 ''1F1=θ[t]/.%;''1θ[t]的表达式

''2F2=θ[t]/.%%;''2θ[t]的表达式

x[t]12k11=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→求解''1θ的表达式的参数 1θ[t]12k12=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ 2θ[t]12k13=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '12x [t]k14=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '1

12θ[t]k15=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '2

12θ[t]k16=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ ''12x [t]k17=F1 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→

x[t]12k21=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→求解''2θ的表达式参数 1θ[t]12k22=F12/.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ 2θ[t]12k23=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '12x [t]k24=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '1

12θ[t]k25=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ '2

12θ[t]k26=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→ '''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ ''12x [t]k27=F2 /.x[t]0 /.θ[t]0 /.θ[t]0 /.?→→→

'''''12x [t]0 /.θ[t]0 /.θ[t]0 /.x [t]0;→→→→ Simplify[k11]

Simplify[k12]

Simplify[k13]

Simplify[k14]

Simplify[k15]

Simplify[k16]

Simplify[k17]

Simplify[k21]

Simplify[k22]

Simplify[k23]

Simplify[k24]

Simplify[k25]

Simplify[k26]

Simplify[k27]

M=1.32;m1 = 0.108; m2 = 0.108; m3 = 0.208;

l = 0.2; l = 0.2; g = 9.8

12

k11

k12

k13

k14

k15

k16

k17

k21

k22

k23

k24

k25

k26

k27

附录2 直线二级倒立摆的LQR镇定控制

Matlab计算程序

clear all;

format long;

k12=;k13=;k17=;

k22=;k23=;k27=;

A=[0 0 0 1 0 0;

0 0 0 0 1 0;

0 0 0 0 0 1;

0 0 0 0 0 0;

0 k12 k13 0 0 0;

0 k22 k23 0 0 0];

B=[ 0 0 0 1 k17 k27]';

C=[1 0 0 0 0 0;

0 1 0 0 0 0;

0 0 1 0 0 0];

D=[0; 0; 0];

Q11=1;Q22=1;Q33=1;

Q=[Q11 0 0 0 0 0;

0 Q22 0 0 0 0;

0 0 Q33 0 0 0;

0 0 0 0 0 0;

0 0 0 0 0 0;

0 0 0 0 0 0];%LQR控制的Q加权矩阵中只需确定小车位移、摆杆1

%和摆杆2的角度

R=1;%将LQR的R加权阵设置为1

K=lqr(A,B,Q,R) %计算LQR控制器的反馈增益

Ac=[(A-B*K)]; %加入反馈增益K后的矩阵

Bc=[B];

Cc=[C];

Dc=[D];

T=0::5;

U=0*ones(size(T)); %以T的长度为矩阵大小，设置输入信号为零

Cn=[1 0 0 0 0 0];

Nbar=rscale(A,B,Cn,0,K); %求取输入信号的比例系数

Bcn=[Nbar*B];

x0=[0 0 0 0 0]; %扰动信号

[Y,X]=lsim(Ac,Bcn,Cc,Dc,U,T,x0); %系统对输入U的时间响应，初始状态X0

%返回输出矩阵Y和状态矩阵X

pos=Y(:,1); %小车位移向量

angle1=Y(:,2); %摆杆1角度向量

angle2=Y(:,3); %摆杆2角度向量

plot(T,pos,'k-','LineWidth',2) %黑色实线

hold on

plot(T,angle1,'k--','LineWidth',2) %黑色短横虚线hold on

plot(T,angle2,'k:','LineWidth',2) %黑色小点虚线legend('CarPos','Angle1','Angle2')

附录三：MATLAB模块图

本文来源：https://www.bwwdw.com/article/fgye.html