惯性导航与组合导航论文

惯性导航与组合导航

惯性导航与组合导航

论 文

科任老师：

姓名： 学号： 完成日期：

1

惯性导航与组合导航

摘要：

所谓惯性导航就是利用载体上安装的陀螺和加速度计测量到的载体相对惯性空间的角速度信息和比力信息，通过解算常微分方程组，得到载体在导航坐标系中的位置、速度、姿态和时间信息。在捷联式惯性导航中光学陀螺是我们时常用到的一种惯性器件，光学陀螺又包括激光陀螺、光纤陀螺等等。我们利用陀螺和加速度计得到数据信息，再依靠导航算法对这些数据进行处理，本文主要提到的算法有方向余弦法、欧拉角法、四元素算法以及常用于组合导航系统数据融合的卡尔曼滤波算法。

关键字：

捷联惯性导航、光学陀螺、导航算法、卡尔曼滤波算法

2

惯性导航与组合导航

目录

摘要

一． 陀螺仪的力学特性 ………………………….………….5

1．陀螺仪的进动性 ………………………….………………5 （1）进动性的定义 ……………………………….……….5 （2）进动性的规律 ……………………………….……….5 （3）进动性的应用 ……………………………….……….5 2．陀螺仪的定轴性 …………………………….……….……6 （1）定轴性的定义 …………………………….………….6 （2）定轴性的应用 ……………………………….……….6 3．陀螺仪的章动…………………………………….……….6

二．光学陀螺 ………………………………………………….6

1．光纤陀螺…………………………………………………6 2.循环干涉型光学陀螺………………………………………6 3.激光陀螺 …………………………………………………7 （1）、棱镜式激光陀螺 ……………………………………7 （2）、反射镜式激光陀螺 …………………………………8

三．导航解算 ………………………………………………….8 1、捷联式惯性导航系统算法概述 ……………………………8

（1）、系统的初始化………………………………………8 （2）、惯性仪表的误差补偿 ………………………………9

3

惯性导航与组合导航

（3）、姿态矩阵计算………………………………………9 （4）、导航计算 …………………………………………9 （5）、导航和控制信息的提取……………………………10 2、方向余弦法………………………………………………10 3、欧拉角法…………………………………………………11 4、四元素算法………………………………………………12 （1）、四元素算法基础……………………………………13 （2）、矢量坐标变换的四元素描述 ………………………14 <1>、旋转矢量的坐标变换…………………………14 <2>、固定矢量的坐标变换…………………………14 （3）、四元素和方向余弦的关系…………………………14 （4）、四元素的微分方程及其解…………………………15 （5）、四元素微分方程求解的数值积分法…………………15

5、等效旋转矢量法…………………………………………15

（1）、转动的不可交换性…………………………………16 （2）、等效旋转矢量微分方程……………………………16

四．Kalma滤波算法…………………………………………16

1、卡尔曼滤波的相关原理 …………………………………16 2、卡尔曼滤波的通俗解释 …………………………………16 3、卡尔曼滤波的作用………………………………………19

参考书目 ……………………………………………………20

4

惯性导航与组合导航

一． 陀螺仪的力学特性。 1. 陀螺仪的进动性。 （1）进动性的定义。

所谓“进动性”，就是当陀螺转子以高速旋转时，如果施加的外力矩是沿着除自转轴以外的其它轴向，陀螺并不顺着外力矩的方向运动，其转动角速度方向与外力矩作用方向互相垂直，这种特性，叫做陀螺仪的进动性。例如：对于三自由度陀螺来说，若外力矩绕外环轴作用，陀螺仪将绕内环轴转动；若外力矩绕内环轴作用，陀螺仪将绕外环轴转动。对于二自由度陀螺（没有外框）来说，当强迫其绕第三轴（假想的外框轴）运动时，则陀螺将绕内框轴转动。 （2）进动性的规律。 <1>.进动方向。

进动角速度的方向取决于转子动量矩H的方向(与转子自转角速度矢量方向一致)和外力矩M的方向, 而且是动量矩矢量以最短的路径追赶外力矩。这是三自由度陀螺的情况，如下图。

这可用右手定则判定。即伸直右手，大拇指与食指垂直，手指顺着自转轴的方向，手掌朝外力矩的正方向，然后手掌与4指弯曲握拳，则大拇指的方向就是进动角速度的方向。

对于二自由度陀螺来说，进动角速度的方向也可用右手定则判定。即伸直右手，大拇指与食指垂直，手指顺着自转轴的方向，手掌朝强迫转动的角速度矢量的正方向，然后手掌与4指弯曲握拳，则大拇指的方向就是进动角速度的方向。 <2>.进动角速度

5

惯性导航与组合导航

对于三自由度陀螺来说，进动角速度的大小取决于转子动量矩H的大小和外力矩M的大小。其计算式为 =M/H。 即外力矩愈大，其进动角速度也愈大；转子的转动惯量愈大，进动角速度愈小；转子的角速度愈大，进动角速度愈小。

对于二自由度陀螺来说，其进动角速度的大小取决于转子动量矩H的大小和强迫转动角速度的大小。 （3）进动性的作用。

对于三自由度陀螺来说，利用其进动性，可对自转轴的漂移进行修正或跟踪等；对于二自由度陀螺来说，利用其进动性，可测量运动物体的角速度或角加速度。这些也都广泛地应用于航空、航天、航海等领域。

三自由度陀螺有一个重要的特性——定轴性 1.陀螺仪的定轴性。 （1）定轴性的定义

所谓“定轴性”，就是当陀螺转子以高速旋转时，在没有任何外力矩作用在陀螺仪上时，陀螺仪的自转轴在惯性空间中的指向保持稳定不变的特性。也称为稳定性。其稳定性与以下的物理量有关： <1>、转子的转动惯量愈大，稳定性愈好； <2>、转子角速度愈大，稳定性愈好。

所谓的“转动惯量”，是描述刚体在转动中的惯性大小的物理量。当以相同的力矩分别作用于两个绕定轴转动的不同刚体时，它们所获得的角加速度一般是不一样的，转动惯量大的刚体所获得的角加速度小，也就是保持原有转动状态的惯性大；反之，转动惯量小的刚体所获得的角加速度大，也就是保持原有转动状态的惯性小。转动惯量的大小与转子的质量、形状等因素有关。质量越大，转动惯量就越大；沿外缘分布的物质越多，其转动惯量也越大。 (2)、定轴性的应用

利用陀螺仪的“定轴性”，可以用来测量运动物体的姿态、稳定运动物体的运动方向，测量其方位，等等。因此，在姿态仪表、航向仪表、导航系统、飞行控制系统中都有三自由度陀螺。它广泛地应用于航空、航天、航海等领域。 2. 陀螺仪的章动。

当陀螺的自转角速度w 不够大时，则除了自转和进动外，陀螺的对称轴还会在铅垂面内上下摆动，即q 角会有大小波动，称为章动。

二、光学陀螺。 1.光纤陀螺

光纤陀螺是全固态的光学陀螺。由于所用光电子器件实现了产业化生产，在性能价格比上光纤陀螺优于现有的激光陀螺产品。直到现在，现有激光陀螺产品仍然采用气体激光器和分立元件组成的光学谐振腔。这些光学元件仍然需要采用传统的光学零件工艺生产，成本较高。因此可以肯定，光纤陀螺必将逐步取代现

6

惯性导航与组合导航

有的气体激光陀螺产品。

然而，不能否认的是，现有的光纤陀螺存在着以下的几点缺陷。 1）

导航级产品性能不够稳定，受环境温度影响较大，必须采用温度控制装置，而采用温度控制装置将丧失光学陀螺快速启动的优点，无法和现有的激光学陀螺产品相比较。

2） 3）

战术级产品成本较高，面临着挠性陀螺的挑战，尤其是在我国，这种问题反映的更加明显。

商用级产品体积较大，无法在微型惯性测量组合（MIMU）中应用。

总体说来，目前的光纤陀螺产品在提高性能、降低成本，以及小型化等方面都面临着挑战，在系统结构和所用器件两个方面都应当加以改进。 2.循环干涉型光学陀螺

在循环干涉型光纤陀螺中，采用光纤敏感环取代光纤线圈，使双向光束在光纤敏感环中循环多次传播。该系统方案的结构图如下

循环干涉型光纤陀螺的系统方案

循环干涉型光纤陀螺的主要部件包括：1）低想干长度光源——超辐射发光二极管；2）分束器C1；3）起偏器；4）光电探测器；5）相位调制器；6）定向耦合器C2和C3；7）光纤敏感环。

该系统方案的主要优点有： 1） 2）

使用宽带光源，能够有效地抑制系统中由Rayleigh背向散射Kerr效应和偏振耦合带来的误差;

输出信号的实质是多次循环干涉信号的和，可以看做是多个具有不

7

惯性导航与组合导航

同敏感线圈长度的普通光纤陀螺。

如果能检测到M次循环的信号，就能够在缩短光纤敏感环长度的情况下，达到具有M倍光纤敏感环长度光纤陀螺的水平。同时，由于缩短了光纤敏感环的长度，使得循环干涉型光纤陀螺方案可能采用集成光学器件制作，实现光学陀螺的微型化和低成本。由此可知，循环干涉型光纤陀螺同时具有谐振型和干涉型两类光学陀螺仪的优点，是一种很有潜力的光学陀螺仪方案。

3.激光陀螺。

1、棱镜式激光陀螺在设计上具有的特点： （1）棱镜式环形腔。

（2）He—Ne气体激光器。采用150MHz高频电源激励。不采用直流高电压有以下优点，交流激励电源的幅值稳定性要求较低；由于微波可以透过微晶玻璃直接传入环形腔，在环形腔内不需要设置激励电极，环形腔的结构得到简化，损耗较小；环形腔内没有Langmuir离子流，减小了离子流造成的RLG零偏误差。

（3）光程长度控制装置（PLC）。由密封容器、压电晶体以及镍电阻加热丝组成，反馈控制信号来自环形腔内光强信号。

（4）闭锁阈值控制装置。没有采用光路控制减小背向散射光束的幅值。 （5）抖动偏频装置。抖动频率为77Hz，没有抖动角传感器。执行元件是单E型交流电磁铁。为了产生叠加的随机抖动角速度，在结构中需要安装随机滚动的小钢珠。

（6）读出装置。顺、逆时针方向的部分光束经过合光棱镜形成干涉条纹。采用两只探测器分别读出正、余弦电压信号、用于辨别输入角速度信号方向。

（7）磁屏蔽罩。由于光束在玻璃棱镜中传播，Faraday效应将造成RLG的零偏误差。为此，必须在每个棱镜上安装双层的磁屏蔽罩。

应当指出早期的棱镜式RLG抖动频率较低，导致整周期采样的读出信号频带教窄，在高机动性载体上无法应用。为此，应当考虑增加抖动角传感器，建立非（抖动）整周期采样的读出装置。

2.反射镜式激光陀螺的设计特点。

（1）反射镜环形腔。腔体材料采用低膨胀石英玻璃，不需要恒温控制。三角形环形腔由一块球面镜和两块平面镜组成，球面镜的曲率半径为1~5m。采用多层介质膜反射镜，保障反射率>99%。

（2）直流激励装置。由两个阳极和一个公用阴极组成，目的是保障双向离子流动的对称性。在环形腔中传播一周后，光能的损耗要求小于10^-3。激光介质的功率应能保证环形腔内光束的功率达到mW的量级。为了消除双向光束增益之间的模式竞争，采用的增益介质为氖气和两种氦气的同位素的混合气体，其中

（3）光程长度控制回路。采用专门的探测器测量环形腔内双向光束光强I1、I2的变化，执行元件为压电陶瓷位移装置，通过控制球面镜的平移，调节环形腔

8

惯性导航与组合导航

的周长，保持环形腔内双向光束始终处于谐振状态。

（4）减小闭锁阈值的光路控制。利用PLC控制信号同时控制球面镜的偏转，改变背向散射光束的方向，使得合成的背向散射光束幅值最小。

（5）抖动偏振装置。频率约为400Hz，没有抖动角转换器。

（6）读出装置。由合光棱镜和两块探测器组成，两块探测器分别读出正、余弦电压信号，读出信号的光功率为10uW的量级。

三、导航解算。

1、捷联式惯性导航系统算法概述

捷联式惯导算法是指从惯性仪表的输出到给出需要的导航与控制信息所必须进行的全部计算问题的计算方法，计算的内容和要求根据捷联式惯导的应用或功能要求的不同有很大的差别。一般来说，有以下几个方面的内容： （1）、系统的初始化

系统的初始化包括3 项任务。

1) 给定飞行器的初始位置和初始速度等初始信息。

2) 数学平台的初始对准，确定姿态矩阵的初始值，是在计算机中用对准程序来完成的。在物理概念上就是把\数学平台\的平台坐标系和导航坐标系相重

合，称其为对准。

启动 自检初始姿态矩阵计算 N 迭代次数 Y 导航计算 3) 惯性仪表的校准，对陀螺的标度因数进行标定，对陀螺的漂移进行标定，对加速度计的标度因数标定。

（2）、惯性仪表的误差补偿

对捷联式惯导系统来说，由于惯性仪表直接安装在载体上，因此，载体的线运动和角运动都引起较大的误差。惯性元件的输出首先必须经过误差补偿后，才能将其输出值作为姿态和导航计算信息。

（3）、姿态矩阵计算

不管捷联式惯性导航应用和要求如何，姿态矩阵的计算都是不可少的，可以给出飞行器的姿态和为导航参数的计算提供

控制信息提取 结束

9

惯性导航与组合导航

必要的数据，是捷联式惯导算法中的最重要的一部分，也是捷联式系统所特有的。 (4)、导航计算

导航计算就是将加速度计的输出信息变换到导航坐标系，计算出飞行器的速度、位置等导航信息。 (5)、导航和控制信息的提取

包括飞行器的姿态信息、飞行器的角速度和线加速度等信息。这些信息可以从姿态矩阵的元素和陀螺加速度计的输出中提取出来。左上图是捷联式惯性导航系统算法流程图。 2、方向余弦法

方向余弦矩阵经常被用来在捷联惯导系统分析中描述两个坐标系之间的相对姿态，其优点是易于在两个坐标系之间作矢量转换。

方向余弦的矩阵实质上就是由机体系到导航系的坐标变换矩阵，它有9个元素，解出这九个元素后，利用各个元素和姿态角的函数关系求出三个姿态角。

?R?R?LbLbbLbLL?RbL?bib??iLRb b??ibzb??0?iby?Lb?L??iby??，iL???iLzL???iLy0???L??iLzL??iLyL???iLy?

?0?b?bib???ibzb???iby?0b?ibx0L?iLx0??LLL ?iL??ie??eL在此假设陀螺的输出已经过各种误差补偿，载机的速度和位置数据是解算得出的值。

假设方向余弦矩阵具有如下形式

?R11RbL???R21??R31R12R22R32R13?R23?? R33??bbR13?ibx?R11?ibzbbR23?ibx?R21?ibzbbR33?ibx?R31?ibzLLR32?iLy?R22?iLzLLR12?iLz?R32?iLxLLR22?iLx?R12?iLybb?R11?iby?R12?ibxbb?R21?iby?R22?ibx?

bb?R31?iby?R32?ibx?LL?R33?iLy?R23?iLzLL?R13?iLz?R33?iLx?

LL?R23?iLx?R13?iLy?代入上述方程，得到

bb?R12?ibz?R13?iby?bbRbL?b?ib?R22?ibz?R23?ibybb?R32?ibz?R33?iby?LL?R31?iLy?R21?iLz?LLL?LLRb??R11?iLz?R31?iLxLL?R21?iLx?R?11iLy?这样得到九个常微分方程

bbLLR11?(R12?ibz?R13?iby)?(R31?iLy?R21?iLz) ? 10

惯性导航与组合导航

bbLLR12?(R13?ibx?R13?ibz)?(R32?iLy?R22?iLz) bbLLR13?(R11?iby?R12?ibx)?(R33?iLy?R23?iLz) bbLLR21?(R22?ibz?R23?iby)?(R11?iLz?R31?iLx) bbLLR22?(R23?ibx?R21?ibz)?(R12?iLz?R32?iLx) ????bbLLR23?(R21?iby?R22?ibz)?(R13?iLz?R33?iLx) bbLLR31?(R32?ibz?R33?iby)?(R21?iLx?R11?iLy) ??bbLLR32?(R33?ibx?R31?ibz)?(R22?iLx?R12?iLy) bbLLR33?(R31?iby?R32?ibx)?(R23?iLx?R13?iLy) ??此时就可以利用四阶Runge-Kutta法编程计算，在具体计算时，只需要选择6个方程即可。 3、欧拉角法

描述两个坐标系之间相对姿态的传统方法是欧拉角旋转序列。一个欧拉角序列是给定坐标系绕其坐标轴所做的一系列旋转，这个旋转序列结束后该坐标系指向一个新的姿态。这个最终姿态的具体方位取决于所选欧拉角序列中每一个旋转的幅度和旋转轴。

例如，用欧拉角序列来描述飞行器坐标系相对于当地海平面坐标系的姿态。这个序列包括：关于当地海平面坐标系Z轴的“偏航”旋转，接着是关于平移后Y轴的“俯仰”旋转，最后是关于平移后X轴的“滚转”旋转。因此欧拉角序列是：偏航（关于Z轴），俯仰（关于Y轴），滚动(关于X轴)。

为了以数学形式描述欧拉角序列，可以将旋转矢量的概念应用于相关序列的每一个欧拉角旋转。使用这个方法分析上述例子中的飞行器坐标轴欧拉角序列。首先为这个欧拉角序列定义坐标系如下：

坐标系A——初始当地水平坐标系。

坐标系A1——坐标系A绕Z轴旋转给定偏航角。 坐标系A2——坐标系A1绕Y轴旋转给定俯仰角。

坐标系B——通过将坐标系A2绕X轴旋转给定滚转角后获得的飞行器坐标系。

根据以上坐标系的定义，可以为上述序列中的每一个欧拉角旋转定义三个旋转矢量：

11

惯性导航与组合导航

????ZA,???????YA,???

1????XA,???2其中：?ZA,?YA1,?XA2分别为沿着坐标系A的Z轴、坐标系A1的Y轴和坐标系A2的X轴的单位矢量；?、?、?分别为航偏角、俯仰角和滚转角。

由上述旋转矢量定义可以得到与坐标系定义相关的三个方向余弦矩阵：

AAAACA?I?sin?(??)?(1?cos?)(??)(?ZAZAZA?) 1A1A2A2A2CA?I?sin?(??)?(1?cos?)(??)(?YAYAYA1?) 211A2A2A2A2CB?I?sin?(?XA?)?(1?cos?)(??)(??) XAXA222其中，根据定义有

A?ZA?0??0??1??,?A1??1?,?A2??0? ??0YA1XA2??????????1???0???0???cos????sin???0?cos????0???sin?0?1???0cos???0sin??sin?cos?00?0?? 1??将上式代入之前的三个方向余弦矩阵中即可得显示的方向余弦表达式：

ACA1A1CA20sin??10?? 0cos?????sin??? cos???0A2CB4、四元素算法。 （1）四元素算法基础 <1>·什么是四元数？

四元数一般定义如下：q=w+xi+yj+zk(其中w是实数，x,y,z是虚数)。其中: i*i=-1；j*j=-1；k*k=-1；也可以表示为：q=[w,v]其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量。

四元数也是可以归一化的，并且只有单位化的四元数才用来描述旋转（面向）首

先||q||=Norm(q)=sqrt(w2+x2+y2+z2)

12

因w2+x2+y2+z2=1所以

惯性导航与组合导航

Normlize(q)=q/Norm(q)=q / sqrt(w2 + x2 + y2 + z2)四元数与旋转到底的关系，轴、角描述到四元数的转化:

w=cos(theta/2) x=ax*sin(theta/2) y=ay*sin(theta/2) z=az*sin(theta/2)

其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度，轴角描述的“四元组”并不是一个空间下的东西，首先（ax,ay,az）是一个3维坐标下的矢量，而theta则是级坐标下的角度，简单的将他们组合到一起并不能保证他们插值结果的稳定性，因为他们无法归一化，所以不能保证最终插值后得到的矢量长度（经过旋转变换后两点之间的距离）相等，而四元数在是在一个统一的4维空间中，方便归一化来插值，又能方便的得到轴、角。 <2>四元数的表示方法： 1·Q=(t;x,y,z) 2·Q=(t;V)，V=(x,y,z)， 3·Q=t+xi+yj+zk

<3>四元数之间的乘法运算：

虚数单位之间的乘法：ii=-1,ij=-ji=k(其他的组合也是循环地)。或：A=(a;U)、B=(b;V)、AB=(ab-U·V;aV+bU+U×V)。“U·V”是内积，「U×V」是外积。

PS<详解~> 四元组相乘： Q1=w1+x1i+y1j+z1k=(w1,v1) Q2=w2+x2i+y2j+z2k=(w2,v2)

Q1*Q2=(w1*w2-,w1*v2+w2*v1+v1xv2)

( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k

对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0） （2）矢量坐标变换的四元素描述 <1> 旋转矢量的坐标变换

假定矢量r绕通过定点o的某一轴转动了一个角度θ，则转动四元数为

??Q?cos??sin22 如转动后的矢量用r?表示，则以四元数描述的r?和r间的关系按下式确定

r'?Q?r?Q*

13

惯性导航与组合导航

式中

Q*?cos?2??sin?2。

<2> 固定矢量的坐标变换

如果矢量固定不动，而动坐标系b相对参考坐标系n转动了一个角度，则以四元数描述的矢量在两个坐标系上的分量的变换为

rn?Q?rb?Q*rb?Q*?rn?Q

（3）四元数和方向余弦的关系

以导航坐标系为固定矢量的参考坐标系，以飞行器坐标系为动坐标系，则四元数分量和方向余弦的关系为

222?q0?q12?q2?q32(q1q2?q0q3)2(q1q3?q0q2)???b222Cn??2(q1q2?q0q3)q0?q12?q2?q32(q2q3?q0q1)??2(qq?qq)2(qq?qq)q2?q2?q2?q2?0223010123??13

上式与

?cos?cos??sin?sin?sin?bCg?C?C?C????sin?cos????sin?cos??cos?sin?sin?cos?sin??sin?cos?sin?cos?cos?sin?sin??cos?cos?sin??sin?cos???sin??cos?cos???

式比较，显然两个姿态变换矩阵相等，即对应元素相等，如果已知变换四元数Q的四个元，则可以求出姿态矩阵的九个元素，并构成姿态矩阵。反过来，如果知道了姿态矩阵的九个元素，也可以相应的求出变换四元数的四个元。 （4）四元数微分方程及其解

四元数微分方程的形式为

11bbQ(t)?Q(t)??rb(t)?M*(?nb(t))Q(t)22

其中

Q?t?b?是姿态四元数，nb(t)是飞行器坐标系相对与导航坐标系的旋转角速度

矩阵，解上式微分方程，则

Q(t?h)?e12?tt?hbM(?nb(?))d??Q(t)

其中，Q(t+h)和Q(t)分别表示了飞行器在t+h和t时刻的姿态四元数，h为捷联解算周期。 令

14

惯性导航与组合导航

?0???t?hxb??????tM*(?nb(?))d??????y?????z???x0???z??y???y??z0???x???z????y????x??0??

12对Q(t?h)?e?tt?hbM(?nb(?))d??Q(t)进行泰勒级数展开，并将上式带入式

Q(t?h)?e12?tt?hbM(?nb(?))d?Q(t?h)?{Icos?????????sin}?Q(t)2??2

?Q(t)中，可求得四元数微分方程的解为：

其中

2?????x2???y???z2

将上式截取有限项，得四元数的各阶近似算法 （5）四元数微分方程求解的数值积分法

求解矩阵和四元数微分方程时，人们更乐于用数值积分方法，其中尤以龙格-库塔法(Runge-Kutta)得到了广泛的应用。根据对计算精度的不同要求，又可分为一阶、二阶、四阶龙格-库塔法。

这里直接给出四阶龙格-库塔法的表达式，如下：

1Q(t?h)?Q(t)?(K1?2K2?2K3?2K4)6

其中

1bK1?h?M*(?nb)t?Q(t)211bK2?h?M*(?nb)1[Q(t)?K1]t?h22211bK3?h?M*(?nb)1[Q(t)?K2]t?t222 ，h为捷联解算周期。

可见，四元数微分方程的四阶龙格-库塔解法，需要知道t时刻、t+h/2时刻和t+h时刻的陀螺采样值。求出四元数的各元素之后，将其带入式

222?q0?q12?q2?q32(q1q2?q0q3)2(q1q3?q0q2)???b2222Cn??2(q1q2?q0q3)q0?q1?q2?q32(q2q3?q0q1)??2(qq?qq)2(qq?qq)q2?q2?q2?q2?0223010123??131bK4?h?M*(?nb)t?h[Q(t)?K3]2，即可求得姿态矩阵。

5、等效旋转矢量法

等效旋转矢量法是建立在刚体矢量旋转思想基础上的，与四元数的不同在

15

惯性导航与组合导航

于：在姿态更新周期内，四元数法计算姿态四元数，而旋转矢量法先计算姿态变化四元数，再计算姿态四元数。等效旋转矢量法分两步来完成：①旋转矢量的计算。旋转矢量描述了飞行器姿态的变化。②四元数的更新。四元数描述了飞行器相对参考坐标系的实时方位。 （1） 转动的不可交换性

在力学中，刚体的有限转动不可交换的。如果刚体先绕x轴转动90°，再绕y轴转动90°，和先绕y轴转动90°，再绕x轴转动90°，两种情况的结果是不同的，这就是转动的不可交换性。转动的不可交换性决定了转动不是矢量，也就是两次以上的转动不能相加。

在方向余弦法和四元数法中都用到了角速度矢量的积分，在非定轴转动时，ω矢量的方向是随时间变化的，因此对角速度矢量进行积分是无意义的。只有积分区间很小时，上式才近似成立，从而引入不可交换误差。显然，采样周期必须很小，否则，计算结果中会有较大的不可交换误差，而采样周期太小，使计算机实时计算的工作量增大。为减小不可交换性误差，1971年Johon E.Bortz提出了旋转矢量概念。

（2）等效旋转矢量微分方程(Bortz方程)

等效旋转矢量微分方程为

bb???nb????nb??121?[1?2?sin?2(1?cos?)b]??(???rb)

四、卡尔曼滤波算法。 1、卡尔曼滤波的相关原理

状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。

受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。

卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。 2、卡尔曼滤波的通俗解释

16

惯性导航与组合导航

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 （1）卡尔曼滤波器的介绍 ：

为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg=5^2/(5^2+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：23+0.61*(25-23)=24.22度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

17

惯性导航与组合导航

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值。 （2）卡尔曼滤波器算法 ：

在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随机变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述： X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值： Z(k)=H X(k)+V(k)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态： X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) 其中Kg为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

18

惯性导航与组合导航

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易用计算机编程实现。

3.卡尔曼滤波的应用

（1）在准静态初始化过程中增益确定； （2）基于动态移动的初始化（即“传递对准”）；

（3）使用记载速度传感器进行惯性导航系统输出更新（即多普勒雷达）； （4）基于GPS卫星测量信号的惯性导航系统输出更新。

卡尔曼滤波是一种常用的分析过程，用于根据可获得的系统输出估计未知的系统参数。在惯性导航的应用中，卡尔曼滤波根据可用的惯性导航系统测量值，参照其他导航数据源提供的等效参数值来估计和矫正未知的导航系统惯性误差。这种作为参考的导航数据源通常称为“辅助导航设备”。可用的惯性导航系统输出和辅助设备输出之间的比较产生一个误差信号（称为“测量”），包含了来源于惯性导航系统和辅助设备的综合误差。卡尔曼滤波器处理这个测量信号，分离估计所有重要的惯性导航系统和辅助设备误差。下图给出了一个卡尔曼滤波器在GPS、惯性陀螺和里程计组合导航系统中的示范应用框图。

GPS数据 GPS 局部滤波器1 主滤波器 时间更新 航向角 角速度陀螺 局部滤波器2 里程计 位移量 信息融合

19

惯性导航与组合导航

参考资料：

1.进动性 _百度百科

http://baike.http://www.wodefanwen.com//link?url=pMeLqMZu6L1n1aRLEBz4iXq_dVxV_uQ-Aq1iUq6fkQFUv-27C9AsJOvPHCz-J9t7PhnrM5TLbGHYx5Nbm-7Riq；

2.定轴性 _百度百科

http://baike.http://www.wodefanwen.com//link?url=cu-6WXqRCuw-dQz4yUKyP7lBoVHyI2rCdDfkPItQKdRvUCPJpexLv-_d3jcy4c7Ls3GsVV89wQrGKUaFTYmIKa；

3.卡尔曼滤波_360百科

http://baike.so.com/doc/3054305.html；

4.《光学陀螺系统与关键器件》 章燕申 伍晓明 著 。中国宇航出版社；

5.《应用捷联惯导系统分析》 吴铁军 马龙华 李宗涛 编著。国防工业出版社；

6.《惯性导航与组合导航基础》 刘智平 毕开波 著。国防工业出版社。

20

本文来源：https://www.bwwdw.com/article/64vp.html