卡尔曼滤波与组合导航-第四章

卡尔曼滤波与组合导航

Theoryof

KalmanFilterandIntegratedNavigation主讲：房建成教授电话：82339487Email：fangjiancheng@课件下载：kalman_filter@

密码：1234561第四章惯性/卫星组合导航一、惯性/卫星组合导航的意义二、惯性/卫星组合导航系统的类型三、导航系统误差分析和误差方程四、惯性/卫星组合导航的工作模式五、SINS/GPS先进滤波方法24.1.1惯性导航惯性导航系统的基本工作原理是以牛顿力学定律为基础，利用陀螺仪建立空间坐标基准（导航坐标系），利用加速度计测量载体的运动加速度，将运动加速度转换到导航坐标系经过两次积分运算，最终确定出载体的位置和速度等运动参数。34.1惯性/卫星组合导航的意义与外界不发生任何光、电和磁联系——隐蔽性好；工作不受气象条件的限制——可用性强；完全依靠运动载体设备自主完成导航任务——自主性好；能够提供比较齐全的导航参数——参数齐全；目前已广泛应用于潜艇、水面舰艇、军用飞机、战略导弹和战术导弹、战车和人造卫星等领域——应用面广。4.1.1惯性导航优点44.1惯性/卫星组合导航的意义系统精度主要取决于惯性测量元件，导航参数的误差随时间而积累，不适宜长时间导航。一般惯导系统的加热和初始对准所需时间较长，很难满足远距离、高精度导航和其它特定条件下的快速反应要求。位置误差速度误差4.1.1惯性导航缺点54.1.2卫星导航

卫星导航定位的基本原理是用户接收机接收到导航星座卫星发射的导航信号，经相关处理来测定由卫星到接收机的传播时间延迟，或测定卫星载波信号的相位在传播路径中变化的周数，解算出由接收机到卫星之间的距离，从而确定接收机的位置。64.1惯性/卫星组合导航的意义4.1.2卫星导航优点：误差不积累能够在全球范围，为多个用户，全天候、实时、连续地提供高精度三维位置、速度及时间信息。美国：GPS；俄罗斯：GLONASS；目前己经投入运营或正在建设的几个主要的卫星导航系统有：欧空局：GALILEO；中国：北斗。74.1.2卫星导航结论：GPS不能保证安全、连续、精确、可靠导航美国2000年之后每年都将审议一次SA政策；美国军方声称随时都有可能改变GPS政策；GPS的系统信号在高纬度地区经常出现盲区；美国国防部曾强调，限制敌人在战时利用GPS。1）美国GPS可能存在问题894.1.2卫星导航与GPS相比，GLONASS因运行时间短，用户尚少，目前还不具备象GPS增强系统和IGS网络长期不间断的观测信息支持。GPS接收机市场十分活跃，产品不断翻新，而GLONASS目前还未达到这一水平，且GLONASS接收机供应严重不足。此外，因为没有GLONASS卫星的精确轨道源数据，故无法测定精度。与GPS相比这是GLONASS的个一主要缺陷。2）GLONASS存在的主要问题10“伽利略计划”是由欧盟委员会和欧洲空间局共同发起并组织实施的欧洲民用卫星导航计划，它受多个国家政策和利益的制约，政策具有摇摆性。由于欧盟受美国的影响极大，“伽利略计划”本身的独立性值得怀疑；GALILEO计划目前已经延后，考虑到目前的金融危机，未来的GALILEO如何发展现在还看不清楚。3）GALILEO存在的主要问题4.1.2卫星导航114.1.2卫星导航无法为用户提供所在高度的数据，因此需要预先存储需定位目标的地面高程信息，并通过与地面中心站的联系才能推算高度。用户必须向地面中心站申请定位，才能获得定位信息，于是用户的隐蔽性成问题。由于地面中心站是北斗一代的核心，地面中心站一旦遭攻击，整个卫星系统将陷入瘫痪。4）北斗卫星定位系统可能存在的问题124.1.2卫星导航动态性能差信息主要从朝向卫星的天线获得。一旦载体做翻滚或者拐弯等大机动动作，则将无法接收卫星信号，从而也无法导航。带宽有限接收机的环路带宽不能同时满足高精度性能和动态跟踪性能之间的矛盾要求。当航行体大机动运动时，系统将难以正常工作。提取载体的姿态信息比较困难接收机的数据更新率低13

惯性、卫星导航系统都存在各自的优缺点，对导航信息进行信息融合，其优点如下：(1)互补、超越。组合导航系统融合了各导航子系统的导航信息，相互取长补短，超越了单个子系统的性能和精度，同时提高了系统环境适应性；(2)冗余、可靠。同一导航信息可通过多个导航子系统测量，获得冗余的测量信息，增强了系统的冗余度，提高了系统的可靠性；(3)低成本。通过组合导航技术在保证导航系统精度的同时，可降低单系统对器件的要求，从而降低组合导航系统的成本。4.1惯性/卫星组合导航的意义14Kearfott‘sKN-4072/4GPS/INS((MRLG)应用于”全球鹰”无人机4.1惯性/卫星组合导航的意义15惯性/GPS组合导航系统用于增程制导炮弹4.1惯性/卫星组合导航的意义16GPS/INS制导的JDAM4.1惯性/卫星组合导航的意义一、惯性/卫星组合导航的意义二、惯性/卫星组合导航系统的类型三、导航系统误差分析和误差方程四、惯性/卫星组合导航的工作模式五、SINS/GPS先进滤波方法17184.2惯性/卫星组合导航的工作模式4.2.1组合导航系统的组成1、组合导航子系统2、组合导航滤波器191、根据状态选择的不同可分为：直接法如果直接以各导航子系统的导航输出参数作为状态，则称实现组合导航的滤波处理为直接法滤波。间接法如果以各子系统的误差量作为状态，即以导航参数的误差量或子系统的误差源作为估计对象，则称实现组合导航的滤波处理为间接法滤波。4.2组合导航系统的组成和分类4.2.2组合导航系统的工作模式204.2.2组合导航系统的工作模式直接法和间接法的比较状态量状态方程滤波方法直接法导航参数变化较快，阶数较大，运算频率高，运算量大运动或动力学方程直接反映真实导航参量的变化情况，但是方程解算复杂非线性间接法导航参数误差变化较慢，解算频率低，运算量小误差方程通常是按照一阶近似推导出来的，方程线性且简单线性212、根据组合系统误差修正方式的不同可分为：反馈校正（闭环式）输出校正（开环式）4.2.2组合导航系统的工作模式224.2.2组合导航系统的工作模式输出校正的特点优点工程实现简单，稳定性高，即使测量信息质量下降时，也能保证解算的稳定性；缺点由于仅仅对惯导的输出结果进行修正，并未补偿或校正惯性元件的参数（如陀螺漂移、加速度零偏等），所以误差输入量将随时间不断增长，导致推广卡尔曼滤波器出现较大的模型线性化误差，从而使组合系统的定位精度随工作时间延长而下降23优点利用滤波输出的误差最优估值反馈回导航计算机去调整惯性元件的参数，减弱惯导系统误差的传播速度，误差输入保持小量，从而保证卡尔曼滤波器线性化模型的准确性。缺点系统的稳定性差，特别是当测量信息有较大误差时，容易导致系统的不稳定，甚至发散。反馈校正的特点4.2.2组合导航系统的工作模式一、惯性/卫星组合导航的意义二、惯性/卫星组合导航系统的类型三、导航系统误差分析和误差方程四、惯性/卫星组合导航的工作模式五、SINS/GPS先进滤波方法24254.3导航系统误差分析和误差方程4.3.1惯性导航的误差方程

测试、标定、对准1、惯性导航误差分析（1）元件误差：陀螺漂移、指令角速度刻度系数误差、加计零偏和刻度系数误差，计算机舍入误差、电流变换装置误差等（2）安装误差：陀螺和加速度计的安装误差（3）初始条件误差：平台的初始对准误差、力学编排方程中的初始速度和位置误差。（4）干扰：冲击和振动等运动干扰（5）其他：地球模型描述误差、有害加速度补偿忽略二阶小量引起的误差惯性导航误差标定加速度通道偏置误差刻度因数安装误差角速度通道常值误差标度因数与g有关项安装误差惯性测量单元误差要求严格

正交装配三陀螺三加计xyz占SINS误差的90％必须标定补偿4.3.1惯性导航的误差方程

26将地理坐标系(东北天)作为导航解算的基本坐标系，考虑高度，并假设地球为旋转椭球体。1）数学平台失准角误差方程2、地理系下惯性导航系统误差方程274.3.1惯性导航的误差方程

2）速度误差方程284.3.1惯性导航的误差方程

3）位置误差方程

4）惯性器件误差方程陀螺仪误差和加速度计误差在此都作为随机常值来考虑，它们的误差方程写为：、，、、。

294.3.1惯性导航的误差方程

影响GPS定位的误差，可以分为四大类：与卫星有关的误差，如卫星星历误差、卫星钟误差、相对论效应等；与传播路径有关的误差，如大气延迟误差、多路径效应等；与接收设备有关的误差，如接收机钟误差、天线高的量取误差等；其它误差，如地球自转等。这些误差，对解算的基线向量具有不同的影响规律，有的在模型中能得到较好的消除或削弱，有的通过采用合适的改正模型其大部分影响可以消除，有的采用一定的观测措施能限制在较小的范围内，而有的却难以改正。4.3导航系统误差分析和误差方程4.3.2卫星导航30这些误差的细节及其影响参见右表。为了便于理解，通常均把各种误差的影响投影到观测站至卫星的距离上，以相应的距离误差表示，并称为等效距离偏差。表中所列对观测距离的影响，即为与相应误差等效的距离偏差。

4.3导航系统误差分析和误差方程4.3.2卫星导航314.3导航系统误差分析和误差方程4.3.2卫星导航1、伪距量测方程的常用形式：2、载波相位量测方程为

上式中包括了GNSS星座卫星星钟，GNSS接收机钟差、大气折射误差及其它因素造成的误差等，这些误差模型在实际中均非常复杂，难以精确建模。32一、惯性/卫星组合导航的意义二、惯性/卫星组合导航系统的类型三、导航系统误差分析和误差方程四、惯性/卫星组合导航的工作模式五、SINS/GPS先进滤波方法334.4惯性/卫星组合导航的工作模式4.4.1INS/GNSS组合模式组合模式主要可分为四种：（1）非耦合系统。利用GNSS的位置和速度对INS进行重置（2）松耦合系统。利用GNSS和INS的位置、速度误差作为Kalman滤波的观测量（3）紧耦合系统，以GNSS的伪距、伪距率作为Kalman滤波的观测量（4）深组合（超紧耦合）系统，将GNSS信号跟踪功能与INS/GNSS组合结合在一起34松组合又称级联Kalman滤波(CascadedKalmanFilter)方式。观测量——INS和GNSS输出的速度和位置信息的差值；系统方程——INS线性化的误差方程；通过扩展Kalman滤波（Extended

KalmanFilter=EKF）对INS的速度、位置、姿态以及传感器误差进行最优估计，并根据估计结果对INS进行输出或者反馈校正。1、松耦合(Loosely-CoupledIntegration)4.4.1INS/GPS组合模式351、松耦合(Loosely-CoupledIntegration)4.4.1INS/GPS组合模式36GNSS接收机通常通过自己的Kalman滤波输出其速度和位置，这种组合导致滤波器的串联，使组合导航观测噪声时间相关（有色噪声），不满足EKF观测噪声为白噪声的基本要求，严重时可能使滤波器不稳定。当导航星少于4颗时，无法进行组合。

松耦合的缺点系统结构简单，易于实现，可以大幅度提高系统的导航精度，并使INS具有动基座对准能力。

松耦合的优点4.4.1INS/GPS组合模式37观测量——伪距和伪距率的差值。通过EKF对INS的误差和GNSS接收机的误差进行最优估计，然后对INS进行输出或者反馈校正。由于不存在滤波器的级联，并可对GNSS接收机的测距误差进行建模，因此这种伪距、伪距率组合方式比位置、速度组合具有更高的组合精度。而且在可见星的个数少于4颗时也可以使用。2、紧耦合（Tightly-CoupledIntegration）4.4.1INS/GPS组合模式384.4.1INS/GPS组合模式2、紧耦合（Tightly-CoupledIntegration）394.4.1INS/GPS组合模式2、紧耦合（Tightly-CoupledIntegration）40

采用紧耦合方法的优点

(1)不用考虑将一个卡尔曼滤波器的输出用做第二个滤波器的测量输入时所产生的统计问题；(2)隐含完成GPS位置和速度协方差的交接；(3)系统不需要用完整的GPS数据来辅助INS，即使只跟踪到单个卫星信号，GPS数据也会输入滤波器，但是精度下降很快。深组合是使用惯性导航信息对GNSS接收机进行辅助导航的组合方式。主要思想：既使用滤波技术对INS的误差进行最优估计，同时使用校正后的INS速度信息对接收机的载波环、码环进行辅助跟踪，从而减小环路的等效带宽，增加GPS接收机在高动态或强干扰环境下的跟踪能力。嵌入式组合将INS和GNSS进行一体化设计，通过共用电源、时钟等进一步减小体积、降低成本和减小非同步误差的影响。3、深组合（Deeply-CoupledIntegration）4.4.1INS/GPS组合模式414.4.1INS/GPS组合模式3、深组合（Deeply-CoupledIntegration）42

通过一同跟踪GPS信号，而不是使用独立的跟踪回路，其他信号和惯性数据辅助每一个信号的跟踪，这样可以带来下列3个主要的好处：(1)由于使用相同数据跟踪的独立量减少了，从而提高了有效信噪比，而且跟踪的卫星越多，信噪比的提高就越大；(2)改进了多路径阻抗；(3)信号障碍或干扰造成短时中断后，信号的重新截取速度更快。

要想获得这里所介绍的潜在好处就会大大增加复杂程度；某些结构会增加计算量且需要严格的时间同步，有的结构还需要高性能的INS数据以保持跟踪回路的锁定。深组合和其他组合结构之间的主要差别在于需要非常快的更新速率(一般为200Hz至1kHz)，以保持锁定时信号的跟踪功能。4.4.1INS/GPS组合模式43三种工作模式的对比选取15阶状态变量平台误差角速度误差位置误差陀螺随机常值漂移加计随机常值偏置4.4.2INS/GPS组合数学模型

1、松耦合的数学模型444.4惯性/卫星组合导航的工作模式系统状态方程状态转移矩阵噪声转移矩阵454.4惯性/卫星组合导航1、松耦合的数学模型系统状态方程W系统噪声向量陀螺随机漂移加计随机误差464.4惯性/卫星组合导航1、松耦合的数学模型系统量测方程观测量由GPS获得位置速度误差474.4惯性/卫星组合导航1、松耦合的数学模型系统量测方程观测量量测矩阵481、松耦合的数学模型4.4惯性/卫星组合导航494.4惯性/卫星组合导航504.4惯性/卫星组合导航1、松耦合的仿真实例《导航系统》袁信等

取低精度的捷联惯导系统和GPS综合。设惯导系统的等效陀螺漂移为0.1º/h，等效加速度计零偏为1ug；GPS接收机为带SA误差的C/A码接收机，其伪距测量误差为偏置10m，随机32m，随机伪距率误差为0.05m/s。假设导航信息的初始误差为：水平姿态误差角300″，方位误差角600″，位置误差50m，速度误差0.6m/s,当GPS星座采用最优21星座时，飞行航线包括起飞、爬升，转弯、加速、减速、平飞等飞行状态。初始航向角为35º，平飞速度为300m/s，初始位置为北纬30º，东经120º，海拔高度1000m。按位置综合、速度综合、位置和速度交替综合、位置和速度联合综合四种情况，各仿真1小时。其仿真结果如图6.5到6.8所示。其稳态误差如表6.1所列。514.4惯性/卫星组合导航524.4惯性/卫星组合导航53545556574.4惯性/卫星组合导航584.4惯性/卫星组合导航2、紧耦合的数学模型594.4惯性/卫星组合导航2、紧耦合的数学模型604.4惯性/卫星组合导航2、紧耦合的数学模型伪距的量测方程614.4惯性/卫星组合导航2、紧耦合的数学模型伪距率的量测方程624.4惯性/卫星组合导航2、紧耦合的数学模型634.4惯性/卫星组合导航2、紧耦合的数学模型644.4惯性/卫星组合导航2、紧耦合的数学模型654.4惯性/卫星组合导航2、紧耦合的数学模型66674.4惯性/卫星组合导航2、紧耦合的数学模型684.4惯性/卫星组合导航2、紧耦合的数学模型694.4惯性/卫星组合导航组合导航的前提——数据预处理1、野值处理2、时间同步3、坐标系统一一、惯性/卫星组合导航的意义二、惯性/卫星组合导航系统的类型三、导航系统误差分析和误差方程四、惯性/卫星组合导航的工作模式五、SINS/GPS先进滤波方法704.5SINS/GPS先进滤波方法4.5.1滤波稳定性研究滤波器发散的定义:

在KALMAN滤波计算过程中，常常有这样一种现象：当量测值数量K不断增大时，按滤波方程计算的估计均方差阵趋于零或某一稳态值，但估计值相对实际的被估计值的偏差却越来越大，从而使滤波器失去作用，这种现象称为滤波器发散。714.5.1滤波稳定性研究滤波器发散的原因:（1） 描述系统动力学特征的数学模型和噪声的统计模型不准确，引起的发散称为滤波发散。（2） 由计算的舍入误差积累引起的滤波器发散称为计算发散。724.5SINS/GPS先进滤波方法4.5.1滤波稳定性研究滤波器发散的解决方法:（1） 计算机飞速发展——通常不再考虑计算误差；（2） 滤波发散的最主要因素——模型不准确，噪声突变（3）捷联惯性/卫星由于长时间直线飞行，系统可观测性差，长时间后系统模型不准确，常常导致滤波发散（4）必须对滤波器进行改造：多模自适应，H∞滤波，混合校正滤波，多级准闭环滤波734.5SINS/GPS先进滤波方法4.5.2混合校正Kalman滤波技术(1)输出校正不修正惯导内部的误差状态。随着时间增加，误差越来越大，导致数学模型与实际系统不吻合，结果组合系统导航精度降低。(2)反馈校正的组合导航系统的导航误差逐渐偏离，由于卡尔曼滤波器在滤波开始到稳定需要一段时间。若在滤波初期将第一步得到的估计值反馈回惯导系统，再将下一步的预测值置零，将导致整个系统的精度降低。两种传统校正方式的缺点:744.5SINS/GPS先进滤波方法4.5.2混合校正Kalman滤波技术SINSDGPS输出校正反馈校正XIXG+-混合校正滤波技术结构图KALMAN滤波器混合校正有效地提高了滤波器的稳定性！754.5SINS/GPS先进滤波方法4.5.3基于系统可观测度分析的自适应反馈校正滤波764.5SINS/GPS先进滤波方法4.5.3基于系统可观测度分析的自适应反馈校正滤波飞行试验陀螺漂移分别为0.103°/h（1σ）、0.112°/h（1σ）和0.137°/h（1σ加计偏置分别为115μg（1σ）、89μg（1σ）和106μg（1σGPS的速度误差为0.1m/s（RMS），单点定位误差为5m（RMS）载波相位差分位置误差为0.05m（RMS）。774.5SINS/GPS先进滤波方法4.5.4多模自适应滤波技术（1） SINS/GPS系统中通常假设系统模型单一且始终不变；（2） 单一的模型难以描述不同工作条件、工作状态下的传感器误差；（3）处理上述问题最有效的方法就是采用多参考误差模型的自适应卡尔曼滤波估计。784.5SINS/GPS先进滤波方法基于模型1的卡尔曼滤波器基于模型1的卡尔曼滤波器基于模型1的卡尔曼滤波器基于模型1的卡尔曼滤波器假设检验算法∑uZ4.5.4多模自适应滤波技术滤波器库794.5SINS/GPS先进滤波方法4.5.5滤波器实时性研究——降维滤波(1)滤波器的计算复杂度与状态维数的三次方成正比；(2)系统状态变量过多——维数灾难；(3)对SINS/GPS进行可观测度分析，有些系统状态变量不可观测；(4)基于系统可观测度分析的降维滤波是解决该问题的有效方法。804.5SINS/GPS先进滤波方法系统状态变量选取可观测度低传统15维状态变量略去可观测度低的四个状态变量814.5SINS/GPS先进滤波方法4.5.5滤波器实时性研究——降维滤波系统状态变量选取11维状态变量略去可观测度低的四个