654523993毕业设计（论文）基于卡尔曼滤波的倒立摆控制系统噪声抑制

《654523993毕业设计（论文）基于卡尔曼滤波的倒立摆控制系统噪声抑制》由会员分享，可在线阅读，更多相关《654523993毕业设计（论文）基于卡尔曼滤波的倒立摆控制系统噪声抑制（45页珍藏版）》请在装配图网上搜索。

1、基于卡尔曼滤波的倒立摆控制系统噪声抑制作 者 姓 名：指 导 教 师：单 位 名 称：流程工业综合自动化国家重点实验室专 业 名 称：自动化东 北 大 学2007年6月Titleby Supervisor: Northeastern UniversityJune 2007东北大学本科毕业设计（论文） 毕业设计（论文）任务书毕业设计（论文）任务书毕业设计（论文）题目：基于卡尔曼滤波的倒立摆控制系统噪声抑制设计(论文)的基本内容：（1）了解倒立摆系统。（2）对倒立摆系统进行建模。（3）辨识倒立摆系统参数。（4）倒立摆平衡控制，包括将动力学模型线性化、仿真、实验等。（5）学习卡尔曼滤波，并应用该方法

2、实现倒立摆系统的噪声抑制。（6）总结研究成果，撰写毕业论文。毕业设计（论文）专题部分：题目：设计或论文专题的基本内容：学生接受毕业设计（论文）题目日期第1周指导教师签字：2012年2月27日- V -基于卡尔曼滤波的倒立摆控制系统噪声抑制摘要关键词： TitleAbstractKey words： 目录毕业设计（论文）任务书I摘要IIAbstractIII目录IV第一章 绪论11.1 课题背景及研究的目的和意义11.2 状态估计算法的研究概述3第二章 二级倒立摆系统的数学模型62.1 二级倒立摆系统实验软硬件条件62.2 二级倒立摆系统的动力学模型72.2.1 拉格朗日方程72.2.2 二级倒

3、立摆的模型72.3 小结13第三章 二级倒立摆系统参数辨识143.1 能量法辨识方程建立153.2 辨识实验173.2.1 辨识程序实现173.2.2 参数辨识的物理实验19第四章 二级倒立摆平衡点控制204.1 基于LQR方法设计平衡控制器204.1.1 控制理论介绍204.1.2 控制器设计204.1.2.1 下平衡控制器设计224.1.2.2 上平衡控制器设计234.2 仿真以实际实验效果234.2.1 仿真实验效果234.2.2 仿真曲线254.3 实际实验效果264.3.1 下平衡点平衡控制264.3.2 上平衡点平衡控制284.4 小结29第五章 Kalman滤波器设计305.1

4、Kalman滤波器基本形式305.2 Kalman滤波器设计315.3 Kalman滤波器仿真实验与分析325.4 小结35第六章 结论与展望36参考文献37致谢38第一章 绪论1.1 课题背景及研究的目的和意义在控制理论发展的过程中，某一理论的正确性及实际应用中的可行性需要一个按此理论设计的控制器去控制一个典型对象来验证，倒立摆系统就是控制领域中的典型对象之一。倒立摆系统作为一个典型的单输入多输出、非线性、强祸合、自然不稳定的系统，从理论上将涉及系统控制中的很多关键问题:如非线性问题、鲁棒性问题、镇定问题、随动问题以及跟踪问题等，而且结构简单、构件组成参数和形状易于改变、成本低廉，其控制效果

5、可以通过其稳定性直观地体现。因此，作为一个多用途的综合性实验装置，倒立摆系统成为控制领域中不可或缺的研究设备和验证控制策略有效性的实验平台早在二十世纪50年代，麻省理工大学电机工程系的控制论专家根据火箭发射助推器原理设计出了单级倒立摆系统的模型。在70年代，人们对倒立摆实物系统进行了实际研制1,2。后来在此基础上，人们又不断进行拓展，产生了许多新的倒立摆结构。对于现阶段被重点研究并制作出实物平台的倒立摆，大体可以分为直线倒立摆与平面倒立摆两类。其中直线倒立摆包括二级倒立摆、多级倒立摆、环形倒立摆、球车系统、简易倒立摆等，平面倒立摆主要包括XY平台平面倒立摆和两自由度旋转机械臂平面倒立摆两种类型

6、。 (a)火箭助推器 (b)两足步行机器人图1.1倒立摆原理的典型应用倒立摆的研究具有重要的工程背景，其控制方法在军工、航天、机器人和一般工业过程领域中都有着广泛的用途，如两轮电动车的平衡控制、机器人行走过程中的平衡控制、火箭发射中的垂直度控制和卫星飞行中的姿态控制等均涉及到倒置问题。因此对倒立摆控制机理的研究具有重要的理论和实践意义。对于倒立摆系统，位移及摆角信号在传输过程中不可避免的会引入噪声干扰，有时噪声干扰甚至会严重影响对系统的稳定控制，对于模拟信号尤甚。信号噪声干扰是工程领域和人们生活中的一大公害。最常见的噪声干扰比如:收音机远离发射台时发出的杂音，电视屏幕上的雪花点，或是引起模/数

7、转换器转换错误的信号等等。图1.2电视雪花希望抑制信号中的噪声，就需要使用滤波器对信号进行滤波。由于模拟滤波器需要一定的成本，且精度较差，因此考虑使用数字滤波器。但本二级倒立摆实物平台的摆角信号噪声幅度过大，是一般的时不变滤波器难以滤除的，所以需要使用现代滤波方法滤波。通过对一系列带有观测噪声和干扰信号的实际观测数据的处理，从中得到所需要的各种参量的估计值，这就是估计问题3。估计问题又分为状态估计与参数估计。由于现代滤波方法都考虑了系统的模型的统计特性，因此均可视为状态估计方法。卡尔曼滤波是美国学者卡尔曼(Kalman)于1960年提出的4，是采用状态方程和观测方程组成的线性随机系统的状态空间

8、模型来描述滤波器，并利用状态方程的递推性，按线性无偏最小均方差估计准则，采用递推算法对滤波器的状态变量做最佳估计，从而求得滤掉噪声的有用信号的最佳估计。由于卡尔曼滤波器采用递推形式，数据量小，易于计算机实现，所以一经提出便在各行各业得到了广泛应用，是当今被研究最多的滤波算法之一。鉴于卡尔曼滤波器功能强大且实现起来较为简单，本文利用卡尔曼滤波器对系统的摆角信号进行状态估计，然后利用估计值作为控制器摆角信号的输入，最终实现对二级倒立摆的稳定控制。这对加深控制理论的理解，锻炼实际动手能力，掌握信号处理方法是很有意义。1.2 状态估计算法的研究概述在确定性系统的情况下，线性系统的状态估计的主要方法有L

9、uenberger观测器5，非线性系统的状态估计主要方法有扩展Luenberger观测器，状态重构只能在系统能观测的部分实现，优点是重构速度能够任意快，缺点是在具体实施过程中要受到一些限制因素的影响，例如灵敏度、噪声等。当随机噪声干扰存在于系统装置或系统观测通道中时，就不能使用确定性系统情况下的观测器进行状态估计了，这时要使用统计估计方法实现系统的状态估计。由于观测信号方式的利用和状态估计准则的不同，导致了状态估计方法的不同。最初的状态估计算法是高斯提出的最小二乘法，然后是Fisher极大似然法，随后Wiener提出了维纳滤波，后来发展到Kalman提出的卡尔曼滤波方法，状态估计理论的发展从频

10、域到时域、从非递推算法到递推算法、从非平稳随机过程到状态空间模型，估计理论越来越丰富和成熟，适用范围越来越广，在航天、航空、军事、生产过程控制等领域，状态估计技术被广泛应用6。1795年，F.K.Gauss提出了最小二乘法，引入了带有估计误差的多个观测数据的概念，并用它处理比较简单的轨道测量问题，但没有考虑被估参数和观测数据的统计特性。1912年，从事概率密度研究的R.A.Fisher提出了极大似然估计法，对估计理论的广泛研究做出了又一个重大贡献，但需要知道观测数据的概率分布函数。1941年，Wiener提出维纳滤波方法，并应用在火炮打飞机的控制系统中，经典的Wiener滤波方法只适用于频域，

11、并且仅限于对平稳时间序列进行滤波和预报，不适用于时变、多变量、非平稳时间序列的滤波和预报，由于维纳滤波算法不能进行递推，需要对全部的历史数据进行存储，因此在实际工程应用中不容易实现。由于维纳滤波的上述局限和缺点，加上计算机应用技术发展的需要，1960年，美国学者Kalman提出了卡尔曼滤波方法。卡尔曼滤波算法能够解决时变、多变量、非平稳时间序列的滤波和预报问题，并且这是一种可递推算法，在计算机上很容易实现，这些特点都弥补了Wiener滤波的不足7。卡尔曼滤波的实现是基于信号和噪声的状态空间模型，利用现时刻的观测值，并结合前一时刻的状态预测值，来更新当前时刻的状态变量的估计值，得出当前时刻最优的

12、状态估计值8。Kalman滤波理论提出以后，立即在实际工程中得到了广泛应用，早期最成功的应用实例是C-SA飞机导航系统的设计以及阿波罗登月计划。随着电子计算机技术的普遍应用，在实际工程中，特别是航空航天技术中，Kalman滤波理论迅速被广泛应用。已有的关于卡尔曼滤波在线性系统中应用的很多理论己经十分成熟，作为一种重要的最优估计理论Kalman滤波已经被广泛地应用在导航制导系统、目标跟踪、全球定位系统、通信和工业控制等领域中。卡尔曼滤波算法是一种线性状态的最优估计，是在系统状态方程和量测方程都是线性方程，系统噪声和观测噪声均为高斯白噪声，且统计特性已知的条件下，得到最优滤波结果。当系统模型具有非

13、线性特性时，如果仍然采用线性模型描述系统并且使用KF进行滤波，将会引起线性模型近似误差，甚至将会导致滤波状态发散。而所有的实际系统都有一定程度的非线性。这就促使人们寻求新的非线性滤波器。在之后的近半个世纪里，许多学者以Kalman滤波器为基础进行了推广和改进。只有完整描述非线性系统的条件后验概率，才是解决非线性系统滤波问题的最佳方法，但是这种完整的描述在实际系统中不容易实现，所以人们又提出了许多次优滤波的近似方法。进行非线性系统滤波问题的次优近似，主要有两种方法:1) 把非线性环节进行线性化处理，忽略或逼近高阶项；2) 用采样的方法去近似非线性函数的分布9。其中第一种方法是解决非线性系统问题的

14、传统方法，这种方法应用最广泛的是扩展卡尔曼滤波(Extended Kalman Filter EKF)算法。EKF最初是StanleySchmidth在1967年为了将卡尔曼滤波应用在非线性航天器导航问题提出的，EKF算法是通过对非线性函数进行Taylor展开，对二阶及其高阶项进行忽略，实现非线性系统的线性化转换。虽然EKF算法在非线性滤波问题中应用广泛，但它仍然具有理论局限性，具体表现在以下方面: 1)当系统非线性程度比较严重时，忽略Taylor展开式的高阶项将会增大线性化引起的误差，从而导致EKF的滤波误差增大甚至滤波发散。2)滤波过程需要求取系统的雅可比矩阵，但求取雅可比矩阵复杂且计算量

15、大，不容易求取，有时甚至很难得到非线性函数的雅可比矩阵。3)由于EKF算法需要对非线性函数进行求导，所以非线性函数的表达式必须明确，并且不适用于不可微的非线性函数。由于近似非线性函数不容易，而相比之下近似非线性函数的概率密度分布更容易，因此人们想到使用采样的方法来近似非线性函数的概率密度分布来解决非线性系统的滤波问题。Unscented变换(Unscented Transformation ，UT)随后就发展起来，这是一种通过非线性变换传播均值和协方差的方法10,11。这种方法滤波更准确，更容易实现。基于Unscented变换的Kalman滤波算法(Unscented Kalman Filte

16、r UKF)就是基于UT变换，选择一种确定性采样的方法，然后套用标准的卡尔曼滤波算法的框架，最后得出估计的状态。这种滤波算法是牛津大学Julier121,13等学者首先提出的。UKF算法不需要计算系统的雅可比矩阵，复杂度降低，还能处理有非加性噪声的系统，应用范围变宽，UKF算法采用确定性采样策略，避免了粒子的退化问题，滤波精度大大提高。UKF的研究取得了大量成果，已被应用于许多领域，UKF广泛应用于导航制导、随机信号的处理等领域，然而，UKF在化工过程领域的应用相对较少，特别是反应过程复杂的化工过程，如发酵反应等化工过程。- 38 -第二章 二级倒立摆系统的数学模型在研究Pendubot控制策

17、略之前必须先得到它的动力学模型，本章将基于拉 格朗日动力学理论建立Pendubot的动力学模型。欠驱动机器人Pendubot是一个在垂直平面上运动的两杆机器人，它在肩部有一个驱动，而肘部没有，因此Pendubot是典型的欠驱动系统，同时由于Pendubot两杆之间强的耦合性及加速度约束不可积，因而具有较强的非线性特征。建立机器人动力学模型主要采用以下两种理论:(1)动力学基本理论，如牛顿一欧拉方程;(2)拉格朗日动力学42，特别是二阶拉格朗日方程。拉格朗日动力学和与牛顿的力学原理在经典力学的范畴内是等价的，但它们研究的途径或方法则不相同43-45，因此基于这两种理论所建立的动力学模型是等价的，

18、只是描述形式不同。直接运用牛顿方程的力学体系也称为矢量力学，拉格朗日动力学则称为分析力学。牛顿一欧拉方程的方法，需从动力学出发求得加速度，并消去各个内作用力，这对于复杂的系统十分繁杂。而基于拉格朗日动力学的方法是从系统能量的角度考虑问题，它只需计算速度而不要求内作用力，因此是一种比较简洁的方法。由于拉格朗日动力学方法仅需计算Pendubot的动能与势能，与牛顿一欧拉方程相比更为简洁，而且还能够充分反映Pendubot的动力学结构特征，因此本论文采用拉格朗日动力学方法建立Pendubot动力学模型，也就是基于运动能量来进行。2.1 二级倒立摆系统实验软硬件条件如图2.1所示为倒立摆机器人控制系统

19、结构示意图。倒立摆机器人摆臂分为主动臂和欠驱动臂，其角度信号分别通过高精度光电编码器返回，上位机通过网络化实验控制器使驱动器控制力矩电机，使其实现控制要求。图 2.1 倒立摆机器人控制系统结构示意图2.2  二级倒立摆系统的动力学模型2.2.1 拉格朗日方程对于任何机械系统，其拉格朗日函数都可以定义成该系统动能和势能之差，即(2.1)系统的动能和势能可以用任意选取的坐标系来表示。系统的动力学方程（第二类拉格朗日方程）为 (2.2)由于势能不显含速度项，因此动力学方程也可以写成 (2.3)那么，对于欠驱动机器人倒立摆机器人系统，其拉格朗日运动方程则为：(2.4)基于倒立摆机器人的动力学

20、模型，其中摆臂连杆受到的力矩为，只有摆臂连杆的主动关节受力。由于假设两杆均为刚体，所以其动能与势能可根据每一杆的总质量与相对于重心的惯量来确定。2.2.2 二级倒立摆的模型倒立摆机器人系统模型可用图3.1所示坐标描述：图 2.2 倒立摆机器人控制系统动力学模型示意图其中：主动臂的长度；：主动臂相对于连接点到质心的距离；：欠驱动臂相对于连接点到质心的距离；：主动臂相对于坐标轴的角度；：欠驱动臂相对于主动臂的角度；：主动臂相对于质心转动惯量；：欠驱动臂相对于质心转动惯量；：主动臂质量；：欠驱动臂质量；：重力加速度。根据上述模型，进行系统的动能和势能的推导。计算平移动能的一般表达式为，而系统两个连杆

21、的角速度为 (2.5)主动臂的平移动能可以直接表示为(2.6)势能与机械臂质心的高度有关，高度用y坐标表示，于是势能可以直接写成(2.7)对于欠驱动臂，先写出质心处笛卡儿坐标位置的表达式，然后求微分，以便得到关节角速度。(2.8)于是，速度的笛卡儿坐标分量为 (2.9)速度的平方值为(2.10)从而欠驱动臂的平移动能为 (2.11)连杆的高度已经给出，由式(2.7)，它的势能为(2.12)同时我们注意到系统除了平移动能外，还存在旋转动能，由式(2.6)可得系统的旋转动能部分为 (2.13)拉格朗日算子为，因此根据上述式子，可得(2.14)为了求得动力学方程，现在根据式(2.14)，对拉格朗日算

22、子进行微分，即 (2.15)由以上的各微分项，根据第二类拉格朗日方程，我们就可以直接得到主动关节的力矩表达式为 (2.16)再把拉格朗日算子对和求微分，进而得到欠驱动关节的动力学方程：(2.17)于是欠驱动关节的动力学方程为(2.18)式和式可以简写成如下形式：(2.19)在考虑摩擦力矩的情况下，上述表达式可以拓展成为(2.20)其中，其中各个参数的表达式如下：如果系统动力学模型表示成式(2.20)，那么有一个特征要注意：和并非两个完全独立的矩阵，两者之间是一个反对称矩阵。其物理意义是机械臂动能的导数等于驱动器力矩和重力矩的功率输入之和(2.21)因此，对于任意时间有(2.22)我们得到如式(

23、2.20)的系统动力学表达式，在实际应用中，为了计算与实验的方便，引入以下五个参数表达式：(2.23)因此有(2.24)则可将倒立摆机器人动力学方程改写成状态方程形式：(2.25)令得到状态方程：(2.26)即Pendubot系统的状态方程为(2.27)其中，(2.28)2.3 小结本章基于拉格朗日动力学理论建立Pendubot的动力学模型。在建立Pendubot机器人动力学模型时，把Pendubot看作是一个个关节连接起来的刚体。并用基于运动能量的拉格朗日动力学方法建立Pendubot动力学模型。这种方法仅需计算Pendubot的动能与势能，因而与牛顿一欧拉方程相比更为简洁，而且还能够充分反

24、映Pendubot的动力学结构特征。本章所得到的动力学模型是研究Pendubot控制策略的基础。第三章 二级倒立摆系统参数辨识首先，针对倒立摆机器人控制系统参数辨识需要了解倒立摆机器人控制系统动力学模型，根据倒立摆机器人控制系统动力学模型建立相应辨识方程。倒立摆机器人控制系统动力学模型示意图如图3.1所示。图 3.1倒立摆机器人控制系统动力学模型示意图其中：为主动臂长度，为主动臂的质心长度，为欠驱动臂的质心长度，为主动臂质量，为欠驱动臂质量，为主动臂质心转动惯量，为欠驱动臂质心转动惯量。倒立摆机器人系统动力学模型通过拉格朗日动力学建立。倒立摆机器人控制系统动力学方程和相关参数如下：(3.1)其

25、中，为电机力矩矢量，q为倒立摆机器人系统的角度矢量位置，,。由动力学方程可知，方程含有很多正常数项，主要是系统中各部分的质量，转动惯量和长度，所以为了简化方程，便于控制器设计，设计系统常数如下： (3.2)这5个参数即为系统所需辨识的参数。3.1 能量法辨识方程建立能量法即通过能量守恒定律来建立辨识方程，是机械系统最常用的在线或离线辨识方法之一。针对被控对象的输入能量和输出能量建立能量守恒方程，通过梯度法、最小二乘等方法求得所需的辨识参数。倒立摆机器人的输入能量来自力矩电机，输出能量主要表现为动能和势能，一般情况下忽略摩擦以及热能的损耗。首先，列出倒立摆机器人控制系统的动能表达式 (3.3)以

26、及势能表达式 (3.4)通过动力学方程可知，动能和势能的具体表达式如下，由于倒立摆机器人的动能和势能表达式与系统所需辨识参数关系为线性的，因此可利用新参数简化式(3.3)和式(3.4)，便于辨识算法的设计。令，为倒立摆机器人系统在时刻的总能量，为倒立摆机器人系统在时刻的总动能，为倒立摆机器人系统在时刻的总势能。建立式(3.3)总动能的等价表达式：其中，。建立式(3.4)总势能的等价表达式：其中，、所以 由能量守恒定律得到 (3.5) 将式(3.5)线性参数化，得到参数辨识方程 (3.6)其中。由于为常数，而可以通过给定信号和反馈信号计算获得，所以式(3.6)可以通过最小二乘法得到辨识参数。3.

27、2 辨识实验3.2.1 辨识程序实现根据辨识方程可知，当方程组中线性无关方程数大于需辨识参数个数时，通过最小二乘方法即可获得最优解，同时所需辨识参数为正常数，所以我们最终采用离线的非负最小二乘辨识系统参数。首先，打开EasyControl 软件，打开identification.dlm文件，设置采样周期，打开检测主动臂和欠驱动臂角位移和速度的示波器，并利用示波器的数据储存功能将数据存储在Matlab的Workplace中；然后给定系统的激励信号，本实验所采用的激励信号为一阶阶跃信号，信号幅值要求主动臂摆动瞬间超过0度，但小于90度，最终稳定在-45到0度之间即可；最后进行辨识实验。实验完成之后

28、，运行identification.m辨识程序，即可获得辨识结果。通过多次实验求得平均值，然后设计控制器，验证辨识结果的可实用性。参数辨识程序的实现需要经过两个步骤：数据采集和参数辨识程序。(1) 数据采集程序数据采集程序采用Simulink来编写，如图3.2所示为倒立摆机器人控制系统开环运行程序，主要任务为采集在激励信号下主动臂和欠驱动臂的角度信号和角速度信号。图3.3为速度平均滤波方式Simulink程序。取三次速度的平均值作为当前时刻的速度值。 (3.7) 图 3.2 倒立摆机器人控制系统开环运行的Simulink程序图 3.3 速度平均滤波方式Simulink程序(2) 参数辨识程序实

29、现采用Matlab的m语言编写非负最小二乘辨识程序，程序代码如下所示：dq1 (1)=0;%第一个采样周期主动臂的速度值dq1 (2)=0; %第二个采样周期主动臂的速度值dq2 (1)=0; %第一个采样周期欠驱动臂的速度值dq2 (2)=0; %第二个采样周期欠驱动臂的速度值dq2 (3)=0; %第三个采样周期欠驱动臂的速度值dq1 (3)=0; %第三个采样周期主动臂的速度值g=9.8;%重力加速度% dL1、dL2、dL3为动能表达式，dL4 、dL5为势能表达式dL1=(0.5*dq1.2);dL2=(0.5*dq1.2+dq1.*dq2 +0.5*dq2.2);dL3=(cos(

30、q2).*(dq1.2+dq1.*dq2);dL4=(g*sin(q1);dL5=(g*sin(q1 +q2);% tdq1为电机输入能量，在这里将驱动器增益常数和电机力矩常数考虑在需辨识的参数中% signal_Gain%为输入电压，0.4125为电机力矩常数，2.68为驱动器增益tdq1=signal_Gain.*dq1 *0.4125*2.68;for i=1:(length(dL1)-10), DL(i,1)=dL1(i+10)-dL1(i); DL(i,2)=dL2(i+10)-dL2(i); DL(i,3)=dL3(i+10)-dL3(i); DL(i,4)=dL4(i+10)-d

31、L4(i); DL(i,5)=dL5(i+10)-dL5(i); Itq(i,1)=trapz(Time(i:i+10),tdq1(i:i+10);endtheta=lsqnonneg(DL,Itq) %辨识结果3.2.2 参数辨识的物理实验本实验针对倒立摆机器人系统而进行的参数辨识，为用户提供有效可靠的参数值。实验所需参数设置如表3.1所示。表 3.1 实验参数设置激励信号幅值采样周期编码器精度0.48伏5毫秒1250*4 脉冲/圈系统常数如下：电机的转矩常数为0.4125Nm/A.驱动器的放大倍数为2.68A/V。电源作用为驱动器提供电压。Ø 实验过程运行EasyControl，

32、下载图3.2所示的Identification.dlm文件，设置示波器时间范围。运行程序，保存实验曲线，并将曲线数据保存为.m文件，然后运行该.m文件，将所需数据存在Workplace中，运行非负最小二乘辨识程序(identification.m文件)，获得参数辨识结果。Ø 实验结果实际实验结果：第四章 二级倒立摆平衡点控制4.1 基于LQR方法设计平衡控制器4.1.1 控制理论介绍LQR (linear quadratic regulator)即线性二次型调节器，其对象是现代控制理论中以状态空间形式给出的线性系统，而目标函数为对象状态和控制输入的二次型函数。LQR理论的设计目标是设

33、计状态反馈控制器，并使二次型目标函数取最小值，而由权矩阵与唯一决定，故此、的选择尤为重要。LQR理论是现代控制理论中发展最早的、也是最为成熟的一种状态空间设计法。特别地 ，LQR可得到状态线性反馈的最优控制规律 ，易于构成闭环最优控制。而且 Matlab 的应用为LQR 理论仿真提供了条件 ，更为我们实现稳、准、快的控制目标提供了方便。对于线性系统的控制器设计问题，如果其性能指标是状态变量和(或)控制变量的二次型函数的积分，则这种动态系统的最优化问题称为线性系统二次型性能指标的最优控制问题，简称为线性二次型最优控制问题或线性二次问题。线性二次型问题的最优解可以写成统一的解析表达式和实现求解过程

34、的规范化，并可简单地采用状态线性反馈控制律构成闭环最优控制系统，能够兼顾多项性能指标，因此得到特别的重视，为现代控制理论中发展较为成熟的一部分。4.1.2 控制器设计本实验采用LQR最优控制方法，首先通过系统模型建立空间状态方程 (4.1)设计LQR控制器，使二次型目标函数取最小值，其表达式如下： (4.2)在系统完全可控的条件下，其全状态反馈为 (4.3)其中，。式中的P为Riccati方程的解。可见，此处决定值大小的关键在于指标函数加权矩阵和的选择。不过，这里也可以使用Matlab指令来直接得到值。通过这个函数的参量，也可以说明了值与的取值有着直接的关系。最终设计状态反馈控制器：。控制系统

35、结构框图如图4.1所示：xdxu-LQR控制器控制对象图 4.1 控制系统结构框图当倒立摆机器人达到垂直向上不稳定平衡位置附近时，用LQR方法设计平衡控制器来控制倒立摆机器人，使之在垂直向上不稳定平衡点附近保持平衡。为设计平衡控制器，首先必须将倒立摆机器人的非线性运动方程线性化，同时设计一个具有线性模型的全状态反馈控制器。泰勒级数近似表达式用于线性化。(4.4)其中，为状态向量，为倒立摆机器人唯一控制输入。分别为倒立摆机器人的平衡状态向量及控制输入。平衡控制器是将倒立摆机器人控制在不稳定平衡点，因此为0，因此我们所要做的是找到偏微分矩阵并求其在平衡点的值。其中 (4.5)在平衡情况下，电机提供

36、的转矩主要克服两根杆重力相对于支撑点的转矩，因此倒立摆机器人的平衡点可作如下定义: (4.6)将倒立摆机器人的动力学方程对于状态向量求偏微分则有. (4.7)将倒立摆机器人的动力学方程对于控制输入求偏微分则有 (4.8)根据不同平衡点的不同，并结合前面应用系统辨识出的参数常量的值，就可以得到系统平衡点处的线性化模型形式： (4.9)得到线性化模块后，根据LQR方法设计全状态反馈控制器。4.1.2.1 下平衡控制器设计根据上述的描述，在下平衡情况下，。将该初始条件代入得： (4.10)通过以上的线性化模型，我们可以使用LQR方法全状态反馈控制器。本实验中的输出量可以认为是，且。则 (4.11)因

37、此，方程中的各个参量可以有如下取值： (4.12)最终计算得(4.13)4.1.2.2 上平衡控制器设计根据上述的描述，在上平衡情况下，。将该初始条件代入得： (4.14)与上平衡控制器的设计方法相同，这里输出量可以认为是，方程中的各个参量取值与下平衡控制器设计时相同。最终计算得(4.15)4.2 仿真以实际实验效果4.2.1 仿真实验效果采用MATLAB软件所提供的Simulink工具箱进行仿真程序设计，并借助Easy Control软件进行硬件实时仿真。操作步骤如下：(1) 打开Easy Control软件，并连接控制器。打开Simulink工具箱，新建工程文件进行辨识程序的模块搭建。模块

38、搭建如图4.2所示。a. Pendubot下平衡点Simulink仿真程序b. Pendubot上平衡点Simulink仿真程序图 4.2 将辨识后的五个参数输入到参数模块中，并修改控制器值，对Simulink程序的工程属性参数进行设置。属性参数设置包括：将仿真时间更改为inf，即不限时间；将“solver options”中类型改为“fixed-step”，即固定步长；将“real-time workshop”中的编译方式改为“xpctarget.tlc”。保存以上编辑好的程序，并编译下载至控制器中，打开观测相应信号的示波器。开始实验，通过示波器的高精度存储功能保存测得数据。使用MATLAB

39、绘制Pendubot的位置曲线。4.2.2 仿真曲线根据以上仿真程序所得Pendubot主动臂和欠驱动臂的位置仿真曲线，如图4.3和4.4所示。 a 下平衡点控制主动臂位置q1曲线b 下平衡点控制欠驱动臂位置q2曲线图 4.3 a 上平衡点控制主动臂位置q1曲线b 上平衡点控制欠驱动臂位置q2曲线图 4.4 4.3 实际实验效果4.3.1 下平衡点平衡控制将上面所得到的下平衡点参数下载至Simulink程序中（图4.5）中，其中图4.6为subsystem2子模块结构图，主要功能为将编码器返回的脉冲信号转化为我们需要的角位移信号，为接下来的控制提供所必须的实时信号。图4.7为SubSystem

40、的程序框图，包括LQR控制器和控制器切换部分。图4.8为程序保护模块程序框图，主要的目的为保证设备和人员的安全，当控制器设计或是硬件信号出现问题时，除了采用急停开关，通过程序保护模块在软件商实现保护。图 4.5 下平衡Simulink程序框图图 4.6 Subsystem3子程序模块框图（Subsystem2与其结构相同，不再赘述）图 4.7 SubSystem的程序框图图 4.8下平衡程序保护模块程序将程序下载至控制器，运行程序，然后将Pendubot扶至平衡点附近，按下驱动开关。观察Pendubot的平衡情况，然后施加扰动，分别记录下正常运行和施加扰动时的波形。如图4.9和图4.10。a

41、正常运行时q1的曲线 b 正常运行时q2的曲线图 4.9 a 突加扰动后q1的曲线 b 突加扰动后q2的曲线图 4.10 由图像可见，此时系统在下平衡点的稳定性和抗扰动性都很理想。4.3.2 上平衡点平衡控制上平衡simulink程序如图4.11所示，使用。图 4.11 上平衡Simulink程序框图将程序下载至控制器，运行程序，然后将Pendubot扶至平衡点附近，按下驱动开关。观察Pendubot的平衡情况，如图4.12。 a 正常运行时q1的曲线 b 正常运行时q2的曲线图 4.12 4.4 小结本章用基于LQR的控制策略实现了Pendubot的上下平衡点控制，并给出实际及仿真的结果。本

42、章设计了Pendubot的平衡控制器：首先将Pendubot的非线性运动方程线性化，然后设计一个具有线性模型的LQR控制器。在实物控制中，发现初始状态对控制效果有很大影响。若初始状态与平衡点偏离较大，控制效果会比较差，甚至无法控制。此外，控制器参数对控制效果也有一定影响，实际中作者采用试错法对参数进行选定，最终选择控制效果最好的参数。第五章 Kalman滤波器设计5.1 Kalman滤波器基本形式卡尔曼滤波器是一种线性最小方差估计，因此应从它对线性系统的滤波作用开始进行理论分析。引入一个离散控制系统的控制模型 ：(5.1)定义系统的观测变量为z，得到测量方程为：(5.2)其中，表示时刻的状态向

43、量，表示时刻的输入向量，表示时刻的观测向量，分别为过程激励噪声和观测噪声，它们为相互独立、正态分布的白噪声，协方差分别是Q，R（这里假设它们不随系统状态变化而变化），则有卡尔曼滤波器的一般计算步骤如下：状态一步预测：(5.3)新息序列:(5.4)一步预测误差方差矩阵:(5.5)更新估计滤波方差矩阵：(5.6)滤波增益矩阵:(5.7)以观测值更新状态估计：(5.8)滤波初值：(5.9)值得注意的是，系统的初始状态是某种已知分布的随机向量，其期望为，方差为。并且系统噪声序列和观测噪声序列、都与不相关，即:(5.10)(5.11)从方程的形式可以看出，卡尔曼滤波器在一周期内的计算过程可以分为两步骤:

44、时间更新与测量更新。时间更新包括对系统状态的提前预测与对均方误差矩阵的提前预测。测量更新包括计算滤波增益、更新系统状态的估计值和更新均方误差。所以这两个步骤又可称为预测与校正。这样反复循环，从1时刻到k时刻，便能充分合理的利用观测值Z。据此可画出卡尔曼滤波运算过程如图5.1所示。图 5.1 卡尔曼滤波器运算过程图5.2 Kalman滤波器设计根据以上的推导的公式，可以对本二级倒立摆系统设计出卡尔曼滤波器。为了便于实际设计与应用，决定采用离散卡尔曼滤波器对倒立摆信号进行处理。由于pendubot模型方程是连续的形式，所以需要对其进行离散化。重写上述推导的倒立摆状态空间方程如下:(5.12)其中

45、要设计出完整的滤波器，还需要确定几个滤波参数与迭代初值。具体数值确定如下:R的值是根据系统噪声的特点选取的，因为主要的量测噪声来源于摆角信号，所以只有角度信号和角速度信号对应的方差有值。又因为系统的噪声干扰主要来源于量测噪声，因此忽略系统噪声，即Q=0。5.3 Kalman滤波器仿真实验与分析人为的在模型中加入噪声，为了实现二级倒立摆稳摆控制的目的，系统控制方案如图5.2所示。选用LQR控制器，该控制器的输出即控制量u作为指令送达电机，同时u也作为卡尔曼滤波器的一个输入。稳态卡尔曼滤波器的另一输入信号则是混入了大量测量噪音的倒立摆状态变量.该滤波器以此二者一为输入，经过系列运算后，将滤波后的状

46、态变量送给LQR控制器，作为稳摆控制器的决策依据。此控制方案充分考虑了过程噪音和测量噪音对系统性能的影响，在没有增加任何硬件成本的基础上，可以有效提高系统的控制效果。图 5.2 倒立摆前置卡尔曼滤波的LQR 控制过程使用SIMULINK搭建卡尔曼滤波器模块，其结构图与算法图如图5.3、图5.4所示。图 5.3 Kalman滤波结构图图 5.4 Kalman滤波算法框图此外，由于Kalman滤波算法需要一段时间的训练，因此需要设计一个训练切换开关，在训练一段事件后，观测信号由带噪声的原信号切换至经kalman滤波后的观测信号。仿真实验以下平衡点为例（上平衡点与下平衡点相同），仿真实验中，训练切换

47、开关定时为1s，仿真结果如图5.5，图5.6所示。从对比图中可以看出，稳态片尔曼滤波器有效滤除了噪音的影响，获取了系统运动的真实状态。并且在1s后，控制器输出量震荡明显的减小，从而减少了电机的震动与损耗，取得了更好的控制效果。图 5.5 滤波前后q1,q2的对比图 5.6 控制器输出信号5.4 小结在实际工作条件下，机械存在着噪声干扰。本章已二级倒立摆为被控对象，进行噪声抑制控制，考虑到卡尔曼滤波器这种经典的状态估计方法相对于传统的滤波方法的诸多优势，因此，本系统引入了卡尔曼滤波器对信号进行滤波处理。设计了针对本二级倒立摆系统的卡尔曼滤波器。通过仿真实验证明，卡尔曼滤波器可以在不增加任何成本的

48、前提下，有效地滤除摆角信号中的噪声，实现了控制器对观测信号含有噪声的系统的稳定控制。在实际的使用过程中发现，控制器模型参数不精确对于卡尔曼滤波器的滤波效果的影响非常大；并且卡尔曼滤波器需要一小段时间进行训练，训练时间与采样时间和初始参数等因素有关。第六章 结论与展望东北大学本科毕业设计（论文）Error! Reference source not found. Error! Reference source not found.本论文以东北设计制作的二级倒立摆实物仿真控制系统为研究对象，研究了如何在电位器测量摆角且含有大噪声的情况下实现对二级倒立摆的稳定控制。现将本论文的主要工作总结如下:（1

49、） 基于拉格朗日方程建立了pendubot的数学模型，为控制器和滤波器的设计奠定基础。本文首先对pendubot系统利用拉格朗日方程进行了数学建模。然后，提出了利用物理学守恒定律建模的方法，此种方法推导过程简单直观，且能够揭示出数学模型的物理含义。（2） 测定Pendubot的实际参数。本论文利用基于能量的系统参数在线测定法及CAD模型法两种方法进行了Pendubot系统的参数测定。根据基于能量的系统参数在线测定法和CAD模型法得出的Pendubot参数结果相差不大，这意味着本文所提出的两种参数辨识方法有效。本文在控制器设计中选用基于能量的系统参数在线测定法得到的参数。（3） 设计了pendu

50、bot平衡控制器。本文中Pendubot的平衡控制器采用LQR方法设计，首先将Pendubot的非线性运动方程线性化，同时设计一个具有线性模型的全状态反馈控制器。并通过一些列仿真实验和实物实验证明了该控制器的有效性。（4） 基于卡尔曼滤波器对电位器摆角信号进行检测，实现了对pendubot系统摆角信号较为准确的估计。引入了卡尔曼滤波器对含有噪声摆角信号进行滤波处理，设计了针对本pendubot系统的卡尔曼滤波器。通过仿真实验证明，卡尔曼滤波器可以有效地滤除摆角信号中的噪声，实现了控制器对观测信号含有噪声的系统的稳定控制。总体来说，本文在上述方面取得了一些成果，但是在研究过程中还有一些问题只得进

51、一步研究，其中包括：（1） 本文的控制策略没有鲁棒性分析，今后需考虑Pendubot系统的鲁棒性分析及设计问题，确保鲁棒控制策略对模型不确定性与外界扰动具有鲁棒性。（2） 卡尔曼滤波（KF）对模型精准度要求较高，一些对模型要求较低的新算法是进一步的研究方向。参考文献1. E. Bryson. Jr., David G Luenberger. The Synthesis of Regulator Logic Using State-Variable Concepts. Proceedings of the IEEE. 1970,58(11):1803-18112. Hooshang Hemami

52、, Franck C. Weimer, Said H. Koozekanani. Some Aspects of the Inverted Pendulum Problem for Modeling of Locomotion Systems. IEEE Transactions on Automatic Control. 1973,12:6586613. 邓自立，张焕水.自校正Kalman滤波、预报、去卷、平滑新方法。控制理论与应用。1994,11(2):1371454. R.E.Kalman.On the Stability of Time-Varying Linear Systems.

53、IRE Transactions on Circuit Theory. 1962,December:4204225. Luenberger D. An introduction to observersJ. IEEE Transactions on automatic control 1971 (6): 596-6026. 高媛.最优和自校正多传感器观测融合滤波方法和算法研究D.哈尔滨:黑龙大学，20107. 张爱军.水下潜器组合导航定位及数据融合技术研究D.南京:南京理工大学，20098. Kalman R. A new approach to linear filtering and prediction problemsJ. Journal of basicEngineering, 1960 (1): 35-459. 潘泉，杨峰，叶亮，梁彦，程咏梅.一类非线性滤波器UKF综述J.控制与决策, 2005 (OS): 481-489,49410. Julier S., Uhlmann J. Unscented filtering and nonlinear estimationJ. Proceedings of the IEEE, 2004 (3): 401-42211. 程水英.无味变换与无味卡尔曼滤波J.计算机工程与应用，2008 (24): 25-35致谢