无迹Kalman滤波在IMU和GPS组合导航系统中的应用研究

无迹Kalman滤波在IMU和GPS组合导航系统中的应用研究答辩人：王利循导师：郭杭教授第1章绪论1.1导航子系统GPS的简介1.2捷联惯导SIMU子系统1.3主要研究内容和意义1.1导航子系统GPS的简介1、GPS系统的组成GPS系统包括三部分即：⑴空间星座部分；⑵地面控制部分；⑶用户部分。2、空间星座部分GPS空间星座部分由24颗工作卫星组成，这些卫星共同组成了GPS卫星星座，其分布图如图1.1所示。其中21颗卫星用于导航，3颗作为活动的备用卫星。这些工作卫星分布在6个轨道面内，每个轨道面上分布有4颗卫星，轨道倾角为55°，以平均11小时58分的周期绕地球运动，轨道平均高度为20200㎞。目前，该系统已经有30颗工作卫星，每颗卫星每天约有5小时位于地平线以上，同时位于地平线上的卫星数目随时间地点而定，最少4颗最多可达11颗。图1.1GPS卫星星座分布图2、空间星座部分3、地面控制部分GPS地面控制部分由分布全球的若干跟踪站组成，这些跟踪站根据其功能又可分为1个主控站、3个注入站、5个监控站。主控站位于美国克罗拉多的法尔孔空军基地，其作用是根据监控站对GPS的观测数据，计算卫星星历和卫星钟差改正数等，这些数据通过注入站注入到卫星中去；同时，它还对卫星进行控制向卫星发布指令，当卫星出现故障时，调度备用卫星；另外，主控站也具有监控站的功能。除了位于克罗拉多的主控站外，其余四个分别位于夏威夷、阿松森群岛、迭哥迦西亚、卡瓦加兰，监控站主要作用是接收卫星信号监测卫星状态；三个注入站分别位于阿松森群岛、迭哥迦西亚、卡瓦加兰，其作用是将主控站计算出的卫星星历和钟差改正数等注入到卫星中。4、用户部分GPS用户部分由GPS接收机、数据处理软件以及其他用户设备，如计算机、气象仪等。这部分的作用主要是接收GPS卫星信号，解译导航电文，以进行导航和定位等工作。1.2捷联惯导SIMU子系统1、贯导系统惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统，有时简称惯导。惯性导航系统在结构上可分为平台式惯导系统和捷联式惯导系统。2、平台式惯导系统和捷联式惯导系统平台式惯导系统是用一个实际的物理平台模拟导航坐标系，并用模拟的方法给出载体的姿态和方位信息。捷联式惯导系统没有实体平台，将陀螺仪和加速度计直接安装在机动载体上，惯导平台的功能由计算机来完成。3、平台式惯导系统和捷联式惯导系统的优缺点平台式惯导系统的优点是对陀螺仪的动态范围要求低，导航解的求解过程简单；其缺点是成本高，故障率大。捷联式惯导系统的优点是体积小、重量轻、成本低，便于安装、维护和更换，可提供更多导航和制导信息，便于余度配置，系统性能和可靠性较高；其缺点是要求惯性器件有较大的动态范围，导航求解较复杂。鉴于导航精度的要求，捷联式惯导系统成为惯性系统发展的重点，本文将主要介绍的是捷联惯导系统(SIMU)，其原理见图1.2。4、捷联惯导系统(SIMU)原理图图1.2SIMU的原理框图1.3主要研究意义1.3主要研究意义Kalman滤波理论一经提出，立即受到工程界的重视，而工程应用中遇到的实际问题又使Kalman滤波的研究更深入更完善。R.E.Kalman最初提出的滤波基本理论只适用于线性系统，并且要求观测方程也必须是线性的。针对EKF由于线性化所带来的问题，一些相应的改进算法被提出，如迭代滤波和二阶滤波。而Julier提出了一种基于UT(UnseentedTranformation)的非线性滤波方法UKF(UnscentedKalmanFitler)。UT对后验均值和方差的逼近精度高于线性化方法，且不用计算Jacobian矩阵，比EKF实现简单。1.3主要研究意义因此,本文所提出的将UKF方法用于IMU/GPS组合导航系统的直接法卡尔曼滤波,既避免了对非线性的系统状态方程进行线性化,又保证了系统具有较高的导航定位精度,使直接法滤波的非线性问题得到了较好的解决,从而为组合导航的滤波方法提供了一种新的思路,具有良好的理论研究与工程参考价值。第2章Kalman滤波原理及其在导航系统中的应用2.1Kalman滤波原理2.2Kalman滤波器算法2.3Kalman滤波在导航系统中的应用2.4本章小结2.1Kalman滤波原理1、Kalman滤波原理Kalman滤波实际上是一种最优估计方法，它是一套由计算机实现的处理随机信号的递推算法，利用系统噪声和观测噪声的统计特性，以系统的观测量为滤波器的输入，以系统的状态或参数作为滤波器的输出，滤波器的输入和输出之间是由时间更新和观测更新算法联系在一起的，根据系统方程和观测方程估计所需要处理的信号。2.2Kalman滤波器算法2.2Kalman滤波器算法本文在这里我们先要引入一个离散控制过程的系统。该系统的状态方程可用一个线性随机微分方程来描述：(2.1)系统的测量方程：（2.2）式中：－k时刻的系统状态；－k时刻对系统的控制量，如果没有输入控制信息，则U默令为0；－k时刻系统状态矩阵；Bk－k时刻系统输入控制矩阵；－k时刻的测量值；Hk－k时刻测量系统的矩阵；－k时刻过程噪声矩阵，为高斯白噪声，它的协方差为Qk；－k时刻测量噪声矩阵，为高斯白噪声，它的协方差为Rk。1kkkkkkXXBUWkkkkZHXVkXkUkkZkWkV步骤一首先利用系统的过程模型，来预测下一状态的系统。X0为初始时刻的状态向量，满足以下统计特性：(2.3)假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：(2.4)式中：－是利用上一状态（k-1）预测的结果；－是上一状态（k-1）最优的结果，－为现在状态的控制量，如果没有控制量，它可以为0。00,00ˆ()var()(0)xEXXXD,11,1ˆˆkkkkkkkXXBU,1ˆkkX1,1ˆkkXkU步骤二到现在为止，我们的系统结果已经更新了，可是，对应于状态预测值的协方差还没更新。本文用P表示协方差：(2.5)式中：－是对应的协方差；－是对应的协方差；－表示的转置矩阵；－是系统过程噪声的协方差。,11,11TkkkkkkkPPQ,1kkP,1ˆkkX1,1kkP1,1ˆkkXTkk1kQ步骤三有了现在状态的预测结果，然后再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态(k)的最优化估算值：(2.6)式中：－为k时刻的增益矩阵，即：(2.7),ˆkkX,,1,1ˆˆˆ()kkkkkkkkkXXKZHXkK1,1,1TTkkkkkkkkkKPHHPHR步骤四到现在为止，已经得到了k状态下最优的估算值。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，还要更新k状态下的协方差：(2.8)当系统进入k+1状态时，就是式子(2.5)的。这样，算法就可以自回归的运算下去。,ˆkkX,ˆkkX,,1,1,TTkkkkkkkkkkkkPPKHPHRK,kkP1,1kkP2.3Kalman滤波在导航系统中的应用2.3Kalman滤波在导航系统中的应用Kalman滤波最成功的工程应用是设计运载体的高精度组合导航系统。20世纪80年代起，随着各种可供装载的导航系统越来越多，非相似导航子系统的增加使量测信息也相应的增多，这对提高组合导航系统的精度十分有利。1979年至1985年间，Spyeer，Biemrna和Kerr等人先后提出了分散滤波思想，而并行计算技术的成熟也为分散滤波的发展应用创造了成熟的条件。1988年起，Carlosn提出了联邦滤波理论(FederatedFiltering)，由此为容错组合导航系统提供设计理论。采用联邦滤波结构设计的组合导航系统具有较高容错能力，正因为如此使其成为美国空军新一代导航系统的通用滤波器。2.4本章小结Kalman滤波因实用性一经提出就受到广泛关注和研究，在各种工程项目中数据去噪处理，尤其在组合导航系统得到广泛的应用。经过几十年的发展，人们针对Kalman滤波出现的问题提出了各种相应的改进算法并应用到实际工程中。从低维的到高维的，从单系统到多系统的组合系统，从线性的到非线性的等等方面的改进和应用，无不显示Kalman滤波的魅力及其越来越成熟的理论。第3章UKF非线性滤波方法3.1UKF基本原理3.2噪声为加性时的UKF方法3.3本章小结3.1UKF基本原理1、连续性系统的离散化设连续线性系统带随机干扰下的微分方程式为：(3.1)式中：A、G可以是时变量；W是白噪声过程，且方差分别为Q。（3.1）式离散化的状态方程为：(3.2)式中：(3.3)Wk的噪声序列方差为：Qk(3.4)Qk中取第一项，则有结论：状态的方差乘以T，这是离散后的结论。XtAXtGWt1kkkXXW222!ATATeIAT22!TTTTkAGQGGQGATQGQGT2、非线性离散系统由前面的推导可假设随机非线性离散系统为：(3.5)其中，Wk和Vk为零均值白噪声，其统计特性为E[Wk]=0，E[WkWjT]=Qk*δkj，E[Vk]=0，E[VkVjT]=Rk*δkj，E[WkVjT]=0。111()kkkkXfXW()kkkZhXV3、EKF递推算法EKF围绕状态估计值将非线性函数f(·)展成泰勒级数并取其一阶近似，围绕状态预测值将非线性函数h(·)展成泰勒级数并取其一阶近似，从而得到非线性系统的近似线性化模型：(3.6)其中，和分别称为向量函数f(·)和h(·)的Jacobian矩阵，1ˆkX/1ˆkkX/1111kkkkkkXXWkkkkZHXV/1kkkH3、EKF递推算法由线性化模型(3.6)就可以根据Kalman滤波方程进行递推计算：(3.7)(3.8)(3.9)(3.10)(3.11)由于EKF对非线性系统进行了线性化近似，将会引入模型误差，递推过程中需要计算非线性函数f(·)和h(·)的Jacobian矩阵和Hk，当处理高维复杂的系统模型时，这一过程十分繁琐且容易出错。/11ˆˆ()kkkXfX/1/1ˆˆˆ[()]kkkkkkkXXKZhX1/1/1[]TTkkkkkkkkkKPHHPHR/1/11/1111TTkkkkkkkkkkPPQ/1[]kkkkkPIKHP/1kk4、UKF非线性滤波方法Julier和Uhlmann等提出了UKF非线性滤波方法，利用无迹变换（UT）方法将一组逼近状态估计统计分布的采样点(i=1，2，…，n，其中n为采样点个数)直接代入非线性状态方程f(·)获得一组状态的变换点，将直接代入非线性观测方程h(·)获得一组观测的变换点，再由变换点和的前二阶矩统计特性获得状态预测值和状态预测协方差阵、观测估计值和观测协方差阵以及状态预测与观测之间的协方差阵的近似值，然后按照Kalmna滤波方程进行递推计算。UKF方法不需要对非线性系统进行线性化近似，避免了计算Jacobian矩阵。1ˆkX,1ikx,/1ikkx,/1ikkx,/1ikkz,/1ikkx,/1ikkz/1ˆkkX/1kkP/1ˆkkZ,zkP,xzkP5、无迹变换(UT)的主要原理UKF算法是基于UT(UnscentedTransform)变换的最小方差估计，UT变换是一种计算一个随机变量的非线性变换的统计特性的方法，其主要思想是是选择一组sigma点，使其样本均值和协方差与状态随机变量的均值和协方差Pxx一致，将这些点进行非线性变换后可获得变换点的均值和协方差Pzz。5、无迹变换(UT)的主要原理对于均值为，方差为P的n维随机变量x，其变换函数为y=f(x)，Julier在他的论文中给出如下一