基于卡尔曼滤波的二级倒立摆研究

基于卡尔曼滤波的倒立摆控制系统噪声抑制

作者姓名：王清超

指导教师：杨春雨副教授

单位名称：流程工业综合自动化国家重点实验室

专业名称：自动化

东北大学

2007年6月

Title

by

Supervisor:

Northeastern University

June 2007

毕业设计（论文）任务书

基于卡尔曼滤波的倒立摆控制系统噪声抑制

摘要

关键词：

Title

Abstract Key words：

目录

毕业设计（论文）任务书 .................................................................................. I 摘要 ..................................................................................................................... II Abstract............................................................................................................... I II 目录 .................................................................................................................... I V 第一章绪论 .. (1)

1.1 课题背景及研究的目的和意义 (1)

1.2 状态估计算法的研究概述 (3)

第二章二级倒立摆系统的数学模型 (6)

2.1 二级倒立摆系统实验软硬件条件 (6)

2.2 二级倒立摆系统的动力学模型 (7)

2.2.1 拉格朗日方程 (7)

2.2.2 二级倒立摆的模型 (7)

2.3 小结 (13)

第三章二级倒立摆系统参数辨识 (14)

3.1 能量法辨识方程建立 (15)

3.2 辨识实验 (17)

3.2.1 辨识程序实现 (17)

3.2.2 参数辨识的物理实验 (19)

第四章二级倒立摆平衡点控制 (20)

4.1 基于LQR方法设计平衡控制器 (20)

4.1.1 控制理论介绍 (20)

4.1.2 控制器设计 (20)

4.1.2.1下平衡控制器设计 (22)

4.1.2.2上平衡控制器设计 (23)

4.2 仿真以实际实验效果 (23)

4.2.1 仿真实验效果 (23)

4.2.2 仿真曲线 (25)

4.3 实际实验效果 (26)

4.3.1 下平衡点平衡控制 (26)

4.3.2 上平衡点平衡控制 (28)

4.4 小结 (29)

第五章Kalman滤波器设计 (30)

5.1 Kalman滤波器基本形式 (30)

5.2 Kalman滤波器设计 (31)

5.3 Kalman滤波器仿真实验与分析 (32)

5.4 小结 (35)

第六章结论与展望 (36)

参考文献 (37)

致谢 (38)

第一章绪论

1.1课题背景及研究的目的和意义

在控制理论发展的过程中，某一理论的正确性及实际应用中的可行性需要一个按此理论设计的控制器去控制一个典型对象来验证，倒立摆系统就是控制领域中的典型对象之一。

倒立摆系统作为一个典型的单输入多输出、非线性、强祸合、自然不稳定的系统，从理论上将涉及系统控制中的很多关键问题:如非线性问题、鲁棒性问题、镇定问题、随动问题以及跟踪问题等，而且结构简单、构件组成参数和形状易于改变、成本低廉，其控制效果可以通过其稳定性直观地体现。因此，作为一个多用途的综合性实验装置，倒立摆系统成为控制领域中不可或缺的研究设备和验证控制策略有效性的实验平台早在二十世纪50年代，麻省理工大学电机工程系的控制论专家根据火箭发射助推器原理设计出了单级倒立摆系统的模型。

在70年代，人们对倒立摆实物系统进行了实际研制[1,2]。后来在此基础上，人们又不断进行拓展，产生了许多新的倒立摆结构。对于现阶段被重点研究并制作出实物平台的倒立摆，大体可以分为直线倒立摆与平面倒立摆两类。其中直线倒立摆包括二级倒立摆、多级倒立摆、环形倒立摆、球车系统、简易倒立摆等，平面倒立摆主要包括XY平台平面倒立摆和两自由度旋转机械臂平面倒立摆两种类型。

(a)火箭助推器(b)两足步行机器人

图1.1倒立摆原理的典型应用

倒立摆的研究具有重要的工程背景，其控制方法在军工、航天、机器人和一般工业

过程领域中都有着广泛的用途，如两轮电动车的平衡控制、机器人行走过程中的平衡控制、火箭发射中的垂直度控制和卫星飞行中的姿态控制等均涉及到倒置问题。因此对倒立摆控制机理的研究具有重要的理论和实践意义。

对于倒立摆系统，位移及摆角信号在传输过程中不可避免的会引入噪声干扰，有时噪声干扰甚至会严重影响对系统的稳定控制，对于模拟信号尤甚。信号噪声干扰是工程领域和人们生活中的一大公害。最常见的噪声干扰比如:收音机远离发射台时发出的杂音，电视屏幕上的雪花点，或是引起模/数转换器转换错误的信号等等。

图1.2电视雪花

希望抑制信号中的噪声，就需要使用滤波器对信号进行滤波。由于模拟滤波器需要一定的成本，且精度较差，因此考虑使用数字滤波器。但本二级倒立摆实物平台的摆角信号噪声幅度过大，是一般的时不变滤波器难以滤除的，所以需要使用现代滤波方法滤波。

通过对一系列带有观测噪声和干扰信号的实际观测数据的处理，从中得到所需要的各种参量的估计值，这就是估计问题[3]。估计问题又分为状态估计与参数估计。由于现代滤波方法都考虑了系统的模型的统计特性，因此均可视为状态估计方法。卡尔曼滤波是美国学者卡尔曼(Kalman)于1960年提出的[4]，是采用状态方程和观测方程组成的线性随机系统的状态空间模型来描述滤波器，并利用状态方程的递推性，按线性无偏最小均方差估计准则，采用递推算法对滤波器的状态变量做最佳估计，从而求得滤掉噪声的有用信号的最佳估计。由于卡尔曼滤波器采用递推形式，数据量小，易于计算机实现，所以一经提出便在各行各业得到了广泛应用，是当今被研究最多的滤波算法之一。

鉴于卡尔曼滤波器功能强大且实现起来较为简单，本文利用卡尔曼滤波器对系统的摆角信号进行状态估计，然后利用估计值作为控制器摆角信号的输入，最终实现对二级倒立摆的稳定控制。这对加深控制理论的理解，锻炼实际动手能力，掌握信号处理方法

是很有意义。

1.2状态估计算法的研究概述

在确定性系统的情况下，线性系统的状态估计的主要方法有Luenberger观测器[5]，非线性系统的状态估计主要方法有扩展Luenberger观测器，状态重构只能在系统能观测的部分实现，优点是重构速度能够任意快，缺点是在具体实施过程中要受到一些限制因素的影响，例如灵敏度、噪声等。当随机噪声干扰存在于系统装置或系统观测通道中时，就不能使用确定性系统情况下的观测器进行状态估计了，这时要使用统计估计方法实现系统的状态估计。由于观测信号方式的利用和状态估计准则的不同，导致了状态估计方法的不同。最初的状态估计算法是高斯提出的最小二乘法，然后是Fisher极大似然法，随后Wiener提出了维纳滤波，后来发展到Kalman提出的卡尔曼滤波方法，状态估计理论的发展从频域到时域、从非递推算法到递推算法、从非平稳随机过程到状态空间模型，估计理论越来越丰富和成熟，适用范围越来越广，在航天、航空、军事、生产过程控制等领域，状态估计技术被广泛应用[6]。

1795年，F.K.Gauss提出了最小二乘法，引入了带有估计误差的多个观测数据的概念，并用它处理比较简单的轨道测量问题，但没有考虑被估参数和观测数据的统计特性。1912年，从事概率密度研究的R.A.Fisher提出了极大似然估计法，对估计理论的广泛研究做出了又一个重大贡献，但需要知道观测数据的概率分布函数。1941年，Wiener提出维纳滤波方法，并应用在火炮打飞机的控制系统中，经典的Wiener滤波方法只适用于频域，并且仅限于对平稳时间序列进行滤波和预报，不适用于时变、多变量、非平稳时间序列的滤波和预报，由于维纳滤波算法不能进行递推，需要对全部的历史数据进行存储，因此在实际工程应用中不容易实现。由于维纳滤波的上述局限和缺点，加上计算机应用技术发展的需要，1960年，美国学者Kalman提出了卡尔曼滤波方法。卡尔曼滤波算法能够解决时变、多变量、非平稳时间序列的滤波和预报问题，并且这是一种可递推算法，在计算机上很容易实现，这些特点都弥补了Wiener滤波的不足[7]。

卡尔曼滤波的实现是基于信号和噪声的状态空间模型，利用现时刻的观测值，并结合前一时刻的状态预测值，来更新当前时刻的状态变量的估计值，得出当前时刻最优的状态估计值[8]。Kalman滤波理论提出以后，立即在实际工程中得到了广泛应用，早期最成功的应用实例是C-SA飞机导航系统的设计以及阿波罗登月计划。随着电子计算机技术的普遍应用，在实际工程中，特别是航空航天技术中，Kalman滤波理论迅速被广泛

应用。已有的关于卡尔曼滤波在线性系统中应用的很多理论己经十分成熟，作为一种重要的最优估计理论Kalman滤波已经被广泛地应用在导航制导系统、目标跟踪、全球定位系统、通信和工业控制等领域中。

卡尔曼滤波算法是一种线性状态的最优估计，是在系统状态方程和量测方程都是线性方程，系统噪声和观测噪声均为高斯白噪声，且统计特性已知的条件下，得到最优滤波结果。当系统模型具有非线性特性时，如果仍然采用线性模型描述系统并且使用KF 进行滤波，将会引起线性模型近似误差，甚至将会导致滤波状态发散。而所有的实际系统都有一定程度的非线性。这就促使人们寻求新的非线性滤波器。在之后的近半个世纪里，许多学者以Kalman滤波器为基础进行了推广和改进。

只有完整描述非线性系统的条件后验概率，才是解决非线性系统滤波问题的最佳方法，但是这种完整的描述在实际系统中不容易实现，所以人们又提出了许多次优滤波的近似方法。进行非线性系统滤波问题的次优近似，主要有两种方法:

1)把非线性环节进行线性化处理，忽略或逼近高阶项；

2)用采样的方法去近似非线性函数的分布[9]。

其中第一种方法是解决非线性系统问题的传统方法，这种方法应用最广泛的是扩展卡尔曼滤波(Extended Kalman Filter EKF)算法。EKF最初是StanleySchmidth在1967年为了将卡尔曼滤波应用在非线性航天器导航问题提出的，EKF算法是通过对非线性函数进行Taylor展开，对二阶及其高阶项进行忽略，实现非线性系统的线性化转换。虽然EKF算法在非线性滤波问题中应用广泛，但它仍然具有理论局限性，具体表现在以下方面:

1)当系统非线性程度比较严重时，忽略Taylor展开式的高阶项将会增大线性化引起的误差，从而导致EKF的滤波误差增大甚至滤波发散。

2)滤波过程需要求取系统的雅可比矩阵，但求取雅可比矩阵复杂且计算量大，不容易求取，有时甚至很难得到非线性函数的雅可比矩阵。

3)由于EKF算法需要对非线性函数进行求导，所以非线性函数的表达式必须明确，并且不适用于不可微的非线性函数。

由于近似非线性函数不容易，而相比之下近似非线性函数的概率密度分布更容易，因此人们想到使用采样的方法来近似非线性函数的概率密度分布来解决非线性系统的滤波问题。

Unscented变换(Unscented Transformation ，UT)随后就发展起来，这是一种通过非线性变换传播均值和协方差的方法[10,11]。这种方法滤波更准确，更容易实现。基于Unscented变换的Kalman滤波算法(Unscented Kalman Filter UKF)就是基于UT变换，选择一种确定性采样的方法，然后套用标准的卡尔曼滤波算法的框架，最后得出估计的状态。这种滤波算法是牛津大学Julier[121,13]等学者首先提出的。UKF算法不需要计算系统的雅可比矩阵，复杂度降低，还能处理有非加性噪声的系统，应用范围变宽，UKF算法采用确定性采样策略，避免了粒子的退化问题，滤波精度大大提高。

UKF的研究取得了大量成果，已被应用于许多领域，UKF广泛应用于导航制导、随机信号的处理等领域，然而，UKF在化工过程领域的应用相对较少，特别是反应过程复杂的化工过程，如发酵反应等化工过程。

第二章二级倒立摆系统的数学模型在研究Pendubot控制策略之前必须先得到它的动力学模型，本章将基于拉格朗日动力学理论建立Pendubot的动力学模型。

欠驱动机器人Pendubot是一个在垂直平面上运动的两杆机器人，它在肩部有一个驱动，而肘部没有，因此Pendubot是典型的欠驱动系统，同时由于Pendubot两杆之间强的耦合性及加速度约束不可积，因而具有较强的非线性特征。

建立机器人动力学模型主要采用以下两种理论:

(1)动力学基本理论，如牛顿一欧拉方程;

(2)拉格朗日动力学[42]，特别是二阶拉格朗日方程。

拉格朗日动力学和与牛顿的力学原理在经典力学的范畴内是等价的，但它们研究的途径或方法则不相同[43-45]，因此基于这两种理论所建立的动力学模型是等价的，只是描述形式不同。直接运用牛顿方程的力学体系也称为矢量力学，拉格朗日动力学则称为分析力学。牛顿一欧拉方程的方法，需从动力学出发求得加速度，并消去各个内作用力，这对于复杂的系统十分繁杂。而基于拉格朗日动力学的方法是从系统能量的角度考虑问题，它只需计算速度而不要求内作用力，因此是一种比较简洁的方法。

由于拉格朗日动力学方法仅需计算Pendubot的动能与势能，与牛顿一欧拉方程相比更为简洁，而且还能够充分反映Pendubot的动力学结构特征，因此本论文采用拉格朗日动力学方法建立Pendubot动力学模型，也就是基于运动能量来进行。

2.1二级倒立摆系统实验软硬件条件

如图2.1所示为倒立摆机器人控制系统结构示意图。倒立摆机器人摆臂分为主动臂和欠驱动臂，其角度信号分别通过高精度光电编码器返回，上位机通过网络化实验控制器使驱动器控制力矩电机，使其实现控制要求。

图 2.1 倒立摆机器人控制系统结构示意图

2.2 二级倒立摆系统的动力学模型

2.2.1 拉格朗日方程

对于任何机械系统，其拉格朗日函数都可以定义成该系统动能k E 和势能p E 之差，即

-k p L E E = (2.1) 系统的动能和势能可以用任意选取的坐标系来表示。系统的动力学方程（第二类拉格朗日方程）为

d L L dt q q τ∂∂=-∂∂ (2.2)

由于势能不显含速度项，因此动力学方程也可以写成

p k k E E E d dt q q q τ∂∂∂=-+∂∂∂ (2.3)

那么，对于欠驱动机器人倒立摆机器人系统，其拉格朗日运动方程则为：

()()()1,,[0]()(1()),2T i i i d K q q K q q P q dt q q q i τ∂∂∂-+==∂∂∂， (2.4)

基于倒立摆机器人的动力学模型，其中摆臂连杆受到的力矩为τ，只有摆臂连杆的主动关节受力。由于假设两杆均为刚体，所以其动能与势能可根据每一杆的总质量与相对于重心的惯量来确定。

2.2.2 二级倒立摆的模型

倒立摆机器人系统模型可用图3.1所示坐标描述：

图 2.2 倒立摆机器人控制系统动力学模型示意图

其中

1l ：主动臂的长度；

1c l ：主动臂相对于连接点到质心的距离；

2c l ：欠驱动臂相对于连接点到质心的距离；

1q ：主动臂相对于坐标轴的角度；

2q ：欠驱动臂相对于主动臂的角度；

1I ：主动臂相对于质心转动惯量；

2I ：欠驱动臂相对于质心转动惯量；

1m ：主动臂质量；

2m ：欠驱动臂质量；

g ：重力加速度。

根据上述模型，进行系统的动能和势能的推导。计算平移动能的一般表达式为2

2

mv K =，而系统两个连杆的角速度为 11212ωωq q q ==+，

(2.5)

主动臂的平移动能可以直接表示为 22111112c K m l q = (2.6)

势能与机械臂质心的高度有关，高度用y 坐标表示，于是势能可以直接写成

1111 sin()c P m l g q = (2.7)

对于欠驱动臂，先写出质心处笛卡儿坐标位置的表达式，然后求微分，以便得到关节角速度。

211212211212cos()cos()

sin()sin()c c x l q l q q y l q l q q =++=++ (2.8)

于是，速度的笛卡儿坐标分量为

()()()()()211121************sin sin ()

cos cos c c x l q q l q q q q y l q q l q q q q =--++=+++ (2.9)

速度的平方值为

()()22222222112121212211222cos ()c c v l q l q q q q l l q q q q =+++++ (2.10)

从而欠驱动臂的平移动能为

()()()

2222222211212121221121[22cos ]2c c K m l q l q q q q l l q q q q =+++++ (2.11) 连杆的高度已经给出，由式(2.7)，它的势能为

()()2211212sin sin c P m g l q l q q =++⎡⎤⎣⎦ (2.12)

同时我们注意到系统除了平移动能外，还存在旋转动能，由式(2.6)可得系统的旋转动能部分为 []12211212222101111001122T v I I I q K Q I I Q q q I I q +⎧⎫⎡⎤⎡⎤⎡⎤⎡⎤=+=⎨⎬⎢⎥⎢⎥⎢⎥⎢⎥⎣

⎦⎣⎦⎣⎦⎣⎦⎩⎭ (2.13)

拉格朗日算子为L K P =-，因此根据上述式子，可得 1212v L K P K K K P P =-=++-- (2.14)

为了求得动力学方程，现在根据式(2.14)，对拉格朗日算子进行微分，即

()()()(){

}(){}

()2221112112212212211

2122211222

22211212122121122221222221222111

)2cos cos 2cos cos sin (c c c c c c c c c c c L m l m l m l m l l q m l l q I I I d L m l m l l l l q I I dt m l l l q I m l l q L m q q q q q q q q q q l q q q q ∂=++++∂++++∂⎡⎤=+++++⎣⎦∂⎡⎤+++-⎣⎦∂=-∂()()2112212cos sin()c m l g q m l g q q +-+ (2.15) 由以上的各微分项，根据第二类拉格朗日方程，我们就可以直接得到主动关节的力矩表达式为

(){}(){}

()()()()111

2221c121c21c22121

222c21c222221c22221c22121c12112c212d τdt 2cos cos sin 2sin g cos g sin()

L L q q m l l I I m l l l q m l l q l l q l l q q l m l l q l I q m q m q q m m q q ∂∂=-∂∂⎡⎤=+++++⎣⎦⎡⎤+++-⎣⎦-++++ (2.16) 再把拉格朗日算子对2q 和2q 求微分，进而得到欠驱动关节的动力学方程：

()()()

()()

()2222212221222222222212221222221221222212211222122cos ()cos sin()sin cos c c c c c c c c c L m l m l l q I m l I d L m l m l l q I m l I m l l q q dt L m l l q q q q m l g q q q q q q q q q q ∂⎡⎤=++++⎣

⎦∂∂⎡⎤=++++-⎣⎦∂∂=-+-+∂ (2.17)

于是欠驱动关节的动力学方程为 ()()()()

22

2222212221222222122122120cos sin cos c c c c c d L L dt q q m l m l l q I m l I m l l q q l g q q m q q ∂∂=-∂∂⎡⎤=++++⎣⎦+++ (2.18)

式错误!未找到引用源。和式错误!未找到引用源。可以简写成如下形式：

()()(),D q q C q q q G q τ++= (2.19)

在考虑摩擦力矩的情况下，上述表达式可以拓展成为

()()()(),+D q q C q q q G q F q τ++=

(2.20)

其中，

()()()11122211121222

2,00d d hq hq hq D q C q q G q d d hq ϕττϕ+⎡⎤⎡⎤⎡⎤⎡⎤====⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎣⎦⎣⎦

，，， 其中各个参数的表达式如下： 2221111212112212

212212211222

222222

[2cos()][2cos()]c c c c c c d m l m l l l l q I I d d m l l l q I d m l I =+++++==++=+

2122sin()c h m l l q =- ()111211*********cos()cos()

cos()c c c m l m l g q m l g q q m l g q q ϕϕ=+++=+

如果系统动力学模型表示成式(2.20)，那么有一个特征要注意：()D q 和(),C q q 并非两个完全独立的矩阵，两者之间()()2,D q C q q -是一个反对称矩阵。其物理意义是机械臂动能的导数等于驱动器力矩和重力矩的功率输入之和

1()[()]2T d q D q q G q dt τ⎡⎤=-⎣⎦ (2.21)

因此，对于任意时间有

102T q D C q ⎛⎫-≡ ⎪⎝⎭ (2.22)

我们得到如式(2.20)的系统动力学表达式，在实际应用中，为了计算与实验的方便，引入以下五个参数表达式：

22111211

22222

3212

41121

522c c c c c m l m l I m l I m l l m l m l m l θθθθθ=++=+==+= (2.23)

因此有

()123223223222cos()cos()cos()q q D q q θθθθθθθθ+++⎡⎤=⎢⎥+⎣⎦

(3.1)

()3

22322321321sin()sin()sin(),sin()0q q q q q q C q q q q θθθθ---⎡⎤=⎢⎥⎣⎦ (3.2)

()42512512cos()cos()cos()g q g q q G q g q q θθθ++⎡⎤

=⎢⎥+⎣⎦

(2.24) 则可将倒立摆机器人动力学方程改写成状态方程形式：

()()112()[,]q D q C q q q G q q τ-⎡⎤=--⎢⎥⎣⎦

(2.25) 令11213242x q x x x q q u q τ=====，，，，得到状态方程：

()()()()()()(,,)x t f x g x u t F x t u t t =+=

(2.26)

即Pendubot 系统的状态方程为

()()1111233224()()x

x a x b x u x x x a x b x u x ⎡⎤

⎡⎤⎢

⎥⎢⎥

+⎢⎥⎢⎥=⎢⎥

⎢⎥

⎢⎥⎢⎥+⎣⎦⎣⎦

(2.27) 其中，

2

2

23323232412

2

12333

351332412

2

12333

22

22

3323324322

2

33312

sin() [2cos() 2()]

()2cos(2)2cos()cos()2cos()

+2cos(2)2sin()[cos()()cos()]

()cos(2)22s x x x x x a x x g x x x g x x x x x x x x a x x θθθθθθθθθθθθθθθθθθθθθ++=--+---++=+-+2

2

323241322

2

33312

15132412

2

33312

351332

2

33312

34132

2

33312

in()[()]

cos(2)22[ cos()cos()]

cos(2)22 cos()cos()

cos(2)22 cos()cos()

cos(2)2x x x x x g x x g x x g x x x x g x x x θθθθθθθθθθθθθθθθθθθθθθθθθθθθ+++-+-++-+++--+-

(2.28)

2

12212333

23322233312

2()2cos(2)2[cos()]

()cos(2)2b x x x b x x θ

θθθθθθθθθθ=--+=+-

2.3小结

本章基于拉格朗日动力学理论建立Pendubot的动力学模型。在建立Pendubot机器人动力学模型时，把Pendubot看作是一个个关节连接起来的刚体。并用基于运动能量的拉格朗日动力学方法建立Pendubot动力学模型。这种方法仅需计算Pendubot的动能与势能，因而与牛顿一欧拉方程相比更为简洁，而且还能够充分反映Pendubot的动力学结构特征。本章所得到的动力学模型是研究Pendubot控制策略的基础。