基于卡尔曼滤波的惯性传感器信号处理毕业论文.doc

基于卡尔曼滤波的惯性传感器信号处理毕业论文目录毕业设计（论文）任务书I摘要IIAbstractIII第一章 绪论11.1 惯性传感器信号处理研究目的及意义11.2 国内外MEMS惯性传感器的现状及发展趋势21.3 惯性传感器信号处理研究概况71.4 本文主要研究内容91.5 本章小结10第二章 MTi惯性传感器组成及卡尔曼滤波原理112.1 MTi组件的工作原理及性能指标112.1.1 微机械陀螺仪122.1.2 微机械加速度计132.1.3 MTi微惯性传感器的输出数据格式152.1.4姿态角的定义162.2 MTi惯性传感器姿态确定方法162.2.1 现在比较常用的定姿方法162.2.2 加速度计定姿方法162.3 卡尔曼滤波原理172.3.1 卡尔曼滤波的产生背景172.3.2 卡尔曼滤波算法182.3.3 卡尔曼滤波器192.4 本章小结20第三章 卡尔曼滤波的惯性传感器信号处理213.1 陀螺仪、加速度计的误差分析及测量方程建立213.1.1 MEMS陀螺信号的特点及影响精度的主要因素213.1.2 陀螺仪模型223.1.3 加速度计误差来源223.1.4 加速度计的测量模型223.2 惯性传感器的状态方程的建立233.3 卡尔曼滤波器设计243.4 基于matlab下的卡尔曼滤波方程的程序开发263.4.1 Matlab在数据处理及图像绘制方面的特点263.4.2 在MATLAB下卡尔曼滤波的程序设计273.5 本章小结29第四章 实验仿真与结果分析304.1 惯性传感器实验数据采集304.1.1 惯性传感器静态实验数据采集304.1.2 惯性传感器动态实验数据采集304.2 仿真结果分析304.2.1 惯性传感器静止状态下的仿真实验314.2.2 惯性传感器运动状态下的仿真实验344.3 本章小结39第五章 结束语40参考文献42致谢45第一章 绪论1.1 惯性传感器信号处理研究目的及意义 惯性传感器是导航定位、测姿、定向和运动载体控制的重要部件，从航天、航空、航海到机器人、汽车等军事、商业领域有着广阔的应用前景。因此，对惯性传感器技术的研究一直是各国研究的热点，也是惯性技术发展的重点。随着惯性传感器技术的发展，惯性导航系统（INS）所具有的自主、不易受干扰、实时输出等优良特性使得其在军事、商业相关领域得到了广泛应用。INS的精度主要取决于惯性传感器的陀螺仪和加速度计的精度。1而一般的惯性传感器所测量的数据存在大量的随机噪声，通过陀螺仪来测量载体的姿态角，其具有稳定和短期精度高的优点，适于测量快速变化的信息，但陀螺所测量的载体姿态角误差会随时间快速累积（漂移），当其长时间工作时漂移会无限增大，导致系统无法正常工作。加速度计与磁强计也可以用来测量载体的姿态，且其测量误差不随时间累积，它静态性能好，适用于测量缓慢变化的信息，但当载体加速度较大时这种方法则无法使用。2随着计算机、人工智能等相关技术领域的发展，微型机器人、微型直升飞机等微小型自主式载体将广泛运用到未来的生产以及人类生活中，而微小型、低成本的导航系统是其关键技术之一，这对惯性传感器提出了新的要求。MEMS (Micro Electro Mechanical System)惯性传感器采用微电子加工技术制造的芯片级惯性传感器，是由微传感器、微执行器、信号处理和控制电路、通讯接口和电源等部件组成的一体化的微型器件系统。其目标是把信息的获取、处理和执行集成在一起，组成具有多功能的微型系统集成于大尺寸系统中，从而大幅度地提高系统的自动化、智能化和可靠性水平。MEMS 器件具有体积小、质量轻、成本低、抗冲击、可靠性高等优点，在汽车、电子、家电、机电等行业以及军事领域有着极为广阔的应用前景。3虽然目前MEMS器件的精度还未达到极限，通过改进硬件设计和制造方法可提高其精度，但是通过硬件制造高精度陀螺仪不近技术难，而且其成本也高。寻求通过对惯性传感器的信号进行采集和处理的方法来提高陀螺仪的输出信号精度，就能降低对惯性传感器硬件的精度要求，进而达到降低成本的目的，这是惯性界一直以来的一个重要的研究方向。 针对这两种传感器的特点需要对其测量采集的信息进行融合，提高惯性传感器的测量信号精度，得到更为可靠的姿态信息。卡尔曼滤波具有较强的抑制干扰的能力，并能使控制方式具有良好的动态性能,在非线性最优估计领域得到了很好的应用。1.2 国内外MEMS惯性传感器的现状及发展趋势 MEMS技术最早由Richard Pfeynman(1965年获得诺贝尔物理奖)在1959年提出设想。1962年硅微型压力传感器问世。1979年Roylance和Angell开始压阻式微加速计的研制。1991年Cole开始电容式微加速度计的研制。惯性传感器包括加速度计(或加速度传感计)和角速度传感器(陀螺仪)以及它们的单、双、三轴组合IMU(惯性测量单元),AHRS(包括磁传感器的姿态参考系统)。MEMS加速度计是利用传感质量的惯性力测量的传感器,一般由标准质量块(传感元件)和检测电路组成。根据传感原理不同,主要有压阻式、电容式、压电式、隧道电流式、谐振式、热电耦合式和电磁式等。1998年,美国CSDL设计研制了最早的MEMS陀螺。同年，Drapor实验室研制了另一种形式的MEMS陀螺。MEMS陀螺是利用震动质量块被基座(壳体)带动旋转时的哥氏效应来传感角速度的原理制成。主要形式有框架驱动式(内、外框架两种)梳状驱动式、电磁驱动式等。IMU由于是MEMS技术组合的微型惯性测量单元，所以很多地方称为MIMU。主要由三个MEMS加速度传感器及三个陀螺及解算电路组成。AHRS则为包括三个磁传感器的IMU，并且依据四元素法进行了解算，直接可输出一个运动体的俯仰角、横滚角和航向角。低精度MEMS惯性传感器作为消费电子类产品主要用在手机、游戏机、音乐播放器、无线鼠标、数码相机、PD、硬盘保护器、智能玩具、计步器、防盗系统、 GPS导航等便携式。由于具有加速度测量、倾斜测量、振动测量甚至转动测量等基本测量功能，有待挖掘的消费电子应用会不断出现。中级MEMS惯性传感器作为工业级及汽车级产品,则主要用于汽车电子稳定系统(ESP或ESC)GPS辅助导航系统,汽车安全气囊、车辆姿态测量、精密农业、工业自动化、大型医疗设备、机器人、仪器仪表、工程机械等。高精度的MEMS惯性传感器作为军用级和宇航级产品，主要要求高精度、全温区、抗冲击等指数。主要用于通讯卫星无线、导弹导引头、光学瞄准系统等稳定性应用；飞机/导弹飞行控制、姿态控制、偏航阻尼等控制应用、以及中程导弹制导、惯性GPS导航等制导应用、远程飞行器船舶仪器、战场机器人等。 用作消费电子类的MEMS惯性传感器，主要要求是单价低、尺寸小、温度范围窄、因而精度要求低，甚至是功能性产品。加速度传感器重量轻、功耗小、一般测量范围110g50g，分辨率2mg10mg，陀螺一般量程在300/s，零偏在500/h1000/h,因此有些公司的产品不给出零偏指标或给出0.1/s0.5/s。目前可以生产MEMS加速度传感器的公司比较多,大多数为半导体，如美国的ADI、Invensense、ST、Freescale、Sensor Dynamics、MSI(ICSensor)、MEMSIC(生产地在中国无锡，图1.1)，欧洲的VTI(图1.2)、Infine；生产MEMS陀螺的公司如美国的ADI、 Knoix、ST(图1.3)，欧洲的Infine、Methes，日本的Murata、National、冲电气、富士通。12月3日，中国上海深迪半导体有限公司发布了消费类的MEMS陀螺，无锡一家公司也在研发中。 图1.1 MEMSIC公司的MEMS加速度传感器产品 图1.2 VTI公司的MEMS加速度传感器产品 图1.3 意法半导体ST公司的MEMS陀螺产品 工业级的惯性传感器大多以模块形式出现，对于应用于工业级芯片级产品,还必须进行处理，包括软件和硬件电路，以及对不同工业环境的适应性，大多数要求价格适中，精度一定优于消费类应用加速度传感器的量程选择比较宽1500g，分辨率1mg3mg，陀螺量程大多250/s以内，零偏在50 /h200/h，汽车级可作为一个工业应用的特殊产品，对其可靠性要求高，同时由于需求数量大，和一般工业要求不一样的是单价要低。生产这些产品的公司有美国的BEI(图1.4)、ADI、Silicon design、Honeywell、Delphi、MSI、Crossbow(图1.5)、Microstrain、欧洲的VTI、Colibry、Bosch、Sensonor，日本的北陆电气、SSS公司，中国的西安中星测控。 图1.4 BEI公司的MEMS陀螺产品图1.5 Crossbow公司的MEMS陀螺产品 军工级或宇航级的MEMS惯性传感器精度要求高、工作温度范围宽(-45125)，某些兵器产品要求抗冲击能力强(10000g20000g)尺寸要比光纤和机械类产品要小。加速度传感器量程范围宽1g5000g，分辨率要0.11mg，甚至更高。陀螺量程要求范围宽20/s1000 /s，频响高，50Hz1000Hz，零偏稳定性为1 /h50/h。目前制造商有美国BEI(图1.6)、Crossbow、Silicon design、Honeywell、Drapor，欧洲公司有Xsens、Sorsonor、Colibry、BAE，日本有SSS公司，中国有西安中星测控(图1.7)、电子26所、航天704所。 图1.6 BEI公司的MEMS陀螺产品 图1.7 中星测控的MEMS陀螺产品 最近六、七年以来，国内对MEMS惯性传感器的研发热度很高，尤其是2005年至2008年，而且大多集中在国内的顶尖研究机构。 过去三年，全球MEMS惯性传感器的发展趋势正向两级化发展，一方面消费电子类应用、应用领域不断拓展、需求迅速提高，引来制造商不断增加，竞争加剧，单价不断下降；另一方面，军用级应用精度不断提高，单价上升很快。 MEMS惯性传感器的发展现状是消费类产品向大规模生产发展、单价越来越低，量产后，仅售不足一美元，而军用与宇航级产品向高精度发展，一个单轴陀螺售价可在34千美元。而工业级、汽车级产品更追求高品质和高可靠性，同时兼顾售价。 展望未来，MEMS惯性传感器的发展趋势主要有以下几个方面： 技术方面：精度将不断提高，以陀螺为例，有替代低精度光纤陀螺的趋势。对消费类应用，更寻求进一步简化制造工艺，降低成本的趋势。同时，集成化也是未来发展的趋势，不仅模块制造商走软件、硬件集成的路子，越来越多的上游芯片厂家也走集成块的技术路线。因而不断有双轴、三轴加计、陀螺芯片问世。 竞争力方面：消费类将竞争最为惨烈，新厂家将不断涌进，比投资、比规模将是必然趋势。上下游相互倾轧、收购、重组将会上演。 合作方面：由于产品细分的缘故全球竞争与合作必然结果。上游厂家希望找到下游客户，下游希望寻找合适的供应商，因而产业联盟可能出现。 应用方面：无疑无论是消费类应用，工业级军工级应用，市场会急骤扩大，应用会越来越广泛。1.3 惯性传感器信号处理研究概况 目前提高惯性仪表和惯导系统的精度主要由两条途径5：一是改进仪表的结构设计和加工工艺，探索新型的惯性仪表；二是利用软件补偿技术来提高仪表实际使用精度。由于单靠不断改进仪表设计和加工、调试精度来提高惯性仪表精度的这种方法，在实践中由于成本，结构复杂度等问题遇到了越来越多的困难，这就使惯性仪表和惯性组件的信号处理技术在误差补偿上的应用越来越被重视，由原来片面追求降低仪表的绝对误差，转为重点保证仪表性能的稳定，使用信号处理补偿技术尽可能减少仪表的随机误差。 陀螺信号处理研究主要集中在静态误差模型，动态误差模型和随机误差的补偿这三方面。陀螺静态模型和动态模型由于其具有确定的误差趋势，因此误差建模已形成了固定模式。而陀螺随机漂移由于其干扰噪声来源的随机性，所产生误差的补偿技术也就较为复困难。国内外在陀螺漂移测试及建模方面做了大量工作。由于从陀螺的物理结构层面建立光纤陀螺仪的数学物理模型，然后对误差进行补偿需要对陀螺的机械结构等物理知识了解颇深，大多数工程师不能够在物理层面进行深入研究。因此，大都采用了时间序列统计建模方法，并引进现代滤波技术进行随机漂移补偿。而传统的IIR滤波，平滑滤波等经典数字滤波技术现已作为辅助分析方法应用在陀螺信号处理中。目前工程上应用较多的有Allan方差方法对光纤陀螺噪声进行分析，ARMA模型、神经网络、小波分析等方法对对光纤陀螺随机漂移进行建模6。下面就这几种信号处理技术的发展作简要介绍。 在国内外各种公司与研究所，Allan方差分析法已被公认为是分析光纤陀螺随机误差的标准实用方法。这是由于光纤陀螺仪的输出随机漂移数据具有与原子钟频率波动极其相近的统计特性，利用噪声的Allan方差与功率谱密度之间存在定量的关系，可以在时域上直接从光纤陀螺仪的输出数据得到光纤陀螺仪中各误差源的类型和幅度。Allan方差标准分析法会在高频带对白噪声、零偏不稳定性、角度随机游走和量化噪声的辨识产生不唯一结果，在低频带对速率斜坡的辨识也不唯一。因此各种对Allan方差的改进也相继涌现，西北业大学发表的交叠式Allan方差7和窗函数Allan方差等方法，并应用到陀螺仪的随机误差辨识中8。 在数学建模方面，目前应用较多的自回归滑动平均(ARMA)方法是利用白噪声驱动下的ARMA模型的线性组合对陀螺仪输出的随机误差进行拟合，通过使用不同阶次的模型，可在时域对陀螺仪随机误差的统计特性进行不同精度的建模。ARMA模型的建立技术相对比较成熟，早在1986年Pandit-Wu提出了一套建立ARMA模型的策略和步骤，简称动态数据系统(DDS)。克服了对陀螺漂移数据进行差分后建模，会丢失许多陀螺本身具有的信息的缺点9。华中理工大学的杨叔子等人对时间序列分析法建模也有较深入的研究，并且有书藉发表,并指出有限阶的ARMA(n,m)模型等价于一个无限高阶的AR()模型10，正是由于AR模型拟合具有快速、简单、易于实时实现的优点，陀螺的输出噪声一般可用阶次充分大的AR模型来描述。Kalman滤波算法在这里被引用进来用于在建立ARMA模型基础上对陀螺的漂移数据进行处理，减少原始信号中在低频段分布广泛的噪声11。这方面的应用可常见于各大学研究所人员发表的文章。对陀螺随机漂移的建模，哈尔滨工程大学的张淑侠教授针对激光陀螺和光纤陀螺的特点，分别建立了ARMA模型；西安电子科技大学的张小娜利用小波分析的方法去除MEMS陀螺信号中的趋势项后，建立ARMA模型，验证了小波分析的优越性；对于中等精度的光纤陀螺，通过采用Allan方差分析法，ARMA模型等方法建立的模型，再通过卡尔曼滤波方法进行滤波估计通常具有较好的误差补偿效果。 由于陀螺漂移信号是弱慢时变、弱非线性的，神经网络在理论上具有以任意精度逼近非线性函数的能力，为陀螺随机漂移的非线性建模提供了一条十分有效的途径。现今陀螺随机漂移神经网络建模常用的方案有两种：1)基于神经网络滤波方法，通过软件滤波方式修正陀螺误差。2)多传感器的数据融合技术，该方法在陀螺温度漂移建模中比较常用，尤其南京航空航天大学在这方面做了一定研究12。 在陀螺信号去噪处理中，小波变换以其优良的多分辨率特性得到了广泛的应用。将光纤陀螺信号进行多尺度分解，根据分解后的各尺度系数进行信号重建，得到多尺度陀螺仪漂移数据。对重建后的各尺度漂移数据进行时间序列建模，可以得到陀螺随机漂移的多尺度时间序列模型。然后在该模型基础之上，使用Kalman滤波对各尺度陀螺随机噪声进行滤波，可以有效地提高光纤陀螺的信噪比。国内一些研究人员将小波神经网络技术应用于陀螺随机漂移建模13。二者结合因此具有较高的逼近、容错能力和较强的全局搜索能力，同时具有所需样本数少，运算量相对较小的优点。1.4 本文主要研究内容通过MTi惯性传感器来测量载体的姿态角度，其中陀螺仪用来测量载体的角速度，角速度经积分处理后得到载体的姿态角度，但角度存在累积误差，当其长时间工作时累积误差会无限增大，导致所测得的姿态角信息不可信从而会导致载体的相关控制系统不能正常工作。用加速度计测量载体的姿态角度，它静态性能好，适用于测量缓慢变化的信息，不适合跟踪动态角度运动。针对这两种传感器的特点需要对其采集的信息进行融合，来得到可靠的姿态信息。卡尔曼滤波具有较强的抑制干扰的能力，并能使控制方式具有良好的动态性能，在非线性最优估计领域得到了很好的应用。图1.8为本文的研究内容简要结构图，即采用卡尔曼滤波器对陀螺仪和由加速度计的测量值进行信息融合，以得到较为精确的载体姿态角度。卡尔曼滤波器角速度陀螺仪真实角度加速度计加速度图1.8本文研究内容结构图 本篇论文的内容安排如下： 第l章：介绍了本文的研究内容、目的及意义，总结概括相关技术的现状及未来的发展趋势。 第2章：描述了MTi惯性传感器的组成及性能指标，具体分析了陀螺仪和加速度计的组成结构及基本工作原理，选取了适合于本文的姿态角确定方法。介绍了卡尔曼滤波产生背景及卡尔曼滤波的主要内容。 第3章：简要分析了陀螺仪和加速度计的误差来源，并建立了相应的测量模型，进而建立姿态角测量系统的状态方程和观测方程，并设计了简单实用的卡尔曼滤波器实现对加速度计和陀螺仪测量信息的数据融合。并编制了相应的卡尔曼滤波器的MTALAB程序。 第4章：通过实验获得惯性传感器在静止及运动下的输出信号数据，将数据代入卡尔曼滤波的MATLAB程序中，利用MATLAB软件进行仿真得到陀螺仪和加速度计通过卡尔曼滤波进行数据融合后的俯仰角以及横滚角的信号图像以及实现消除陀螺仪漂移的仿真图像。第5章 ：根据第四章获得的仿真图像，通过对比卡尔曼滤波前后的姿态角仿真图像，总结出卡尔曼滤波对惯性传感器信号处理的作用效果。并指出本文有待于进一步研究的内容和对惯性传感器精度进一步提高的研究方向。1.5 本章小结本章主要分析了惯性传感器在测量载体的姿态角时存在的问题，并指出可行的解决方法，即利用加速度计和陀螺仪通过卡尔曼滤波器得到姿态角的最优预估值，即获得更精确的姿态角度。同时也总结概括了国内外惯性传感器和惯性传感器信号处理研究的发展现状。最后，叙述了本文所要研究的主要内容，即建立惯性传感器的测量模型和通过MATLAB软件实现对实验数据的卡尔曼滤波数据融合的仿真。46东北大学本科毕业设计（论文）Error! Reference source not found. MTi惯性传感器组成及卡尔曼滤波原理第二章 MTi惯性传感器组成及卡尔曼滤波原理MTi是一个微型的测量姿态和航向的系统(AHRS)，如图2.1所示。14内部处理器功耗低。输出的航向角没有漂移，同时提供经过校准的三轴加速度，角速度以及磁场强度。对于照相机、机器人和车辆等设备的稳定和控制，MTi是一款性能非常优秀的测量产品。具有以下特点： (1)360度全方位输出姿态和航向； (2)长时间稳定性和快速动态响应； (3)输出三轴加速度、三轴转速度和三轴地磁场强度； (4)全MEMS 惯性器件； (5)紧凑设计； (6)更新速率快； (7)接收或者产生同步脉冲信号； (8)集成温度、三维安装误差以及传感器交叉轴影响的补偿。图2.1 MTI惯性传感器2.1 MTi组件的工作原理及性能指标 本文使用的MTi是全MEMS结构，由微惯性测量组合系统和三轴磁强计组成。微惯性测量组单元的3只硅微机械加速度计敏感X、Y、Z三个方向的线加速度；3只硅微机械陀螺敏感绕X、Y、Z三个方向的旋转角速度；3轴磁强计测量地球磁场强度。陀螺、加速度计和磁强计固连在微惯性测量单元装置的支架上，以相互垂直位置进行安装。MTi内部低功耗的DSP进行传感器的数据融合，通过USB串行通讯总线接口实时输出3轴线加速度、角速度和磁场强度信息。由表2.1的MTi的物理指标可见具有高动态、低功耗、体积小、实时性、更新速率高等优点。主要应用于机器人技术、航空宇航、自动仪表、舰船工业和隧道工程。15表2.1 MTi惯性传感器的物理指标性能指标性能参数重量50 g尺寸585822 mm3模拟输出0-3.3V输出电压4.515VDC 功耗360mW I/O接口USB(外接转换器)更新率100Hz512Hz工作温度0+55 存储温度0+55 MTi微惯性测量单元中的陀螺仪和加速度计是其中的关键器件，在本论文中使用的是微机械振动式陀螺、微机械线加速度计。下面将分别进行原理性的阐述。2.1.1 微机械陀螺仪 本文中的MTi系统中微机械陀螺是指振动陀螺(vibratory gyroscope，VG)。微机械陀螺的工作原理：利用高频振动的质量块在被基座带动旋转时产生的哥氏加速度，即利用单晶硅或多晶硅制成的振动质量块，旋转会引起能量在两个振动模态之间转换。振动式陀螺的振动部件受到驱动而工作在第一振动模态(驱动模态)，当与第一振动模态垂直的方向有旋转角速度输入时，振动部件因哥氏效应产生了一个垂直第一振动模态的第二振动模态(又称敏感模态)，该模态直接与旋转的角速度成正比。对敏感模态进行检测，即可检测角速度。16图2.2为振动陀螺的基本工作原理图。其中质量块P固连在旋转坐标系的XOY平面，假定其沿二轴方向以相对旋转坐标系的速度v运动，旋转坐标系绕负z角速度旋转。因哥氏效应产生哥氏力如下所示： (2.1)式中m为物块P的质量。 z O y P v Fcor x 图22振动陀螺工作原理图 由式(2.1)可以看出哥氏力Fcor直接与作用在质量块P上的输入角速度成正比，并会引起质量块在y轴方向的位移，获得该位移的信息，即获得输入角速度的信息。MTi微惯性组合测量单元中的陀螺仪的性能指标见表2.2。 表2.2 陀螺仪的性能指标性能指标性能参数测量范围-300deg/s300deg/s线性0.1%零点稳定性5/s噪声宽带40HzMD5:41d741a605262d6ea5f62d6be79f1ec8 -03.28.08.Altium.Designer.v6.9.DVD.ISO-LCZ.iso2.1.2 微机械加速度计 加速度计是按照惯性原理相对惯性空间工作的。加速度本身很难直接测 量，实际上现有的加速度计都是借助敏感质量变成力进行间接测量，加速度计测量原理基于牛顿第二定律：作用于物体上的力等于物体的质量乘以加速度。换句话说，加速度作用在敏感质量上形成惯性力，测量该惯性力，间接测量载体收到的加速度。在惯性空间，加速度计无法区分惯性力和万有引力，因此加速度计的输出反映的是单位检测质量所受的惯性空间的合力，即惯性力与万有引力之和。17惯性技术领域将单位敏感质量所受的力称为比力，加速度计的输出直接反映比力，因此加速度计也称为比力传感器。图23加速度计的简化模型 图2.3给出一种加速度计的简化模型，由惯性质量块m、弹簧C和阻尼器D组成，敏感轴X沿铅直向上，虚线表示基座无加速度时质量块处于自由状态时的位置，此时质量块的质心定义为位移x的原点。设基座具有沿x方向的加速度a，则在惯性力作用下，弹簧发生形变，产生的弹性力与形变方向相反，阻尼力与形变速率方向相反。 根据牛顿第二定律可得： (2.2)该式在x轴上的投影形式为 (2.3) 对常值加速度a，质量块m运动达到稳态时，所以有： (2.4)由于比力f(specific force)定义为作用在单位质量上的非引力外力，所以F弹/m=f。在简单特例中，F弹是可通过弹簧来测量的，即加速度计可测量的比力f=a+g，所以要对加速度计的输出做适当处理后才能获得基座的运动加速度，在此例中应从f中减去g才能得到a。MTi微惯性组合测量单元中的加速度计的性能指标见表2.3。表2.3 加速度计的性能指标性能指标性能参数测量范围 50 m/s2线性 0.2%零点稳定性0.02m/s2噪声 刻度因子稳定性 0.05m/s2(-25+80)带宽30Hz 2.1.3 MTi微惯性传感器的输出数据格式 MTi有两种数据输出模式：原始数据输出模式和校正后数据输出模式；校正后的数据是在有效地补偿了原始数据的非正交化误差、安装误差、温度变换对硅微器件的影响、刻度因子误差等误差基础上获取的数据。校正后数据为十进制数据，校正后的数据是未经过滤波等其他实时处理而仅将物理标定模型应用于从A/D转换器获取的数据。校正后的数据输出格式如下表2.5所示。本文使用的惯性传感器实验数据均为校正后的数据。表2.5 MTi惯性传感器输出数据TSaccXaccYaccZgyrXgyrYgyrZmagXmagYmagZ2.1.4姿态角的定义 载体在空间中的航向和姿态可以用载体坐标系相对于地理坐标系的运动来表示，运动角称为载体的姿态角。本文所涉及的姿态角为俯仰角和横滚角。18俯仰角：载体的纵向轴和当地水平面之间的夹角，向上为正，向下为负，定义域为-9090。横滚角：载体的横向轴与当地水平面之间的夹角，右倾为正，左倾为负，定义域为-180180。2.2 MTi惯性传感器姿态确定方法 2.2.1 现在比较常用的定姿方法 第一种方法为基于陀螺信号，积分求得姿态角，这种方法的优点是能够全自主的工作，不受外界干扰，缺点是微机械陀螺是振动陀螺，输出的是角速度，积分后得到角度，陀螺给出的姿态角是发散的无法长期使用。因此必须引入外界信息进行修正。为了保证两次修正之间限制其发散速度以保证姿态测量精度，对陀螺精度要求很高，是微机械陀螺很难达到的。19 第二种方法是基于三轴加速度计，利用大重力磁场在地理坐标系和机体坐标系之间的方向余弦转换进行绝对角度解算，得到姿态角。缺点是加速度计的动态响应慢，不适合跟踪动态角度运动，并且受到的干扰很利害，因此无法用于载体姿态的动态测量。 本文采用陀螺仪和加速度计组合定姿的方法，即将上述的方法一和方法二结合起来，这种定姿的方法先用加速度计输出的加速度信息计算MTi的姿态角作为量测值来补偿陀螺的漂移，这种算法即可以保证MTi惯性传感器的定姿的精度，又可以由陀螺保证系统具有较高的动态性和稳定性。在这种工作方式下，采用陀螺仪、加速度计组合通过卡尔曼滤波确定载体的姿态修正了陀螺积分后的角度漂移，提高惯性传感器的精度。2.2.2 加速度计定姿方法 当MTi载体坐标系和地理坐标系重合且处于非加速运动状态时，各轴加速度计的测量输出应为： At=0 0 g当MTi处于任意姿态时，假定加速度计测量值是：Ab=ax ay az则由Ab=TAt ，可得：(2.5)由上式可以得到俯仰角和横滚角的公式 (2.6)2.3 卡尔曼滤波原理2.3.1 卡尔曼滤波的产生背景 根据最优意义的不同，有各种不同的最优估计方法。常见的有最小二乘估计、最小方差估计、线性最小方差估计等。最小二乘估计的最优准则是量测估计误差的内积最小，它仅以量测数据为基准来确定估计，不需要被估计量和量测值的任何统计特性；但由于精度较低，一般仅适用于估计静态量或有确定变化规律的动态量。最小方差估计等于被估计量的条件均值，条件均值的计算需要知道被估计量的条件概率密度。当被估计量和量测的随机值都是正态布时，最小方差估计是量测的线性函数，就是线性最小方差估计。线性最小方差估计的最大方便之处是计算估计时只需要被估计量和量测的一、二阶统计知识，而不需要条件概率密度。最小方差估计和线性最小方差估计都是无偏估计，估计的均方误差阵展小，所以估计误差的方差也最小，都有线性关系的性质；对被估计量来讲，这两种估计都适用于随机变量，而不适用于随机过程。20 从估计随机量的精度和实用性来看，线性最小方差估计是最适合的，但是仍然不适用于利用尽可能多的不同时刻的量测数据对非平稳随机过程的状态进行估计。 为了解决这个问题，二十世纪六十年初，Kalman提出了一种新的最优滤波方法卡尔曼滤波：利用上一时刻的估计，再加上实时得到的量测来进行实时估计。卡尔曼滤波是一种递推线性最小方差估计，因为上一时刻的估计是利用上一时刻和以前的量测得到的，所以这种递推的实时估计就是利用所有的量测数据得到的；其次，卡尔曼滤波把被估计量作为系统的状态，用系统状态方程来描述状态的转移过程，因此，各时刻之间的状态相关函数，就可以根据状态方程的转移特性来描述，解决非平稳随机过程估计的困难。卡尔曼滤波的最优准则与线性最小方差估计一样，每一时刻的估计都使估计均方误差最小。由于采用了状态方程，进行卡尔曼滤波的系统必须是线性的；要用卡尔曼滤波来估计非线性系统的状态，必须对系统进行线性化。 2.3.2 卡尔曼滤波算法设随机线性离散系统的方程(先不考虑控制作用)为21 (2.7) (2.8)式中，X(k)是系统的n维状态向量，Z(k)是系统的的m维观测序列，W(k)是p维系统过程噪声序列，V(k)是m维观测噪声序列，是系统的nn维状态转移矩阵，是np维噪声输入矩阵，H是mn 维观测矩阵。 关于系统过程噪声和观测噪声的统计特性，我们假定如下 (2.9)其中Q(k)是系统过程噪声W(k)的pp维对称非负定方差矩阵，R(k)是系统观测噪声V(k)的mm维对称正定方差阵，而kj是Kronecker-函数。上述离散系统卡尔曼滤波的计算流程为： 计算状态一步预测： (2.10) 状态估计： (2.11) 滤波增益矩阵： (2.12) 一步预测误差方差阵： (2.13) 估计误差方差阵： (2.14) 以上五个式子即为随机线性离散系统卡尔曼滤波基本方程。只要给定初值，根据k时刻的观测值Z(k)，就可以递推计算得k时刻的状态估计值。在一个滤波周期内，从卡尔曼滤波在使用信息和观测信息的先后顺序来看，卡尔曼滤波具有两个明显的信息更新过程：时间更新过程和观测更新过程。式(2.10)说明了根据k-1时刻的状态估计预测k时刻状态的方法，式(2.13)对这种预测的质量做了定量描述。该两式的计算中仅使用了与系统的动态特性有关的信息，如状态一步转移矩阵、噪声输入阵、过程噪声方差阵。从时间的推移过程来看，该两式将时间从k-1时刻推进至k时刻，描述了卡尔曼滤波的时间更新过程。式(2.11)、(2.12)及(2.14)用来计算对时间更新值的修正量，该修正量由时间更新的质量优劣P(k|k-1)、观测信息的质量优劣(R(k)、观测与状态的关系（H）以及具体的观测信息Z(k)所确定，所有这些方程围绕一个目的，即正确、合理地利用观测Z(k)，所以这一过程描述了卡尔曼滤波器的观测更新过程。卡尔曼滤波算法可用方框图表示，如图2.4所示，从图中可以明显看出卡尔曼滤波算法具有两个计算回路：增益计算回路和滤波计算回路。其中增益计算回路是独立计算的，滤波计算回路依赖于增益计算回路。图2.4 卡尔曼滤波算法流程图2.3.3 卡尔曼滤波器卡尔曼滤波器是基于卡尔曼算法的数字信号滤波器。滤波器的输入是系统状态的观测值，输出是系统状态的估计值。卡尔曼滤波器的方框图如图2.5所示：KKZ(k) + + 单位滞后 - +HKk,k-1 图2.5随机线性离散系统卡尔曼滤波器结构图2.4 本章小结 本章介绍了MTI惯性传感器的组成及性能指标，简要说明了陀螺仪和加速度计的简化模型及基本工作原理。卡尔曼滤波是现代控制理论的重要结果之一，它实质上是递推的线性最小方差估计。通过系统的观测向量去估计系统的状态向量，从而达到降低噪声的作用。东北大学本科毕业设计（论文）Error! Reference source not found. 卡尔曼滤波的惯性传感器信号处理第三章 卡尔曼滤波的惯性传感器信号处理3.1 陀螺仪、加速度计的误差分析及测量方程建立3.1.1 MEMS陀螺信号的特点及影响精度的主要因素 在惯性导航系统中，陀螺仪是它的核心敏感元件。所以MEMS陀螺的误差就成为惯性导航系统中的主要误差源。陀螺仪方位基准的精度高低，主要取决于陀螺仪漂移误差的大小。一般来说，陀螺漂移率是指陀螺仪输出量相对理想输出量的偏差的时间变化率，并用单位时间内相对惯性空间的相应输入角位移表示。陀螺仪在开环状态工作时的漂移将会造成方位的偏离随着时间积累，即使在闭环状态工作时漂移也是造成测量误差的主要因素。陀螺漂移误差的大小即漂移角速度的量值，通常称为漂移率。22显然，陀螺漂移率越小，它所提供的方位基准精度越高，所以漂移率是衡量陀螺仪精度的主要指标。 导致陀螺仪产生漂移的因素很多，其漂移误差源有两个方面：一方面是内部原因，即陀螺仪本身原理、结构、工艺不尽完善而形成的各种干扰力矩；另一方面是外部原因，即运载体的线运动和角运动形成的各种干扰力矩。但这些外因仍然是通过内因而起作用的。实际的陀螺漂移率中一般都包含了系统性漂移率和随机漂移率两个部分： (1)系统性漂移率：是指与规定的工作条件有关的漂移率分量，它由与加速度无关 的漂移率和与加速度有关的漂移率组成，用单位时间内的角位移表示。系统漂移率反映了陀螺仪在实际使用条件下的工作精度。一般来说，这类漂移是系统性的或有规律的，它可以用确定性的函数关系来描述，可以采用一定的方法进行补偿，对系统性漂移的补偿已经有很成熟的方法。 (2)随机漂移率：是指在规定的工作条件下漂移率中非系统性的随时间变化的分量，这一类漂移是随机性的，如电子线路噪声、摩擦、温度梯度等引起的干扰力矩就属于这种性质。随机漂移率可以用单位时间内角位移均方根值或标准偏差来表示。这种力矩没有一定的规律性，不能用简单的方法进行补偿，但是我们仍然可以借助数理统计分析方法，找出它的统计规律，并采用统计函数来描述。由于随机漂移在惯导系统中不能用简单的方法加以补偿，故它成为衡量陀螺仪精度最重要的指标。我们对MEMS陀螺的降噪系统的研究就是针对MEMS陀螺中的随机漂移。此外，还有表征陀螺漂移长期稳定性的一种随机漂移率，叫做漂移不定性或逐次漂移率。漂移不定性反映了在逐次启动中系统性漂移率的随机变化值，它将影响到惯导系统对陀螺漂移的补偿精度。3.1.2 陀螺仪模型 陀螺仪能直接测量出载体的姿态角速度，同时陀螺仪又有相应的陀螺误差，其测量模型如下： (2.14)式中，是陀螺仪的测量输出，(t)为陀螺仪所测量的载体的真实角速度，为测量白噪声，这里假设为均匀分布的高斯白噪声，为陀螺仪的漂移误差，通常被描述为如下方程，即 (2.15)式中，初始化条件, ,进行离散化后得到 (2.16)式中，T为总的采样时间，Ts为采样时间，C1和C2由实际拟合误差决定。 但由于本文陀螺仪信号的采样频率一般为100Hz，而在本文中总的采样时间T等于于1000倍的采样时间，因而可以近似认为，即认为陀螺仪的漂移误差始终不变。3.1.3 加速度计误差来源加速度计的误差主要来源于以下几个方面：24 (1)固定偏差：恒定加速度条件下，偏离标准加速度电压标称值的固定值。 (2)比例因子误差：输出电压的变化与输入加速度的变化比。既可以简单表 示成一个比例的形式，也可以表示成测量满量程的百分比。 (3)交叉藕合误差：这类加速度计的输出值的误差，是由于加速度计对正交 于输入轴的加速度极为敏感引起的。 (4)随机误差：该误差是由于加速度计的不稳定性引起的，其中包括由于温度不稳定而产生的误差。(在室温下，偏差范围为-100100mV)、内部可调放大器的初始偏差(范围为-2525mV)，零值偏差(-625625mV)等。 对于固定偏差的补偿，可以通过求30s60s的静态均值来修正。 3.1.4 加速度计的测量模型 由于加速度计的测量值中存在随机误差，那么由加速度计测量值得到作为观测值的姿态角度也会带有高斯白噪声，其模型可以描述如下： (2.17)式中，系统观测值，X_acc和Y_acc分别为X和Y轴上加速度计的输出值，测量白噪声，这里假设为均匀分布的高斯白噪声。3.2 惯性传感器的状态方程的建立 对于具体的应用而言，卡尔曼滤波器存在状态方程和测量方程设计问题。必须综合考虑实际过程和运算的简单易用，使得设计的滤波器切实可行。在设计过程中，状态向量的选取影响到整个状态方程的结构，是卡尔曼滤波器设计的一个关键环节。考虑到角度和角加速度存在导数关系，角度可以用来做一个状态向量，而角加速度不适合作为状态向量，因为其导数无法给出，一个可行的选择就是不直接估计角加速度真值，而估计出陀螺仪的零位偏差b，并以此偏差作为状态向量。建立如下系统方程： (2.18)式中表示俯仰角，b代表陀螺仪偏移，Ugro是测量得到的陀螺仪传感速率，wa,wv是高斯白噪声。 离散化后得到如下的状态方程和观测方程： (2.19)其中，、分别为惯性传感器测得的俯仰角以及横滚角，b代表陀螺仪的漂移误差，T为采样周期。3.3 卡尔曼滤波器设计卡尔曼滤波是R. E. Kalman于1960年提出的从与被提取信号有关的观测量中通过算法估计出所需信号的一种滤波算法。实际上, 卡尔曼滤波是一套由计算机实现的实时递推算法, 它所处理的对象是随机信号, 利用系统噪声和观测噪声的统计特性, 以系统的观测量作为滤波器的输入, 以所要估计值(系统的状态或参数)作为滤波器的输出, 滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的, 根据系统方程和观测方程估计出所有需要处理的信号。它与常规滤波的涵义与方法完全不同, 实际上是一种最优估计方法。将卡尔曼滤波器应用到惯性传感器姿态角的误差补偿中，首先需要确定观测量和状态转移矩阵。由上一节建模的到得离散化方程可得：系统的状态方程： (2.20) 系统测量方程如下 (2.21)其中，状态转移矩阵，输入矩阵，输出矩阵，式中Wk ，Vk为白噪声。过程噪声的协防差矩阵Q如式(2.22)，观测噪声的协方差矩阵R如式(2.23)： (2.22)q_acc和q_gyro分别是加速度计所测得的姿态角和陀螺仪所测的角速度的过程噪声的协方差，不同的取值代表了不同的信任程度, 取决于各自传感器的精确度。R矩阵代表观测噪声的协方差。如果R取值较大，那就意味着加速度计的测量值有很大的噪声。快速而合适的决定Q和R矩阵的值要求对整个实验过程有个很好的理解。在本文设计中通过实验来修正其参数值。状态更新方程及时地由当前系统状态和噪声方差估计出下一步的系统状态(先验估计)；而测量更新方程则负责反馈，将新的测量信号加入已经在状态更新方程中得到的先验估计状态，并最终得到系统状态的后验估计。状态更新方程也可以看成状态预计方程，而测量更新方程则可当成状态修正方程，卡尔曼滤波算法