第八章卡尔曼滤波与LQG控制
lqg控制原理
lqg控制原理
LQG（线性二次估计控制器）控制是一种适用于线性系统的控制方法。
它包含了两个部分：LQR（线性二次调节器）和Kalman滤波器。
其中，LQR用于控制器的设计，Kalman滤波器用于估计状态（比如机器人的位置、速度等）。
LQR是一种基于状态反馈的控制器设计方法，通过优化一个二次代价
函数来获取最优的反馈增益矩阵，从而实现控制器的设计。
而Kalman滤
波器则是一种最小均方误差的估计方法，通过把状态估计误差最小化的方
式来估计系统状态。
LQG控制器结合了这两种方法，使用Kalman滤波器来估计系统状态，并使用LQR来控制系统。
LQG控制器能够在状态估计误差存在的情况下
实现系统的最优控制，因此在机器人控制、飞行器控制、汽车控制等领域
被广泛应用。
卡尔曼滤波算法原理
卡尔曼滤波算法原理卡尔曼滤波(Kalman Filter)是一种用来估计系统状态的算法。
它基于对系统的数学模型和测量数据进行分析，通过使用贝叶斯统计推断来计算系统当前的最优状态估计。
卡尔曼滤波算法在控制系统、导航系统、机器人学、图像处理等领域有广泛的应用。
卡尔曼滤波算法的原理可以概括为以下几步：1. 系统建模：首先，需要建立系统的数学模型，包括系统的动态方程和观测方程。
动态方程描述了系统状态的演化规律，而观测方程则描述了系统状态与测量值之间的关系。
这些方程通常以线性高斯模型表示，即系统的状态和测量误差符合高斯分布。
2. 初始化：在开始使用卡尔曼滤波算法之前，需要对系统状态进行初始化。
这包括初始化系统状态的均值和协方差矩阵。
通常情况下，均值可以通过先验知识来估计，而协方差矩阵可以设置为一个较大的值，表示对系统状态的初始不确定性较大。
3. 预测：在每一次测量之前，需要对系统的状态进行预测。
预测过程基于系统的动态方程，将上一时刻的状态估计作为输入，得到当前时刻的状态的先验估计。
预测的结果是一个高斯分布，其均值和协方差矩阵表示了对当前状态估计的不确定性。
4. 测量更新：当获取了新的测量值时，需要将其与预测结果进行比较，以修正对系统状态的估计。
测量更新过程基于系统的观测方程，将预测的状态估计与实际的测量值进行比较，得到对系统状态的最优估计。
测量更新的结果也是一个高斯分布，其均值和协方差矩阵表示了对当前状态估计的不确定性。
5. 迭代：在每一次测量更新之后，会得到对系统状态的最优估计。
然后，可以根据当前估计的状态再次进行预测，并等待下一次的测量更新。
这样，通过不断地迭代，卡尔曼滤波算法可以逐步提高对系统状态的估计精度。
卡尔曼滤波算法的核心思想是将动态方程和观测方程结合起来，使用贝叶斯推断的方法进行状态估计。
通过动态方程对系统进行预测，再通过观测方程修正预测结果，从而得到对系统状态的最优估计。
卡尔曼滤波算法在估计过程中考虑了对系统状态的不确定性，通过动态预测和测量更新不断修正对系统状态的估计结果，达到更准确的状态估计。
卡尔曼滤波的基本原理
卡尔曼滤波的基本原理1. 任务名称卡尔曼滤波的基本原理2. 引言卡尔曼滤波是一种用于估计动态系统状态的方法，它通过融合系统测量和模型预测的信息，提供对系统状态的最优估计。
该滤波器在众多领域，如导航、信号处理、机器人技术等方面得到了广泛应用。
本文将详细介绍卡尔曼滤波的基本原理及其应用。
3. 卡尔曼滤波器的算法卡尔曼滤波器的算法主要由两个步骤组成：预测步骤和更新步骤。
在预测步骤中，根据系统的动力学模型，利用上一时刻的状态估计和模型进行预测；在更新步骤中，根据测量值和预测值之间的差异，对状态进行修正。
3.1 预测步骤预测步骤中，卡尔曼滤波器通过状态转移矩阵和控制向量对上一时刻的状态估计进行预测。
预测的状态向量可由以下公式表示：x k=Fx k−1+Bu k其中，x k表示当前时刻的状态估计，x k−1表示上一时刻的状态估计，F表示状态转移矩阵，B表示控制向量，u k表示当前时刻的控制输入。
预测的协方差矩阵可由以下公式表示：P k=FP k−1F T+Q其中，P k表示当前时刻的协方差矩阵，P k−1表示上一时刻的协方差矩阵，Q表示过程噪声的协方差矩阵。
3.2 更新步骤更新步骤中，卡尔曼滤波器将测量值与预测值进行比较，通过计算卡尔曼增益，对预测的状态进行修正。
卡尔曼增益的计算公式如下所示：K k=P k H T(HP k H T+R)−1其中，K k表示卡尔曼增益，H表示测量矩阵，R表示测量噪声的协方差矩阵。
修正后的状态向量可由以下公式表示：x k=x k+K k(y k−Hx k)修正后的协方差矩阵可由以下公式表示：P k=(I−K k H)P k3.3 初始化在使用卡尔曼滤波器之前，需要对状态向量和协方差矩阵进行初始化。
通常情况下，初始状态向量和协方差矩阵可通过经验估计或历史数据进行初始化。
4. 卡尔曼滤波器的应用卡尔曼滤波器具有很广泛的应用领域，下面将介绍其中几个典型的应用。
4.1 导航在导航领域，卡尔曼滤波器常用于姿态估计、位置估计和速度估计等方面。
卡尔曼滤波简介及其算法实现代码
卡尔曼滤波简介及其算法实现代码卡尔曼滤波算法实现代码（C，C＋＋分别实现）卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣。
所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。
现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。
因为这里不能写复杂的数学公式，所以也只能形象的描述。
希望如果哪位是这方面的专家，欢迎讨论更正。
卡尔曼滤波器– Kalman Filter1．什么是卡尔曼滤波器（What is the Kalman Filter?）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。
跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。
1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。
1957年于哥伦比亚大学获得博士学位。
我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。
如果对这编论文有兴趣，可以到这里的地址下载：/~welch/media/pdf/Kalman1960.pdf。
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。
他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
2．卡尔曼滤波器的介绍（Introduction to the Kalman Filter）为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。
控制系统卡尔曼滤波
控制系统卡尔曼滤波卡尔曼滤波（Kalman filter）是一种经典的状态估计技术，在控制系统中拥有广泛应用。
本文将介绍控制系统中卡尔曼滤波的基本原理、算法流程以及应用实例。
一、卡尔曼滤波的基本原理卡尔曼滤波是基于系统状态和测量数据之间的线性关系，通过递推的方式对系统的状态进行估计。
其基本原理包括两个方面：状态预测和测量更新。
1. 状态预测状态预测是指根据系统的状态方程和上一时刻的状态估计值，通过数学模型预测当前时刻的系统状态。
状态方程通常用线性动力学方程表示，可以描述系统在无外界干扰下的状态演化规律。
2. 测量更新测量更新是指根据系统的测量方程和当前时刻的测量数据，对系统的状态进行修正和更新。
测量方程通常用线性观测方程表示，可以将系统的状态转化为可观测的输出。
二、卡尔曼滤波的算法流程卡尔曼滤波的算法流程主要包括两个步骤：预测步骤和更新步骤。
1. 预测步骤在预测步骤中，通过系统状态方程和控制输入预测系统的状态。
预测的过程包括两个关键的计算：（1）状态预测：根据上一时刻的状态估计值和状态方程，计算当前时刻的状态预测值。
（2）状态协方差预测：根据上一时刻的状态协方差估计值、过程噪声协方差以及状态转移矩阵，计算当前时刻的状态协方差预测值。
2. 更新步骤在更新步骤中，通过测量方程和测量数据来修正和更新系统的状态。
更新的过程包括两个关键的计算：（1）卡尔曼增益计算：根据状态协方差预测值、测量噪声协方差以及测量矩阵，计算卡尔曼增益。
（2）状态估计更新：根据卡尔曼增益、状态预测值和测量残差，计算当前时刻的状态估计值和状态协方差估计值。
三、卡尔曼滤波的应用实例卡尔曼滤波在控制系统中具有广泛的应用，下面将通过一个实际的应用实例来说明其效果。
假设有一个飞行器，通过激光雷达测量距离来估计飞行器与目标之间的距离。
然而，由于环境噪声和测量误差的存在，测量数据会受到一定程度的扰动。
在这个实例中，我们可以使用卡尔曼滤波来对飞行器与目标之间的距离进行估计。
8联邦滤波
第8章联邦滤波和自适应滤波第8章联邦滤波和自适应滤波在组合导航中的应用8.1联邦卡尔曼滤波组合导航系统可提高系统的任务可靠性和容错性能。
因为组合导航中有余度的导航信息，如组合适当，则可利用余度信息检测出某导航子系统的故障，将此失效的子系统隔离掉，并将剩下的正常的子系统重新组合（系统重构），就可继续完成导航任务。
组合导航系统还可协助惯导系统进行空中对正和校准，从而提高飞机或其他载体的快速反应能力。
联邦卡尔曼滤波理论是美国学者 Carlson于1998年提出的一种特殊形式的分布式卡尔曼滤波方法。
它由若干个子滤波器和一个主滤波器组成，是一个具有分块估计、两步级联的分散化滤波方法，关键在于它采用信息分配原理。
它需要向各子滤波器分配动态信息，这些信息包括两大类：状态方程的信息和观测方程的信息。
8.1.1联邦卡尔曼滤波器结构运动方程的信息量与状态方程中过程噪声的协方差阵成反比，过程噪声越弱，状态方程就越精确。
因此，状态方程的信息量可以用过程噪声协方差阵的逆 Q1来表示。
此外，状态估计的信息量可用状态估计协方差阵的逆P1表示，测量方程的信息量可用测量噪声协方差阵的逆 R1表示。
如果把局部滤波器i的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为召、Q i、R,i 1,2, ,n，主滤波器的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为）?m、Q m、P m,假定按以下规则将整体信息分配至各局部滤波器，即P1）? p1 1）?1 p21）?2叮冷PrJX m 斤刃（8.1）Q 1Q11Q21Q n1Q m1Q i1i Q 1（ 8.2）P 1P11P21P n1P m1P i 1i P 1（ 8.3）其中，i是信息分配系数，必须满足下列条件:在设计联邦卡尔曼滤波器时，信息分配系数的确定至关重要，不同的值会有不同的滤波器结构和特性（容错性、最优性、计算量等）。
令i 1/ i （i 1,2……N,m ）,则它的的几种主要结构可简要地表达如下：（1）第一类结构（m i =1/ （ N + D ，有重置），如图8.1所示图8.1 联邦滤波器第一类结构这类结构的特点是：信息在主滤波器和各子滤波器之间平均分配。
卡尔曼滤波原理
卡尔曼滤波原理卡尔曼滤波（Kalman Filtering）是一种用于估计、预测和控制的最优滤波方法，由美国籍匈牙利裔数学家卡尔曼（Rudolf E. Kalman）在1960年提出。
卡尔曼滤波是一种递归滤波算法，通过对测量数据和系统模型的融合，可以得到更准确、更可靠的估计结果。
在各种应用领域，如导航、机器人、航空航天、金融等，卡尔曼滤波都被广泛应用。
1. 卡尔曼滤波的基本原理卡尔曼滤波的基本原理是基于状态空间模型，将系统的状态用随机变量来表示。
它假设系统的状态满足线性高斯模型，并通过线性动态方程和线性测量方程描述系统的演化过程和测量过程。
具体而言，卡尔曼滤波算法基于以下两个基本步骤进行：1.1 预测步骤：通过系统的动态方程预测当前时刻的状态，并计算预测的状态协方差矩阵。
预测步骤主要是利用前一时刻的状态和控制输入来预测当前时刻的状态。
1.2 更新步骤：通过系统的测量方程，将预测的状态与实际测量值进行融合，得到最优估计的状态和状态协方差矩阵。
更新步骤主要是利用当前时刻的测量值来修正预测的状态。
通过不断迭代进行预测和更新，可以得到连续时间上的状态估计值，并获得最优的估计结果。
2. 卡尔曼滤波的优势卡尔曼滤波具有以下几个优势：2.1 适用于线性系统与高斯噪声：卡尔曼滤波是一种基于线性高斯模型的滤波方法，对于满足这些条件的系统，卡尔曼滤波能够给出最优的估计结果。
2.2 递归计算：卡尔曼滤波是一种递归滤波算法，可以在每个时刻根据当前的测量值和先前的估计结果进行迭代计算，不需要保存过多的历史数据。
2.3 最优性：卡尔曼滤波可以通过最小均方误差准则，给出能够最优估计系统状态的解。
2.4 实时性：由于卡尔曼滤波的递归计算特性，它可以实时地处理数据，并及时根据新的测量值进行估计。
3. 卡尔曼滤波的应用卡尔曼滤波在多个领域都有广泛的应用，以下是一些典型的应用例子：3.1 导航系统：卡尔曼滤波可以用于导航系统中的位置和速度估计，可以结合地面测量值和惯性测量传感器的数据，提供精确的导航信息。
基于Kalman_滤波和LQG_控制的起重机控制研究
第40卷第3期Vol.40㊀No.3重庆工商大学学报(自然科学版)J Chongqing Technol &Business Univ(Nat Sci Ed)2023年6月Jun.2023基于Kalman 滤波和LQG 控制的起重机控制研究魏炳胜,许安定上海理工大学机械工程学院,上海200093摘㊀要:针对双载荷轨道起重机在抬升重物时的消摆时间长㊁定位速度慢的问题,提出采用最优控制理论和卡尔曼滤波算法,设计线性高斯二次型(Linear Quadratic Gaussian ,LQG )控制器并对其进行残余振动抑制㊂首先,运用Lagrange 方程建立起重机系统的非线性动力学方程,其次,运用雅可比矩阵将非线性方程线性化,得到系统的状态空间方程;通过证明线性系统的可控性和可观性,选择合适的权函数,设计最优二次型线性调节器(Linear Quadratic Regulator ,LQR )和最优状态估计器来得到LQG 控制器,通过建立相关模型的Simulink 控制系统仿真表明:设计的LQG 控制器能有效衰减残余振动;通过与PID 控制器对比分析,所提出的LQG 控制器能更有效衰减残余振动幅值,振动的超调量更小,所需的稳态时间更短,验证了该控制策略的有效性和可行性㊂关键词:起重机;振动抑制;LQR 控制;卡尔曼滤波;LQG 控制中图分类号:TP273㊀㊀文献标识码:A ㊀㊀doi:10.16055/j.issn.1672-058X.2023.0003.003㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀㊀收稿日期:2022-01-17㊀修回日期:2022-03-15㊀文章编号:1672-058X(2023)03-0020-07作者简介:魏炳胜(1998 ),男,江苏泰州人,硕士研究生,从事机器人控制研究.引用格式:魏炳胜,许安定.基于Kalman 滤波和LQG 控制的起重机控制研究[J].重庆工商大学学报(自然科学版),2023,40(3):20 26.WEI Bingsheng XU Anding.Research on crane control based on Kalman filter and LQG control J .Journal of ChongqingTechnology and Business University Natural Science Edition 2023 40 3 20 26.Research on Crane Control Based on Kalman Filter and LQG Control WEI Bingsheng XU AndingSchool of Mechanical Engineering University of Shanghai for Science and Technology Shanghai 200093 ChinaAbstract Aiming at the problems of long swing elimination time and slow positioning speed when lifting heavy objects a Linear Quadratic Gaussian LQG controller based on optimal control theory and Kalman filter algorithm was proposed to suppress residual vibration.Firstly the Lagrange equation was used to establish the nonlinear dynamic equation of the crane system.Secondly the Jacobian matrix was used to linearize the nonlinear equation and the state space equation of the system was obtained.By proving the controllability and observability of the linear system selecting the appropriate weight function and designing the optimal Linear Quadratic Regulator LQR and the optimal state estimator the LQG controller was obtained.The simulation of Simulink control system with relevant models shows that the designed LQG controller can effectively attenuate the residual pared with PID controller the proposed LQG controller can attenuate the residual vibration amplitude more effectively the overshoot of vibration is smaller and the required steady-state time is shorter.The effectiveness and feasibility of the control strategy were verified.Keywords crane vibration suppression LQR control Kalman filtering LQG control1㊀引㊀言近年来,起重机广泛应用于综合施工平台上㊂起重机作业依赖于吊车司机的操作经验,操作不当㊁加减速运动及外界干扰等都能造成负载扰动㊂这不仅降低了工作效率,还引起了安全隐患㊂由于起重机特殊的使用环境,其在运行过程中的稳定性和定位准确性是起重机作业的重要因素[1-3]㊂如何抑制起重机作业过程中的振动问题,是国内外学者的研究热点[4-5]㊂传统起重机大多数使用双载荷搬运货物,这能提高工作效率,但避免不了振动问题㊂目前,大多数学者第3期魏炳胜,等:基于Kalman滤波和LQG控制的起重机控制研究针对起重机振动问题采用了一些行之有效的控制策略㊂传统的末端定位方法采用PID控制㊁模糊控制和滑模控制㊂陈鹤等[6]充分考虑系统的物理约束和控制性能指标,提出了带有状态约束的吊车最优轨迹规划方法㊂开环控制方法虽然易于实现,但对参数不确定性和外界干扰十分敏感,设计出的控制器鲁棒性较差㊂针对这一现象,MAR等[7]提出了输入整形和PID控制相结合的方案,克服了输入整形器抗干扰能力差的局限;SUN等[8]设计了一种饱和非线性输出反馈控制器和一种非线性PID控制器,有效抑制了起重机振动定位问题,然而此闭环控制方法需要利用所有系统状态的实时反馈信息进行控制,在实际中难以实现;文献[9]基于模糊PID控制进行起重机端部定位控制,达到闭环效果,但结果证明所需的稳态时间较长;吴朝霞等[10]针对起重机提出了滑模控制,但滑模控制避免不了力矩的抖振效果㊂综合以上研究成果,本文基于起重机模型简化图运用Lagrange方程进行动力学建模,然后利用雅可比矩阵[11]将非线性系统线性化,再证明系统的可控性和可观性,选择合适的加权函数,设计最优二次型线性调节器和最优状态估计器来得到LQG控制器㊂在Matlab中的simulink中建立相关模型,在给定系统阶跃信号后,仿真结果表明:LQG控制器比PID控制器能更有效地抑制残余振动,超调量更小,稳态时间更短,验证了该控制策略的有效性和可行性㊂2㊀系统描述起重机模型简化图如图1所示㊂该系统由一台仅在一维轨道上移动的起重机组成,起重机的总质量为M,且其在轨道上移动时不存在摩擦力;F为系统的外界输出力;此外,有两个负载悬挂在连接至起重机的电缆上,载荷的质量分别为m1和m2,电缆的长度分别为l1和l2㊂xFMl 2l1θ1θ2m2m1图1㊀起重机模型简化图Fig.1㊀Simplified diagram of crane model为方便研究,本文对双载荷起重机模型做出以下假设:起重机与轨道之间无摩擦;所有质量都具有均匀的密度;起重机移动只有一个自由度;每个物体的质心位于其几何中心;系统在2D中建模,不考虑杆的质量和厚度;起重机的重心位于悬挂点处㊂3㊀系统的设计与分析3.1㊀模型定义首先将起重机和两个载荷的动能和势能表述出来,再运用Lagrange方程建立起重机的动力学方程㊂其中r1㊁r2㊁r㊃1㊁r㊃2分别表示载荷m1㊁m2的位置和速度, i㊁j均为虚数单位㊂对于载荷质量m1,有r1(t)=(x-l1sinθ1)i+(l1cosθ1)jr㊃1(t)=(x㊃-l1θ㊃1cosθ1)i+(-l1θ㊃1sinθ1)jm1动能为T1=KE=12m(x㊃21+y㊃21)=12m[(x㊃-l1θ㊃1cosθ1)2+(l1θ㊃1sinθ1)2]m1势能为V1=PE1=-m1gl1cosθ1对于载荷质量m2,有r2(t)=(x-l2sinθ2)i+(l2cosθ2)jr㊃2(t)=(x㊃-l2θ㊃2cosθ2)i+(-l2θ㊃2sinθ2)jm2动能为T2=KE=12m(x㊃21+y㊃21)=12m[(x㊃-l2θ㊃2cosθ2)2+(l2θ㊃2sinθ2)2]m2势能为V2=PE2=-m2gl2cosθ2对于起重机M,M的动能为T3=KE=12Mx㊃2M的势能为V3=PE3=0系统的总能量由下式给出:L=(T1+T2+T3)-(V1+V2+V3)=12[Mx㊃2+m1(x㊃-l1θ㊃1cosθ1)2+m1(-l1θ㊃1sinθ1)2+m2(x㊃-l2θ㊃2cosθ2)2+m2(-l2θ㊃2sinθ2)2]-[-m1gl1cosθ1-m2gl2cosθ2]由Lagrange方程可得:12重庆工商大学学报(自然科学版)第40卷d d t ∂L ∂x ㊃-∂L∂x=F (1)d d t ∂L ∂θ㊃1-∂L∂θ1=0(2)d d t ∂L ∂θ㊃2-∂L∂θ2=0(3)其中,∂L∂x㊃=Mx ㊆+m 1(x ㊃-l 1θ㊃1cos θ1)+m 2(x ㊃-l 2θ㊃2cos θ2)d d t ∂L ∂x㊆=Mx ㊆+m 1(x ㊆-(l 1θ㊆1-l 1θ㊃21sin θ1)+m 2(x ㊆-(l 2θ㊆2-l 2θ㊃22sin θ2))(4)∂L ∂x=0(5)将式(4)㊁式(5)代入式(1),得出关于x 的表达式:F =x ㊆(M +m 1+m 2)-m 1l 1θ㊆1cos θ1-m 2l 2θ㊆2cos θ2+m 1l 1θ㊃21sin θ1+m 2l 2θ㊃22sin θ2(6)对于θ1,有∂L∂θ㊃1=m 1l 21θ㊃1-m 1x ㊃l 1cos θ1(7)d d t ∂L ∂θ㊃1=m 1l 21θ㊆1-m 1x ㊆l 1cos θ1+m 1x ㊃l 1θ㊃1sin θ1∂L ∂θ1=m 1x ㊃l 1θ㊃1sin θ1-m 1gl 1sin θ1(8)将式(7)㊁式(8)代入式(2),得出关于θ1的表达式:0=m 1l 21θ㊆21-m 1x ㊆l 1cos θ1+m 1x ㊃l 1θ㊃1sin θ1-m 1x ㊃l 1θ㊃1sin θ1+m 1gl 1sin θ1θ㊆1=x ㊆cos θ1l 1-g sin θ1l 1(9)由式(3)可得出关于θ2的表达式:θ㊆2=x ㊆cos θ2l 2-g sin θ2l 2(10)将式(9)㊁式(10)代入式(6)得:x㊆=㊀F -m 1g sin θ1cos θ1-m 2g sin θ2cos θ2-m 1l 1θ㊃21sin θ1-m 2l 2θ㊃22sin θ2M +m 1sin θ21+m 2sin θ223.2㊀线性化为方便控制律的设计,本文需要在设计控制器之前,对非线性系统进行线性化设计㊂对于起重机模型,系统的非线性状态空间表达式为X (t ㊃)=F (X ,U )Y (t )=H (X ,U )其中,Y 是状态X 和输入U 的函数㊂对于给定的起重机系统,由上述所得的非线性动力学方程可得其状态空间如下所示:x ㊃x ㊆θ㊃1θ㊆1θ㊃2θ㊆2éëêêêêêêêêêêùûúúúúúúúúúú=x ㊃F -m 1g sin θ1cos θ1-m 2g sin θ2cos θ2-m 1l 1θ㊃21sin θ1-m 2l 2θ㊃22sin θ2M +m 1sin θ21+m 2sin θ22θ㊃1x ㊆cos θ1l 1-g sin θ1l 1θ㊃2x ㊆cos θ2l 2-g sin θ2l 2éëêêêêêêêêêêêêêêêùûúúúúúúúúúúúúúúú(11)对于非线性系统,一般可以采用小角度线性化和雅可比线性化㊂本文将采用雅可比进行线性化:A F =∇X F (X (t ),U (t ))B F =∇U F (X (t ),U (t ))C H =∇X H (X (t ),U (t ))D H =∇U H (X (t ),U (t ))平衡点处的雅可比矩阵为(X 0,θ10,θ20)=(0,0,0)相应的一阶导数为(X ㊃,θ㊃1,θ㊃2)=(0,0,0)由此可得:A F =∂f 1∂x 1∂f 1∂x 2∂f 1∂x 3∂f 1∂x 4∂f 1∂x 5∂f 1∂x 6∂f 2∂x 1∂f 2∂x 2∂f 2∂x 3∂f 2∂x 4∂f 2∂x 5∂f 2∂x 6∂f 3∂x 1∂f 3∂x 2∂f 3∂x 3∂f 3∂x 4∂f 3∂x 5∂f 3∂x 6∂f 4∂x 1∂f 4∂x 2∂f 4∂x 3∂f 4∂x 4∂f 4∂x 5∂f 4∂x 6∂f 5∂x 1∂f 5∂x 2∂f 5∂x 3∂f 5∂x 4∂f 5∂x 5∂f 5∂x 6∂f 6∂x 1∂f 6∂x 2∂f 6∂x 3∂f 6∂x 4∂f 6∂x 5∂f 6∂x 6éëêêêêêêêêêêêêêêêêêêêùûúúúúúúúúúúúúúúúúúúú其中,f 1㊁f 2㊁f 3㊁f 4㊁f 5㊁f 6为等式(11)右侧向量的各个元素㊂令输入U =F ,则B F 为B F =∂f 1∂U∂f 2∂U ∂f 3∂U ∂f 4∂U ∂f 5∂U ∂f 6∂U éëêêùûúúT22第3期魏炳胜,等:基于Kalman 滤波和LQG 控制的起重机控制研究通过计算各个元素,得到以下矩阵:A F =01000000-(m 1g )M 0-(m 2g )M 000010000-((m 1+M )g )Ml 10-(m 2g )Ml 1000000100-(m 1g )Ml 2-((m 2+M )g )Ml 20éëêêêêêêêêêêêêêùûúúúúúúúúúúúúúB F =01M 01Ml 101Ml 2éëêêùûúúTC =1000000010*******0éëêêêùûúúúD =00000[]T线性状态空间方程可表示为X ㊃=A F X +B F U Y =CX +DU其中,X =[x ㊃x ㊆θ㊃1θ㊆1θ㊃2θ㊆2]T,输入为U =F ㊂因此,给定系统状态空间为x ㊃x ㊆θ㊃1θ㊆1θ㊃2θ㊆2éëêêêêêêêêêêùûúúúúúúúúúú=0100000-(m 1g )M 0-(m 2g )M000010000-((m 1+M )g )M l 10-(m 2g )M l 1000000100-(m 1g )M l 20-((m 2+M )g )M l 20éëêêêêêêêêêêêêêùûúúúúúúúúúúúúúx x ㊃θ1θ㊃1θ2θ㊃2éëêêêêêêêêêêùûúúúúúúúúúú+01M 01M l 101M l 2éëêêêêêêêêêêêêêùûúúúúúúúúúúúúúF 4㊀可控性和可观性判断线性系统的可控性可由下列矩阵求出:C =BABA 2BA 3BA 4BA 5B []其中,A ,B 是根据线性化系统A F ㊁B F 计算的矩阵,其中C 中的各个矩阵由下式给出:B =01/M 01/(Ml 1)01/(Ml 2)éëêêêêêêêêùûúúúúúúúú㊀㊀AB =1/M 01/(Ml 1)01/(Ml 2)0éëêêêêêêêêùûúúúúúúúúA 2B =0-(gm 1)/(M 2l 1)-(gm 2)/(M 2l 2)0-(g (M +m 1))/(M 2l 21)-(gm 2)/(M 2l 1l 2)0-(g (M +m 2))/(M 2l 22)-(gm 1)/(M 2l 1l 2)éëêêêêêêêêêùûúúúúúúúúúA 3B =-(gm 1)/(M 2l 1)-(gm 2)/(M 2l 2)0-(g (M +m 1))/(M 2l 21)-(gm 2)/(M 2l 1l 2)0-(g (M +m 2))/(M 2l 22)-(gm 1)/(M 2l 1l 2)0éëêêêêêêêêêùûúúúúúúúúú起重机各个参数大小如表1所示㊂表1㊀起重机参数Table 1㊀Crane parameters参㊀数大㊀小起重机质量(M /kg )1000载荷质量(m 1/kg )100载荷质量(m 2/kg )100电缆长度(l 1/m )20电缆长度(l 2/m )10由表内参数,可算出矩阵C 为C =1.00000-0.147000.14171.00000-0.147000.1417000.05000-0.031900.02270.05000-0.031900.0227000.10000-0.112700.12460.10000-0.112700.12460éëêêêêêêêêêùûúúúúúúúúú根据可控性判据: A |Im B ⓪:=ðni =1Im A i -1B ()=R n ㊂上述的可控性矩阵C 的秩为6,是行满秩的,符合可控性判据[12]㊂由此可得线性化系统是可控的㊂线性系统的可观测性可由下列矩阵求出:O =CCACA 2CA 3CA 4CA 5[]T其中,A 是根据线性化系统A F 计算的矩阵㊂对于x (t )㊁θ1(t )㊁θ2(t )的输出向量集,C 可表示为C =1000000010000010éëêêêêùûúúúú32重庆工商大学学报(自然科学版)第40卷根据可观性判据:rank CCACA n -1[]T=n上述的可观性矩阵O 的秩为6,是列满秩的,符合可观性判据㊂由此可知线性化系统是可观的㊂4㊀LQG 控制器设计4.1㊀LQR 控制器起重机线性状态方程形为X ㊃=AX +BU其最优二次线性调节器(LQR)的设计属于无限时间状态调节器问题,性能指标可表示为J =12ʏɕ[X T QX +U T RU ]d t其中,Q 和R 分别为正定的相关状态变量权矩阵和正定的控制输入量权矩阵㊂要求确定最优控制U ∗(t ),使性能指标J 极小㊂由性能指标求得的最优闭环系统:X ㊃=A -BR -1B TP -()XP -满足Riccati 代数方程P -A +A T P --P -BR -1B T ㊁P -+Q =0,起重机的控制输入为U =-kX ,其中,k =R -1B T P -㊂4.2㊀卡尔曼滤波器根据系统的可观性可设计一个最优状态观测器,对于起重机系统:x ㊃=Ax +Bu +Gwy =Cx +Du +Hw +v其中,w 是过程噪声,v 是观测噪声,G ㊁H 是常系数矩阵,用来描述x 中的各部分受到的噪声强度不一致的情况㊂假设噪声w 和v 符合高斯分布,且由协方差矩阵可知:E (ww T )=Q k ,E (vv T )=R k ,E (wv T )=N k 对于给定的观测器动态和代价函数:x ︿㊃=Ax ︿+Bu +L (y -Cx ︿-Du )y ︿=x︿J -=lim t ңɕE (x -x ︿{}x -x ︿{}T )要求确定一个常系数矩阵L ,使得J -最小㊂L =(PC T +N -)R --1R -=R k +HN k +N T k HT+HQ k H T N -=G (Q k H T +N k )同时P 满足以下Riccati 代数方程A T P +PA -(PB +N k )R -1k (B T P +N T k )+Q k=0以上设计的最优状态观测器L ,就是LQG 框架的卡尔曼滤波器㊂4.3㊀LQG 控制器LQG 控制器通过组合一个最优二次型线性调节器LQR 和一个最优状态估计器(卡尔曼滤波器)得到的控制器㊂LQG 控制器框图如图2所示㊂图2中w 是过程噪声,v 是观测噪声㊂yuw uv y卡尔曼滤波器L Q R 控制器L Q G 控制器图2㊀LQG 控制框图Fig.2㊀Block diagram of LQG control5㊀仿真与对比研究在Matlab /Simulink 中建立相关模型的控制系统,经过计算得出LQR 控制器的参数矩阵为k =311321257881256626[]根据系统稳定性判据[13],求出A -Bk 的特征值为λ1=-0.7+0.7i,λ2=-0.7-0.7i λ3=-0.3+0.4i,λ4=-0.3-0.4iλ5=-0.07+0.7i,λ6=-0.07-0.7i由求得的特征值可以看出:以上特征值均在负半平面,所以设计的控制器是稳定的㊂在LQR 控制器中,给定权值Q 为Q =diag (100,100,500,500,200,200),权值R 取不同值,初始条件均为[30.50.50],系统的阶跃响应如图3和图4所示㊂5-55010015010-5θ1/r a d R :1e 2R :1e 5R :1e 8t /s图3㊀不同权值下θ1的阶跃响应Fig.3㊀θ1step response under different weights42第3期魏炳胜,等:基于Kalman 滤波和LQG 控制的起重机控制研究1-105010015010-4θ2/r a d R :1e 2R :1e 5R :1e 8t /s图4㊀不同权值下θ2的阶跃响应Fig.4㊀θ2step response under different weights从图4中可以看出:权值Q 不变,R 越小,系统到达的稳态时间越短,θ1㊁θ2的振动频率有明显减弱,且振动幅度明显减小,能达到更好的控制效果㊂设计的卡尔曼滤波器的观测误差如图5和图6所示㊂0.20.10.0-0.1-0.2-0.31020304050θ1/r a d t /s图5㊀θ1的观测误差Fig.5㊀Observation errors of θ10.20.10.0-0.1-0.21020304050θ2/r a d t /s图6㊀θ2的观测误差Fig.6㊀Observation errors of θ2从图5㊁图6可以看出:在卡尔曼滤波器下,θ1和θ2的观测误差均呈现波动状态,且θ1的观测误差幅值大约为0.2rad,θ2的观测误差幅值大约为0.1rad,均在40s 左右误差为0㊂为了比较对系统状态量的抑振效果,本文选用LQG 控制器和常见的PID [14]控制器进行对比分析㊂3个状态量的阶跃响应如图7㊁图8及图9所示㊂321001020304050?10-3x /mt /sP I D 控制L Q G 控制图7㊀系统状态量x 的阶跃响应Fig.7㊀Step response of system state quantity x20-2-40102030405010-5θ1/r a d t /sP I D 控制L Q G 控制图8㊀系统状态量θ1的阶跃响应Fig.8㊀Step response of system state quantity θ12-2102030405010-5θ2/r a d t /sP I D 控制L Q G 控制图9㊀系统状态量θ2的阶跃响应Fig.9㊀Step response of system state quantity θ2由图7可得:在给定相同阶跃信号时,在LQG 控制器下系统状态x 幅值在0.001左右,且几乎不存在超调现象,稳态时间大约在20s;而PID 控制下的系统状态x 幅值波动较大,稳态时间长,存在明显超调㊂由图8和图9可得:本文控制器下的θ1和θ2幅值均小于1ˑ10-5,而PID 控制下的θ1和θ2存在明显的振荡现象,稳态时间大约在50s,其控制效果较差㊂6㊀结论与讨论针对起重机在抬升重物时的振动及端部定位速度慢㊁稳态时间长等问题,本文提出采用基于最优控制理论和卡尔曼滤波算法,设计了线性高斯二次型控制器对其进行残余振动抑制㊂通过Matlab /Simulink 仿真验证了权值R 较小时,52重庆工商大学学报(自然科学版)第40卷所设计的控制器能保证在较短时间内到达稳态,波动幅值小,且具有稳定性㊂本文设计的卡尔曼滤波器能较好地观测系统状态量,观测误差分别为0.2和0.1左右㊂通过与PID控制器对比分析可验证:所设计的LQG控制器能更有效地衰减残余振动幅值,超调量更小,所需的稳态时间更短㊂仿真与试验结果验证了所提控制方法的有效性和可行性㊂但值得注意的是,LQG控制器在实际中容易因为建模误差和扰动的存在,导致控制器不稳定,如何提高其稳定裕度仍需要今后继续研究㊂参考文献References1 ㊀吕宇飞.基于动力学分析的核环吊运行定位研究D .大连大连理工大学2018.YU Yu-fei.Study on the operation and positioning of nuclear ring crane based on dynamic analysis D .Dalian Dalian University of Technology 2018.2 ㊀李威涛郭建胜.基于变绳长双摆模型的抓斗门座起重机吊重摆动控制研究J .起重运输机2021 18 24 28.LI Wei-tao GUO Jian-sheng.Research on the swing control of the lifting weight of the grab bucket gantry crane based on the variable rope length double pendulum model J .Lifting the Transport Machinery 2021 18 24 28.3 ㊀孙丽娟.基于输入整形技术的起重机吊具防摇控制策略研究D .兰州兰州理工大学2020.SUN Li-juan.Research on anti-sway control strategy of crane spreader based on input shaping technology D .LanzhouLanzhou University of Technology 2020.4 ㊀ZHANG Z C LI L WU Y Q.Disturbance-observer-basedantiswing control of underactuated crane systems via terminal sliding mode J .IET Control Theory&Applications 2018 1218 2588 2594.5 ㊀WU Y M SUN N CHEN H et al.Nonlinear time-optimaltrajectory planning for varying-rope-length overhead cranes J .Assembly Automation 2018 38 5 587 594.6 ㊀陈鹤方勇纯孙宁等.基于伪谱法的双摆吊车时间最优消摆轨迹规划策略 J .自动化学报2016 42 1 153 160.CHEN He FANG Yong-chun SUN Ning et al.Pseudospectral method based time optimal anti-swing trajectory planning for double pendulum crane systems J .Acta Automatica Sinica 2016 42 1 153 160.7 ㊀MAR R GOYAL A NGUYEN V et bined input shaping and feedback control for double-pendulum systems J . Mechanical Systems and Signal Processing 2017 85 267 277.8 ㊀SUN N WU Y M FANG Y C et al.Nonlinear antiswing control for crane systems with double-pendulum swing effects and uncertain parameters design and experiments J .IEEE Transactions on Automation Science and Engineering 2018 153 1413 1422.9 ㊀李帆曹旭阳.桥式起重机模糊PID防摇控制方法研究J .起重运输机械2021 18 29 35.LI Fan CAO Xu-yang.Research on fuzzy PID anti-shake control method of bridge crane J .Lifting the Transport Machinery 2021 18 29 35.10 吴朝霞孙林.基于比例微分-滑模控制的桥式起重机负载防摇控制J .起重运输机械2021 19 77 84.WU Chao-xia SUN Lin.Load anti-sway control of bridge crane based on proportional differential-sliding mode control J . Lifting the Transport Machinery 2021 19 77 84.11 常晏宁王皓.大尺寸测量场转站的参数勘定方法J .机械设计与研究2018 34 3 1 6.CHANG Yan-ning WANG Hao.Parameter determination method of large-scale measurement field transfer station J . Mechanical Design and Research 2018 34 3 1 6.12 郑大钟.线性系统理论M .2版.北京清华大学出版社2005.ZHENG Da-zhong.Linear theory M .2nd ed.Beijing Tsinghua University Publishing House 2005.13 胡寿松.自动控制原理M .7版.北京科学出版社.2012. HU Shou-song.Automatic control principle M .7th ed. Beijing Science Press 2012.14 刘金琨.滑模变结构控制MATLAB仿真基本理论与设计方法M .3版.北京清华大学出版社2019.LIU Jin-kun.MATLAB simulation of sliding mode variable structure control basic theory and design method M .3rd Ed.Beijing Tsinghua University Press 2019.责任编辑:李翠薇62。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
所以近似离散化状态方程为:
x k 1T Gx kT Hu kT ,
0.2 1 0 即x 0.2 k 1 0.4 0.4 x 0.2k 1 u 0.2k
同济大学汽车学院
x1 0 1 x1 0 例3.6.1把状态方程 u x2 2 3 x2 1 T 0.2s. 离散化，
解：(1) 近似离散化:
则G I TA
1 1 0.2 1 0 0 0.2 , 0 1 2 3 0.4 0.4 0 0 0.2 1 0.2
Kk—卡尔曼增益阵
• 卡尔曼滤波的特点：
– – – – –
P(k|k-1)-- 一步测量误差阵
适用于平稳/非平稳时间序列的滤波； 递推形式，便于实时； Kk与观测无关，可离线计算出来； 由于存在两个延迟环节，因此需要给出P0和X(0)（实验表明，可任取）； Kk在滤波收敛之后，不再发生改变。
同济大学汽车学院
G kT x kT H kT u kT .
其中：
G kT I TA kT , H kT TB kT .
同济大学汽车学院
3.6.2 线性时不变系统状态方程的离散化 考虑系统： x t Ax t Bu t ,
其中： 进而：
同济大学汽车学院
连续系统的离散化
框图表示为：
Z变换表示为：
引入状态反馈控制： 可以得到：
同济大学汽车学院
3.6 连续系统的时间离散化
3.6.1 近似离散化
考虑系统
x t At x t B t u t ,
(1)
当采样周期T 很小时，有
x t x k 1 x k T.
x轾 (k + 1)T = e x (kT ) + 臌
AT
ò
t
0 t
e
A(t )
Bdt u (kT ) dt Bu (kT )
同济大学汽车学院
= e x (kT ) +
AT
ò
0
e
A(t )
T 令 G e , H e At d B, 0 AT
则线性时不变系统离散状态方程为：
同济大学汽车学院
Kalman滤波器-最优估计的瞬态解 瞬态增益系数的解如下
测量更新 时间更新
同济大学汽车学院
Kalman滤波器-最优估计的两步更新过程
测量更新,由新息和历史数据进行估计
由测量得到新息 做最优估计
时间更新,为下一步最优估计作准备
同济大学汽车学院
卡尔曼增益Kk的求法
P(k)--误差估计阵
采样周期:
two_mass_sys.m
同济大学汽车学院
(2)
同济大学汽车学院
令t kT , (1) (2) :
x k 1T x kT T A kT x kT B kT u kT ,
x k 1T I TA kT x kT TB kT u kT ,
x k 1T Gx kT Hu kT .
同济大学汽车学院
连续系统离散化的几点说明： (1) 近似离散化是一般离散化的特例 (2) 定常系统离散化是时变系统离散化的特例 (3) 一般说来，没有精确离散化 (4) 离散化是有条件的，“连续化”是无条件的 (5) 连续系统的结论可以在离散系统中找到对应， 反之则未必
~ ~ T ˆ (k | k 1))(X X ˆ (k | k 1))T ] P(k | k 1) E[ X k ,k 1 X k ,k 1 ] E[( X k X k
若设H=1，则基本滤波方程变成： ˆ (k ) (I K ) X ˆ (k | k 1) K Z (k ) X k k
卡尔曼滤波方程中各参数的含义
• 滤波方程中各参数的含义：
– P(k) ----- 滤波误差方差阵
~ ~T ˆ )(X X ˆ )T ] J P(k ) E[ X k X k ] E[(X k X k k k
– P(k|k-1) ----- 一步预测误差方差阵 – Kk ------ 卡尔曼滤波增益
其状态方程的解为：
x t e
A t ห้องสมุดไป่ตู้0
x t0 e
t0
t
A t
Bu d
假设： (1) 等采样周期T：
(2) u t u kT , kT t k 1T .
同济大学汽车学院
令 t k 1T , t0 kT ,
带有离散状态观测器的闭环反馈控制系统：
同济大学汽车学院
Kalman滤波器-最优估计 如何设计状态观测器的增益系数，考虑噪声环境下
测量噪声v(k) 和过程噪声w(k)为零均值高斯噪声,即
测量噪声方差Rv一般由传感器指标决定 过程噪声方差Rw主要考虑未知扰动和模型误差
Kalman滤波的原理是选择观测器增益Ke, 使得如下方差最小:
同济大学汽车学院
拟合、滤波与估计
已知 过去 ←--- 现在时刻 ---→ 将来
？
求过去时间段 某一点的值— —数据平滑、 插值、拟合
？
求现在时刻的 值——滤波
？
求将来时间段 某一点的值— —估计、预测
时间 k
同济大学汽车学院
离散化状态观测器 预测：
滤波：
同济大学汽车学院
离散化状态观测器与闭环控制
同济大学汽车学院
Kalman滤波器-稳态解与LQG
Kalman滤波器稳态解
估计的Hamilton矩阵为:
为Hamilton矩阵的特征根 MATLAB命令为:
同济大学汽车学院
Kalman滤波器- LQG应用实例
LQG=Linear Quadratic Gaussian Regulator
状态变量:
则有：
x轾 (k + 1)T = e x (kT ) + 臌
AT
ò
kT
(k + 1)T
kT
e
A轾 (k + 1)T - t 臌
Bu (t ) d t
e x kT
AT
k 1T
e
A k 1T
Bd u kT
令 t k 1T , 则 d dt ,
同济大学 汽车学院
College of Automotive, Tongji University
钟再敏
离散时间状态观测器与LQG控制 Discrete Time Observers and LQG Control
现代控制理论基础6
目录
1.
引言
同济大学汽车学院
连续系统的离散化 给定LTI系统
LTI系统的输入u为离散控制量，采样周期为T，采样期间ZOH零阶保持， 则采样保持期间的系统相应可以计算得到：