惯导系统
小电站-广东作文
2023年2月21日发(作者：汉字笔画顺序表).
可编辑
1绪论
随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能
代替机电稳定系统成为可能。于是，一种新型惯导系统--捷联惯导系统从20世
纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13
号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地
球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-downinertialnavigation)，捷联
（strap-down）的英语原义是“捆绑”的意思。因此捷联式惯性导航也就是
将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需
要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换
为导航参数的一种导航技术。现代电子计算机技术的迅速发展为捷联式惯
性导航系统创造了条件。惯性导航系统是利用惯性敏感器、基准方向及最初的
位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。在工作
时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。它完全是依靠载
体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，
从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。所以它具有
隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得
惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]
1.1捷联惯导系统工作原理及特点
惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。惯导系统
（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系
.
可编辑
统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一
种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。平台式
惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加
速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解
算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体
上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在
计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算
机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯
导系统的根本点。由于惯性元器件有固定漂移率，会造成导航误差，因此，远
程导弹、飞机等武器平台通常采用指令、GPS或其组合等方式对惯导进行定时
修正，以获取持续准确的位置参数。如采用指令+捷联式惯导、GPS+惯导
（GPS/INS）。美国的战斧巡航导弹采用了GPS+INS+地形匹配组合导航。
惯导系统基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参
考系的加速度，将它对时间进行积分，之后将其变换到导航坐标系，得到在导
航坐标系中的速度、偏航角和位置信息等。对捷联惯导系统而言，平台的作用
和概念体现在计算机中，它是写在计算机中的方向余弦阵。直接安装在载体上
的惯性元件测得相对惯性空间的加速度和角加速度是沿载体轴的分量，将这些
分量经过一个坐标转换方向余弦阵，可以转换到要求的计算机坐标系内的分
量。如果这个矩阵可以描述载体和地理坐标系之间的关系，那么载体坐标系测
得的相对惯性空间的加速度和角速度，经过转换后便可得到沿地理坐标系的加
速度和角速度分量，有了已知方位的加速度和角速度分量之后，导航计算机便
.
可编辑
可根据相应的力学方程解出要求的导航和姿态参数来。捷联惯导系统原理方框
图如图1所示。
捷联惯导系统和平台式惯导系统一样，能精确提供载体的姿态、地速、经
纬度等导航参数。但平台式惯导系统结构较复杂、可靠性较低、故障间隔时间
较短、造价较高，为可靠起见，通常在一个运载体上要配用两套惯导装置，这
就增加了维修和购置费用。在捷联惯导系统中，由于计算机中存储的方向余弦
解析参考系取代了平台系统以物理形式实现的参考系，因此，捷联惯导系统有
以下独特优点。
（1）去掉了复杂的平台机械系统，系统结构极为简单，减小了系统的体
积和重量，同时降低了成本，简化了维修，提高了可靠性。
（2）无常用的机械平台，缩短了整个系统的启动准备时间，也消除了与
平台系统有关的误差。
（3）无框架锁定系统，允许全方位（全姿态）工作。
（4）除能提供平台式系统所能提供的所有参数外，还可以提供沿弹体三
个轴的速度和加速度信息。
但是，由于在捷联惯导系统中，惯性元件与载体直接固连，其工作环境恶
劣，对惯性元件及机（弹）载计算机等部件也提出了较高的要求。
（1）要求加速度表在宽动态范围内具有高性能、高可靠性，且能数字输
出。
（2）因为要保证大攻角下的计算精度，对计算机的速度和容量都提出了
较高的要求。
1.2捷联惯导系统现状及发展趋势
.
可编辑
目前，捷联惯导系统已在军民领域被广泛应用，本文仅介绍其在部分飞航
式导弹/炸弹上的应用。对于飞航式战术地地导弹，由于其全程均在稠密大气层
内飞行，且射程远，飞行时间长，容易受到大气干扰的影响，因此，采用捷联惯
导系统是唯一可选的制导方式；对于中远程的空空导弹，因导弹的发射距离远，
具有攻击多目标的能力，捷联惯导系统也是比较理想的中制导方式；中远程地空
导弹的制导方式一般为初始制导+中制导+末制导，其中中制导一般采用具有捷
联惯导系统的组合导航系统；各类反舰导弹采用捷联惯导系统也可简化设计降低
成本，提高性能价格比。
进入20世纪80~90年代，在航天飞机、宇宙飞船、卫星等民用领域及在各种战
略、战术导弹、军用飞机、反潜武器、作战舰艇等军事领域开始采用动力调谐式
陀螺、激光陀螺和光纤式陀螺的捷联惯导系统，尤其是激光陀螺和光纤式陀螺是
捷联惯导系统的理想器件。激光陀螺具有角速率动态范围宽、对加速度和震动不
敏感、不需温控、启动时间特别短和可靠性高等优点。激光陀螺惯导系统已在波
音757/767、A310民机以及F-20战斗机上试用，精度达到1.85km/h的量级。
20世纪90年代，激光陀螺惯导系统估计占到全部惯导系统的一半以上，其价格
与普通惯导系统差不多，但由于增加了平均故障间隔时间，因而其寿命期费用只
有普通惯导系统的15%~20%。光纤陀螺实际上是激光陀螺中的一种，其原理与
环型激光陀螺相同，克服了因激光陀螺闭锁带来的负效应，具有检测灵敏度和分
辨率极高（可达10-7rad/s）、启动时间极短（原理上可瞬间启动）、动态范围极
宽、结构简单、零部件少体积小、造价低、可靠性高等优点。采用光纤陀螺的捷
联航姿系统已用于战斗机的机载武器系统中及波音777飞机上。波音777由于采
用了光纤陀螺的捷联惯导系统，其平均故障间隔时间可高达20000h。采用光纤
.
可编辑
陀螺的捷联惯导系统被认为是一种极有发展前途的导航系统。我国惯性导航与惯
性仪表队伍已经初具规模，具备了一定的自行设计、研制和生产能力，基本拥有
了迅速发展的物质和技术基础。尽管如此，我国和国外先进技术相比，还有相当
的差距[2]。
尽管捷联惯导系统不能避免惯性器件的固有缺点，但由于它具有诸多优
点，因此，目前捷联惯导系统在各类民用的航天飞行器、运载火箭、客/货机及
军事领域的各类军用飞机、战术导弹等武器系统上都已被广泛采用。随着航空航
天技术的发展及新型惯性器件的关键技术的陆续突破进而被大量应用，捷联惯导
系统的可靠性、精度将会更高，成本将更低，同时，随着机（弹）载计算机容量
和处理速度的提高，许多惯性器件的误差技术也可走向实用，它可进一步提高捷
联惯导系统的精度。此外，随着以绕飞行体轴旋转角增量为输出的新型高精度捷
联式陀螺的出现，用以描述刚体姿态运动的数学方法也有了新的发展，将以经典
的欧拉角表示法向四元素表示法发展。
不管惯性器件的精度多高，由于陀螺漂移和加速度计的误差随时间逐渐积
累（这也是纯惯导系统的主要误差源之一，它对位置误差增长的影响是时间的三
次方函数），惯导系统长时间运行必将导致客观的积累误差，因此，目前人们在
不断探索提高自主式惯导系统的精度外，还在寻求引入外部信息，形成组合式导
航系统，这是弥补惯导系统不足的一个重要措施。
组合导航系统通常以惯导系统作为主导航系统，而将其他导航定位误差不
随时间积累的导航系统如无线电导航、天文导航、地形匹配导航、GPS等作为辅
助导航系统，应用卡尔曼滤波技术，将辅助信息作为观测量，对组合系统的状态
变量进行最优估计，以获得高精度的导航信号。这样，既保持了纯惯导系统的自
.
可编辑
主性，又防止了导航定位误差随时间积累。组合导航系统不仅在民用上而且在军
事上均具有重要意义。由于飞船、战术导弹及飞机的惯性导航系统具有精度与
低成本的要求，所以采用捷联式惯性导航方案是十分适宜的.国外有人把捷联式
惯性导航系统列为低成本惯性导航系统。捷联系统提供的信息全部是数字信息，
所以特别适用于各种舰船的数字航行控制系统及武备系统[3][4]。
随着GPS的普及，SINS/GPS组合导航系统显示出巨大的发展潜力。该
组合导航系统由GPS提供三维位置、三维速度和精确的时间信息，系统的核心是
卡尔曼滤波器，它是在线性最小方差下的最优估计。美国海军在海湾战争发射的
"斯拉姆"导弹的惯导系统采用了GPS技术，其命中精度达10~15m之内；美国于
20世纪80年代研制的已在"三叉戟"核潜艇上部署的射程达11110km的"三叉戟
2"D-5战略导弹，采用了CNS/INS(天文导航系统/惯性导航系统)组合导航系统，
其导弹落点圆周概率(CEP)小于185m。
我国的船用捷联惯性技术较航空、航天等其他行业起步晚,与美、法、俄罗
斯等国家相比有较大差距,且西方对我国在该领域的控制也极为严厉。基于捷联
惯性导航系统的诸多优势,我国对船用高精度捷联惯性导航系统的需求十分强
烈。因此,自主研发是我国发展船用捷联惯性导航系统的唯一出路。相信不久的
将来,随着我国更高精度固态陀螺的研制成功以及船用捷联技术的日益成熟,满足
我国各类舰艇要求的捷联式惯性导航系统必将研制成功,并得到大规模装备和应
用[1]。
2.课题研究的关键，采取的技术路线和解决方法
2.1课题研究的大体阐述
本课题研究了捷联惯导系统的算法核心即姿态更新算法，通过MATLAB的数
.
可编辑
学仿真验证算法的可行性，并且设计了一个捷联式惯导系统的仿真平台，其中可
调用MATLAB的仿真函数产生相应的误差曲线。在这里我采用VisualC++语言
来编制这个界面。
本课题的主要研究内容包括以下几个部分:
1姿态更新算法研究
在捷联式惯导系统中，载体的姿态是从载体坐标系到导航坐标系的坐标变换
中直接得到。研究了捷联惯导系统常用的姿态更新算法，比较了各种算法的特点，
采用四元数法进行姿态解算。
中的数学仿真验证姿态更新算法的可行性
通过采集惯性敏感器件的物理量对捷联导航算法进行研究和分析。由于硬件
实现消费太高，在实验室的研究阶段难以实现的情况下，我们可以通过计算机仿
真来检验算法的可行性。即首先构造航迹模拟器的仿真算法，由航迹模拟器提供
惯性导航的比力和角速度信息，并给出航行参数的参考信息(姿态、速度和位置)。
航迹模拟器的输出作为捷联惯导系统算法的仿真输入，以此验证捷联惯导算法的
可行性。
3.仿真平台的设计
捷联式惯性系统仿真算法及其界面设计的要求是，设计用户界面友好、操
作方便和使用稳定的捷联式惯性系统仿真算法。捷联式惯性系统仿真算法及其界
面设计中主要应包括传感器输出发生器仿真、姿态解算仿真、初始对准仿真和动
态误差在线校正仿真。界面设计要解决上述算法的后台程序的仿真结果在前台界
面的传输和合理的表示，这其中要考虑使用VISUALC++在进行界面设计中如
何有效的结合原有的后台程序和使传感器输出的数据、姿态解算的结果、初始对
.
可编辑
准基准和误差水准和动态误差在线校正精度达到对应真实情况下的捷联惯导系
统的技术水平、输入输出精度和误差精度。
2.2课题研究的关键，采取的技术路线和解决方法
1．研究的关键
①界面上可变参数的变量的选取
可变参数的变量包括传感器模型参数，载体运动参数，卡尔曼滤波器结构参
数和初始条件参数等。
②内核程序（仿真算法）与界面的接口设计
包括姿态解算算法仿真、初始对准算法仿真、在线校正算法仿真。要求设计
的仿真程序在windows环境下仿真运行，能仿真各种运动条件，各种传感器模
型参数下的捷联系统误差，并能将误差数据进行曲线处理。
③界面设计
由于vc++6中大量使用了图形和位图来进行界面设计，使用控件来设计界
面辅助系统，如对话框。使用多个不同的类进行文档/视图的设计。使类似界面
设计等综合设计工程相对复杂和难以进行全面控制。此外，仿真算法包含四个相
对独立的仿真功能算法，对界面的功能要求较全面，因而要求界面的设计者充分
为内核算法全面考虑。
2．技术路线和解决方法
①查阅资料，阅读有关捷联式惯性系统的工作原理、结构、算法等的资料，根据
传感器类型等建立模型，来确定可变参数变量的选择。
②在此捷联惯导系统中，姿态矩阵的算法我采用四元数法。以四元数法为基础。
可推导出数学平台的误差方程，然后依数学模型对误差进行分析、研究和补偿。
.
可编辑
2.3捷联式惯性导航系统仿真算法流程图
3.惯性导航系统的基本知识
3.1陀螺仪简介
现代陀螺仪是一种能够精确地确定运动物体的方位的仪器，它是现代航空，
航海，航天和国防工业中广泛使用的一种惯性导航仪器，它的发展对一个国家的
工业，国防和其它高科技的发展具有十分重要的战略意义。所以本章首先阐述陀
螺仪的基本知识。
3.1.1定义
我们将凡是绕回转体的对称轴作高速旋转的刚体称为陀螺[5]。常见的陀螺是
一个高速旋转的转子，回转体的对称轴为主轴或者极轴，与主轴相垂直并在转子
平面内的对称轴称为赤道轴，转子平面称为赤道平面，转子绕着主轴的旋转称为
陀螺转子的自转。
把陀螺仪安装在一个悬挂装置上，使陀螺主轴除了自转外，在空间还具有一
个或两个转动自由度，这就构成了陀螺仪。所以，陀螺仪是陀螺及悬挂装置的总
称。
陀螺仪之所以能在各种载体的导航和稳定系统中成为载体角位移初始信息
的传感器，是因为它具有稳定性和进动性这两个重要特点，这两大特点可以使陀
螺仪的主轴保持在给定的方位上，从而反映安装陀螺仪的载体相对给定方位的角
输出进行计算
读取初始姿态，速度，位置
参数及其误差值；读取时间
常数；读取陀螺及加速度计
误差参数，初始四元数及姿
态矩阵计算
设置常值
.
可编辑
位移和角速度。我们把陀螺仪具有保持给定方位并能反映载体角位移或角速度的
能力称为陀螺效应。具备陀螺效应使得陀螺仪被越来越广泛地应用在航海、航空、
航天以及地质勘探和石油凿井等重要经济部门。
前面我们把陀螺仪定义为陀螺及悬挂装置的总称其实只是狭义的。因为随
着科技的发展，已经有越来越多的物理现象都可以产生陀螺效应，也就是说产生
陀螺效应不一定要有高速旋转的物体。所以，广义的说，凡是能够产生陀螺效应
的装置都可以称为陀螺仪，例如粒子陀螺仪，激光陀螺仪，振动陀螺仪等。
按照转子转动的自由度分成双自由度陀螺仪(也称三自由度陀螺仪)和单自
由度陀螺仪（也称二自由度陀螺仪）。前者用于测定飞行器的姿态角，后者用于
测定姿态角速度，因此常称单自由度陀螺仪为速率陀螺仪。但通常多按陀螺仪中
所采用的支承方式分类。
3.1.2基本特性
1.稳定性
陀螺仪的主轴能相对某一参考坐标系（如惯性空间）保持方位不变，所以陀
螺仪的稳定性又叫定轴性。另外，陀螺仪的稳定性还表现在它能抵抗外界的冲击
干扰作用。
2.进动性
陀螺仪不同于非陀螺体的最主要的特性就是进动性。当陀螺转子高速自转
时，在外环上加外力矩想使陀螺仪绕外环轴转动，可是结果确实陀螺仪并不是绕
着外环轴转动，而是绕着与外环轴垂直的内环轴转动。反过来，当在内环轴上加
外力矩想使之绕内环轴转动时，却发现陀螺仪反而是绕着外环轴转动。象这样的
转动方向与外力矩的作用方向相垂直的性质称为陀螺仪的进动性。在实际使用
.
可编辑
中，可以认为这种进动是无惯性的。
3.陀螺力矩
它是由陀螺仪进动产生的，它作用于使陀螺仪运动的物体上，而不是陀螺仪
本身。
陀螺仪的以上这些特性，使得陀螺仪能够被广泛用来构成各种不同用途的陀
螺仪器和装置。
3.1.3发展概况
1852年法国科学家J.B.L.傅科制作了一套能显示地球转动的仪器，命名为
陀螺仪。陀螺仪于1914年开始作为惯性基准构成飞机的电动陀螺稳定装置。从
20年代起，陀螺仪广泛应用于各种运载体（如船舶、飞机等）上，成为各种运
载体的自动控制、制导和导航系统中测定姿态、角速度、角加速度、方位的重要
元件。40年代，陀螺仪开始在早期导弹上作为制导系统的姿态基准。但是直至
50年代，陀螺仪在构造原理上改进不大，大体上仍沿袭傅科所制作的陀螺仪，
测量精度不高。50年代以后陆续出现陀螺仪转子的液浮、磁浮、动压气浮、静
电悬浮以及挠性支承技术，使陀螺仪的构造得到很大改善，测量精度大大提高。
1975年激光陀螺仪研制成功，它不存在机械摩擦不受重力加速度的影响，承受
振动的能力强，在飞机和导弹的惯性导航系统中得到广泛应用。
3.2常用坐标系简介
习惯上，讨论一个物体相对于另一个物体运动，必须具有与两个物体相关连
的参考坐标系才能确定其位置。惯性导航区别于其它类型导航方案的根本不同在
于其导航原理是建立在牛顿惯性定律的基础上，牛顿惯性定律是在惯性空间内成
立的，这就首先必须要引入惯性坐标系作为讨论惯导基本原理的坐标基准。对于
.
可编辑
在地球表面附近运动的载体，不论是飞机、舰船还是车辆，知道它们相对地球的
地理位置和相对于地理坐标系的首向角及水平姿态角是最重要的，因此必须在运
动物体上获得一个地理坐标系或一个惯性坐标系。陀螺仪最重要的功用之一就是
用它在载体上模拟地理坐标系或惯性坐标系。在惯性技术领域中常用的坐标系有
以下几种[6][7]:
1.惯性坐标系(简称i系)—Oexiyizi
原点在地球的中心Oe,zi轴与地球自转轴重合，向北为正;xi轴和yi轴在赤道
面
上，这里定义为xi轴指向春分点，yi轴与上两轴形成右手系。认为该坐标系与时
间无关，是相对惯性空间无任何运动的一个理想坐标系。该坐标系也称地球固定
坐标系，导航中常用它作为参考坐标系。
2.地球坐标系(e系)—Oexeyeze
地球坐标系Oexeyeze其原点取在地球中心，ze轴沿极轴(地轴)方向，xe在赤
道平面
与本初子午面的交线上，ye轴也在赤道平面内并与xe、ze轴构成右手直角坐标
系。地
球坐标系(e系)与地球固连，随地球一起转动，ze轴和i系的zi轴重合，相对于
i系
的转动角速率为
ie
。
3.地理坐标系(简称t系)—Oxtytzt
原点在机体的重心，xt轴指向东，yt轴指北，zt轴沿垂线指向天，通常称东
北天
.
可编辑
坐标系。对于地理坐标系还有不同的取法，如北西天、北东地等.坐标系指向不
同仅仅
影响某一矢量在坐标系中求取投影分量的正负号不同而已，而不影响研究机体导
航基本
原理的阐述和导航参数计算结果的正确性。
4.机体坐标系(简称b系)—Oxbybzb
机体坐标系是固连在机体上的，其原点在运动体的重心，xb轴指向运动体
纵轴向
前，yb轴指向机翼右方，zb轴垂直Oxbyb平面向上。机体坐标系相对地理坐标
系的方
位为飞行器的姿态和航向。
5.导航坐标系(简称n系)—Oxnynzn
导航坐标系是惯导系统在求解导航参数时所用到的坐标系。对于平台式惯导
来说，
取平台坐标系作为导航坐标系就可以了.对于捷联惯导来说则不同，测量元件测
到的是
机体系中的量，然而导航参数并不在机体系中求解，需要将加速度计测到的量分
解到某
个便于求解导航参数的坐标系中，再进行导航计算。
选择不同导航坐标系及在载体内部的不同实现方法构成了不同的导航方案。
常用的导航坐标系是地理系。需要注意的是当选择导航系为地理系时，若纬度角
接近90度时，会出现发散的情况，这样就不适合全球导航。在高纬度区，单位
.
可编辑
经度角对应地球表面的弧度变短(平台方位变化速度快)，陀螺力矩器接受很大的
控制电流，物理上难以实现。用于极地导航的导航系统取游动方位角为导航坐标
系。由于本文的导航系统不用于极地导航，所以采用地理坐标系作为导航坐标系
就可以了。而且姿态矩阵是载体系与地理系之间的方位关系，采用地理系进行计
算是最方便的。故本文导航坐标系选定为地理坐标系(东北天)。
3.3惯性导航系统简介
3.3.1导航的概念
导航(navigation)就是正确地引导机动载体沿着预定的航线在规定的时
间内到达目的地，为运动物体提供实时的导航参数是导航的基本任务[8]。所以，
导航是一种广义的动态定位，我们把能够提供运动物体位置、速度、航向等运动
状态的系统称为导航定位系统。在古代，人们利用岸上或海岛上的标志性物体和
天空中星体的位置来确定船舶所在的位置。特别是利用北极星来确定方向，早在
17000年以前的古石器时代就发明了利用恒星进行导航的方法。后来，人们使
用磁罗经、计程仪、天文钟和六分仪等进行导航。但这些装置只能在地面和天空
能见度良好的情况下才能使用，并且其观测时间长，速度慢。随着科学技术的发
展，导航技术在军事和民用方面的使用范围及要求也在不断的拓宽和加深。尤其
在军用导航定位和民用运输、测量、监控方面对定位、定向功能要求的大幅度提
高，使得各个国家对发展各种精度的导航定位定向系统变得十分迫切。高效、高
精度的导航系统更是我国这种发展中国家赶超发达国家的战略性资源和倍能器。
在军用方面，随着新时期军事战略方针的转变及高新技术武器装备的发展，导航
定位定向系统已经成为我军现代化建设中一项不可缺少的重要军事技术装备，其
重要性表现在：它是信息战必不可少的基础设备，是建立战场统一坐标的前提，
.
可编辑
是快速、准确火力部署的保障，同时又是实现武器精确打击能力的必要条件。所
以，导航定位定向系统对迅速提高我军的综合作战能力，加快数字化部队建设至
关重要；在民用方面，国外的导航定位定向系统己在大地测量、定向钻并、隧道
掘进、地面车辆导航、飞机进场着陆、航天航空遥感、机载重力测量、公路监测、
地下油气管道监测、矿井监测、激光断面监测等方面得到广泛地的应用，并取得
了巨大的经济效益。
导航的方法有很多，从上个世纪20年代的仪表导航开始，己先后出现了无
线电定位系统、惯性导航系统、多普勒导航系统和卫星定位系统，它们的出现使
导航进入了一个全新的时代。上世纪30年代，出现了无线电导航，首先使用的
是无线电信标和无线电罗盘。功年代初开始研制伏尔导航系统(VOR)。50年代初
惯性导航系统被用于飞机导航。50年代末多普勒导航系统。60年代开始使用远
程无线电罗兰C（LoranC)，同时还研制出塔康导航系统(TACAN)，以及奥米伽
导航系统(omega)。1963年出现了卫星导航系统，以后发展出了GPS全球定位
系统、GLONASS系统、伽利略系统和北斗星系统[9]。目前在车辆、舰船、飞机、
导弹、宇宙飞行器等上广泛使用的导航方法有：航标方法(目视方法)、无线电导
航、天文导航、惯性导航、卫星定位导航等。其中尤以惯性导航和卫星导航以其
全球、全天候的特点得到广泛的推广和应用．
3.3.2惯性导航的概念
什么是惯性导航或惯性制导呢?在运载体上安装加速度计，经过计算(一次积
分和二次积分)，从而求得运动轨道(载体的运动速度和距离)，进行导航的技术，
称为惯性导航[7][10]。在运载体上安装加速度计，用它来敏感、测量运载体运动的
加速度，经过计算(一次积分和二次积分)，从而求得运动轨道(运载体运动的速度
.
可编辑
和距离)，并且产生对运载体运动所需要的控制信号，控制运载体按要求弹道运
动，称为惯性制导。这就是说，惯性制导是对运载体进行测量和控制，使其沿预
定的轨道运动。作为一种自主式的导航方法，惯性导航是完全依靠载体上的设备
自主地确定出载体的航向、位置、姿态、和速度等导航参数。并不需要外界任何
的光、电、磁参数。因此，惯性导航系统具有隐蔽性好、全天候工作能力等独特
优点。对飞行器、舰船和地面移动载体（特别是用于军事目的）等尤为重要。所
以在近三十年来，在航空、航天、航海、交通和大地测量中惯性导航系统都得到
了广泛的应用。近今年来由于捷联技术在惯导系统中的应用为惯导系统在民用领
域中的应用和发展开辟了更广阔的前景。
3.3.3惯性导航的结构
惯性导航作为一个自主的空间基准保持系统，从原理上讲，各种类型的惯
性导航系统都可以用几何学的观点来解释，它应由以下两个分系统所组成：
（1）指示当地地垂线方向的分系统。它是通过测定舰船所在的重力方向，再对
重力偏差角进行修正，以获取大地参考椭球上该点的位置。
（2）保持惯性空间基准的分系统。它是通过指示地球自转轴的方向，来确定地
心惯性坐标系。
有了地球自转轴方向和当地垂线方向之间的几何关系，即可以确定舰船导航
所需的经纬度值。在惯性导航系统中，用加速度计测量当地地垂线的方向，用陀
螺仪测量地球自转轴的方向。把所测到的这些参数连同事先给出的时间、引力场、
初始位置和初始速度一起送入导航计算机，即可实时计算出载体相对所选择的导
航参考坐标系的位置。所以说，两个惯性敏感器（陀螺仪和加速度计）是惯性导
航系统中的核心部分。加速度计又称比力接受器，它是以牛顿惯性定律作为理论
.
可编辑
基础的。在运动体上安装加速度计的目的是，用它来敏感和测量运动体沿一定方
向的比力（即运动体的惯性力与重力之差），然后经过计算（一次积分和二次积
分）求得运动轨迹（即运动体的速度和所行距离）。测量加速度的方法很多，有
机械的、电磁的、光学的、放射线的等等。按照作用原理和结构的不同，惯性系
统用加速度计可分为两大类，即机械加速度计和固态加速度计。
总结：惯性导航系统通常由惯性测量装置、计算机、控制显示器等组成。惯
性测量装置包括加速度计和陀螺仪，又称惯性导航组合。3个自由度陀螺仪用来
测量飞行器的三个转动运动；3个加速度计用来测量飞行器的3个平移运动的加
速度。计算机根据测得的加速度信号计算出飞行器的速度和位置数据。控制显示
器显示各种导航参数。
3.3.4惯性导航的工作原理
从物理意义上来解释，惯性导航是一门信息科学，也可以说它是一个信息
处理系统。根据牛顿惯性定律，当载体相对惯性空间以加速度a运动时，可以用
载体中的加速度计测出作用在单位质量上惯性力和引力的矢量和的大小，即比力
的大小。
MSF=ma–mg(式2.1)
式中SF——加速度计检测质量受到的比力；
m——感受加速度的检测质量；
a——载体的运动加速度；
g——地球的引力加速度。
式2.1表明，通过载体上加速度计测出比力后，在载体内部不必依赖外界信息而
只是通过惯性元件即可测得载体相对惯性坐标系的加速度。当知道了载体的初始
.
可编辑
位置和初始速度后，只要对该加速度进行两次积分便可以分别先后获取该载体定
位所需要的速度和位置信息。
3.3.5惯性导航的分类
惯性导航系统可分为平台式惯性导航系统和捷联式惯性导航系统两大类。前
者是将陀螺仪和加速度计安装在一个稳定平台上，以平台坐标系为基准测量运载
体运动参数的惯性导航系统；后者是将惯性敏感器（陀螺和加速度计）直接安装
在运载体上，不再需要稳定平台和常平架系统的惯性导航系统。
平台式惯性导航系统，根据建立的坐标系不同，又分为空间稳定和本地水平
两种工作方式。空间稳定平台式惯性导航系统的台体相对惯性空间稳定，用以建
立惯性坐标系。地球自转、重力加速度等影响由计算机加以补偿。这种系统多用
于运载火箭的主动段和一些航天器上。本地水平平台式惯性导航系统的特点是台
体上的两个加速度计输入轴所构成的基准平面能够始终跟踪飞行器所在点的水
平面（利用加速度计与陀螺仪组成舒拉回路来保证），因此加速度计不受重力加
速度的影响。这种系统多用于沿地球表面作等速运动的飞行器（如飞机、巡航导
弹等）。在平台式惯性导航系统中，框架能隔离飞行器的角振动，仪表工作条件
较好。平台能直接建立导航坐标系，计算量小，容易补偿和修正仪表的输出，但
结构复杂，尺寸大。
值得注意的是惯性导航系统的导航精度与地球参数的精度密切相关。高精度
的惯性导航系统须用参考椭球来提供地球形状和重力的参数。由于地壳密度不均
匀、地形变化等因素，地球各点的参数实际值与参考椭球求得的计算值之间往往
有差异，并且这种差异还带有随机性，这种现象称为重力异常。正在研制的重力
梯度仪能够对重力场进行实时测量，提供地球参数，解决重力异常问题。
.
可编辑
3.4捷联式惯性导航系统简介
3.4.1概念
“捷联”（strapdown）这一术语的英文原意就是“捆绑”的意思，因此所
谓捷联系统就是将惯性测量的敏感器（陀螺仪与加速度计）直接捆绑在运载体上，
从而可实现运动对象的自主导航目的[2]。陀螺仪作为角速率传感器而不是作为角
位移传感器；加速度计的输入轴不是保持在已知确定方向上，加速度计测量值是
运载体瞬时运动方向的加速度值。通过计算进内的姿态矩阵实时解析计算而得到
一个“数学解析平台”，它同样可以起到机电结合的稳定平台所提供的在惯性空
间始终保持所要求的姿态作用。
现代捷联式惯性导航系统大多数都使用了高速、大容量的数字计算机和一些
新技术，所以许多方面它己有逐渐取代平台式惯性导航系统的趋势。据有关资料
报道，美国军用惯性导航系统1984年全部为平台式，到了1989年已有一半改为
捷联式;战术导弹的惯性制导系统1954年有83%为平台式，而到1989年下降到
34%;战略性导航的惯性制导系统1984年有16%为捷联式，到1989年己经上升到
4%，而民用航空方面1984年有70%为捷联式惯性导航系统，到1989年己上升
到90%;而在航海方面，西德利钦夫公司早在1985年就己推出捷联式平台罗经
[7][11]。
3.4.2工作原理
前面我们已经知道捷联式惯导系统中有两种敏感器件:陀螺仪和加速度计。
陀螺仪组件测取沿运载体坐标系3个轴的角速度信号，并被送人导航计算机，经
误差补偿计算后进行姿态矩阵计算。加速度计组件测取沿运载体坐标系3个轴的
加速度信号，并被送入导航计算机，经误差补偿计算后，进行由运载体坐标系至
.
可编辑
“平台万坐标系”的坐标变换计算。他们沿机体坐标系三轴安装，并且与机体固
连，它们所测得的都是机体坐标系下的物理量。加速度
计测量的是机体坐标系(b系)相对于惯性空间的加速度在机体坐标系中的投影
b
ib
a，该测试量也称为比力。而对于捷联惯导系统，导航计算机要在导航坐标系
中完成，因此，首先要将机体系中的测试量b
ib
a转换导航坐标系中的物理量n
ib
a，
即实现由机体坐标系到导航坐标系的坐标转换。这一转换由姿态矩阵n
b
C完成，
而n
b
C是利用陀螺仪的输出b
ib
即载体相对惯性空间转动的角速率在机体坐标系
下的投影计算得到[6]。姿态矩阵是随时间的变化而不断变化的。另外，从姿态矩
阵中可以单值的确定飞行器的姿态角。捷联式惯导系统中需要实时地求取姿态矩
阵，以便提取飞行器姿态角（首向角、纵摇角、横摇角）以及变换比力。所以说，
在捷联式惯性导航系统中，是由导航计算机来完成具有常平架的稳定平台功能，
既用“数学解析平台”取代稳定平台的功能。它的原理简图如图2.1所示，图中
虚线框部分起了平台的作用。
图2.1
.
可编辑
3.4.3优、缺点
（1）惯性敏感器便于安装、维修和更换。
（2）惯性敏感器可以直接给出舰船坐标系轴向的线加速度、线速度、角速度以
提
供给舰船稳定控制系统和武备控制系统
（3）便于将惯性敏感器重复布置，从而易在惯性敏感器的级别上实现冗余技
术，
这对提高系统的性能和可靠性十分有利。
（4）由于去掉了具有常平架的平台，一则消除了稳定平台稳定过程中的各种误
差；二则由于不存在机电结合的常平架装置，使整个系统可以做得小而轻，
并易于维护。
当然，由于惯性敏感器直接固接于船体上也带来新的问题，即导致惯性敏感
器的工作环境恶化了。由于惯性敏感器直接承受舰船的振动、冲击及温度波动等
环境条件下，惯性敏感器的输出信息将会产生严重的动态误差。这对陀螺仪和加
速度计就有更高的要求。为保证惯性敏感器的参数和性能有很高的稳定性，则要
求在系统中必须对惯性敏感器采取误差补偿措施。另外还需要用计算机对加速度
计测得的飞行器加速度信号进行坐标变换，再进行导航计算得出需要的导航参数
（航向、地速、航行距离和地理位置等）。这种系统需要进行坐标变换，而且必
须进行实时计算，因而要求计算机具有很高的运算速度和较大的容量。
3.4.4分类
捷联式惯性导航系统根据所用陀螺仪的不同分为两类：一类采用速率陀螺
仪，如单自由度挠性陀螺仪、激光陀螺仪（见陀螺仪）等，它们测得的是飞行器
.
可编辑
的角速度，这种系统称为速率型捷联式惯性导航系统；另一类采用双自由度陀螺
仪，如静电陀螺仪，它测得的是飞行器的角位移，这种系统称为位置型捷联式惯
性导航系统。通常所说的捷联式惯性导航系统是指速率型捷联式惯性导航系统。
3.4.5捷联惯导系统的精度
惯性导航和制导系统对陀螺仪和加速度计的精度要求极高，如加速度计分
辨率通常为0.0001g～0.00001g，陀螺随机漂移率为0.01°/小时甚至更低，并
且要求其有大的测量范围，如军用飞机所要求的测速范围应达10的9次方
（0.01°/小时～400°/秒）。因此，陀螺仪和加速度计属于精密仪表范畴。
目前，捷联系统的精度还未达到平台系统所取得的精度水平，还不能完全满
足各种军用和民用的要求，其原因是[12]：
a)新型捷联用的惯性仪表，如动力调谐陀螺仪、激光陀螺仪、光纤陀螺等
漂移达到0.01°／h，石英加速度计的标度因数误差达到1×10-4之后，进一步
提高仪表精度将会遇到加工工艺、材料、光电元器件等方面技术极限的限制，进
一步提高仪表硬件精度将会更加困难，大幅度地追加投资不一定能够收到成比例
的技术效益，同时也会给低成本优势的捷联系统蒙上阴影。
b)捷联系统中的惯性仪表是直接与载体联接，飞行器的恶劣动力学环境如
过载冲击、振动以及机动飞行等都会给惯性仪表和捷联系统带来动态误差。这类
误差比较难以补偿，这也是捷联系统还没有达到平台系统精度水平的主要原因。
c)为了充分发挥捷联惯性系统的技术优势，利用其它系统的高精度测量信
息来补偿和抑制惯性系统随工作时间延长而增长的误差，达到提高导航（制导）
精度的目的，建立以惯性系统为基础，以其它各种测量信息为辅助的组合导航系
统。
.
可编辑
惯性技术的发展表明：从传统的机械转子型陀螺向固态陀螺仪（激光、光纤
和半球谐振陀螺仪）转移并进一步向以半导体硅为基本材料的微机械振动陀螺发
展；从框架式平台系统向捷联系统转移；从纯惯性捷联系统向以惯性系统为基础
的多体制导航组合系统发展，成为今后惯性技术发展的总趋势。
3.5简单介绍使用新型陀螺仪的捷联导航系统
3.5.1用静电陀螺仪的捷联式系统
静电陀螺仪利用电极对球形转子的静电吸力，以及自动调节电极电压的方
法，使球形转子支承在电极中心；并采用光电测量方法测出壳体相对转子极轴的
转角。它消除了框架陀螺和挠性陀螺由于机械联接所引起的干扰力矩，也避免了
液浮陀螺由于液体扰动所引起的干扰力矩，因此是一种高精度陀螺仪。但由于其
工艺复杂，因而成本较高。
静电陀螺仪原理是50年代初提出的，直到70年代末才进入实用。经过逐步改
进，静电陀螺仪精度已高达0.0001°/小时。它特别适合于高精度惯导系统应用，
曾被用于B-52远程战略轰炸机和F-117A隐身战斗轰炸机，用它构成的静电陀
螺监控器现在是核潜艇惯导系统的主要组成部分。
3.5.2用环形激光陀螺仪的捷联式惯导系统
1963年，美国首先向世界公布了激光陀螺概念；但直到1981年，激光陀
螺才首次被用于当时新生产的波音747飞机惯导系统中；接着于1983年开始批
量生产，其间经历了长达20年的研制周期。激光陀螺长期不能进入实用的主要
原因在于材料和加工工艺上的困难。激光陀螺仪是以激光作为工质，以近代物理
学中的萨格奈克效应作为理论基础作成的一种感测角速度的装置。它不使用机械
转子，而是使用沿闭合光路运行的正、反两个激光光束间的谐振频率差，以此测
.
可编辑
定相对惯性空间的转速和转角。激光陀螺由于没有高速旋转的活动件，因而也被
称为固态陀螺仪。激光陀螺具有机械陀螺无法比拟的优点，是捷联惯性系统理想
的元件。自80年代中期至今，在覆盖军用机和民用机的绝大部分飞机捷联惯性
系统中，激光陀螺已处于统治地位。
3.5.3用光纤陀螺仪的捷联式惯导系统
1975年，美国率先在世界上提出了光纤陀螺的设想。至90年代中期，光
纤陀螺开始走向实用，最初用于战术导弹制导及飞机航姿系统中。光纤陀螺是采
用光纤作为光路、并基于萨格奈克效应的一种新型光学陀螺。当陀螺相对惯性空
间旋转时，由相位测量电路提供输出。这种陀螺通常被称为干涉型光纤陀螺，并
由发光二极管、波束分离器、光纤以及相位探测器等部分组成。光纤陀螺没有困
扰激光陀螺的闭锁问题；与激光陀螺一样，同样没有活动部件；并且具有很宽的
动态范围及低的制造成本。受到光纤技术商业开发推动的光纤陀螺性能将很快地
满足甚至超过激光陀螺，1998年，达到惯性级的光纤陀螺已被研制出来。
4.联式惯导系统仿真
4.1系统仿真
捷联惯导系统的最大特点是没有实体平台，即将陀螺仪和加速度计直接安装
在机动载体上，在计算机中实时的计算姿态矩阵，通过姿态矩阵把导航加速度计
测量的载体沿机体坐标系轴向的加速度信息变换到导航坐标系，然后进行导航计
算。同时，从姿态矩阵的元素中提取姿态和航向信息．由此可见，在捷联惯导系
统中平台的作用已由计算机及其软件的作用
代替了，捷联式惯导系统采用的是数学平台。
力学编排就是按照合适的数学模型由观测量
.
可编辑
计算出导航定位参数。具体地讲，利用陀螺仪测得的载体相对于惯性参照系的旋
转角速度，计算出载体坐标系至导航计算坐标系之问的坐标转换矩阵；将测量的
比力(加速度计测量载体相对于惯性空间的线加速度)变换至导航坐标系，并经过
两次积分得到所需的速度位置信息。
4.2程序算法说明
4.2.1姿态矩阵T
舰船的姿态角实际上就是舰船坐标系OXbYbZb和地理坐标系OXtYtZt之间
的方位关系，如图4-1所示。b系相对于t系的角位置可以用一组欧拉角H、来表
示两个坐标系之间的变换关系。按照首向
角、纵摇角、横摇角的定义，可按下列顺序分图4.1姿态角坐标变
换图
三次转动得到。
bbb
b
bbb
b
bbb
ZYOX
OY
ZYOX
OX
ZYOX
OZt
H
OXtYtZt
2
222
1
111
其变换关系为：
Zt
Yt
Xt
HH
HH
Z
Y
X
b
b
b
100
0cossin
0sincos
cossin0
sincos0
001
cos0sin
010
sin0cos
Zt
Yt
Xt
HHHH
HH
HHHH
coscoscossincossinsincossinsinsincos
sincoscoscossin
sincoscossinsinsincossinsinsincoscos
(式4.1)
.
可编辑
即
Zt
Yt
Xt
T
Z
Y
X
b
t
b
b
b
（式4.2）
则有
b
b
b
t
b
b
b
b
b
t
Z
Y
X
T
Z
Y
X
T
Zt
Yt
Xt
1)(
(式4.3)
这是因为矩阵T为正交矩阵，所以有T-1=TT，可用变换矩阵Tt
b
实现由地理
坐标系OXtYtZt到舰船坐标系OXbYbZb的坐标变换。由于方向余弦矩阵Tt
b
的元
素可以单值地确定舰船的姿态角，故矩阵Tt
b
常常称为姿态矩阵，也称为捷联矩
阵[3]。
显然，姿态矩阵Tt
b
中的元素是舰船的首向角、纵摇角、横摇角的函数。若
知道了它们的元素，则可以单值地计算出H、φ、θ的大小。
coscossinsincos
cossincossinsincoscoscossinsinsincos
cossinsinsincoscossinsinsinsincoscos
HHHHH
HHHHH
Tt
b
333231
232221
131211
TTT
TTT
TTT
(式4.4)
4.2.2矩阵的即时修正算法
在船用捷联式惯性导航系统中，舰船在地球表面的地理位置可用地理坐标
系相对地球坐标系的方位关系来表示；而舰船的姿态角则可用舰船坐标系相对于
地理坐标系的方位关系来表示。描述动坐标系相对参考坐标系之间方位关系的方
法有许多种，如三参数法；四参数法；九参数法；三角函数法及等效转动矢量法
.
可编辑
等。姿态矩阵和位置矩阵都可以用上述几种参数来描述。
1.三参数法[7]
三参数法也叫欧拉角法。是欧拉在1776年提出来的。舰船相对参考坐标系
的方位完全可以由舰船坐标系依次绕三个不同的轴转动三个角来确定，就是在第
一章中所提的首向角H、纵摇角φ、横摇角θ。这样舰船坐标系相对地理坐标系
的角速度的矢量ωtb可以表示为：
ωtb=
H（式4.5）
参照图3.1，可将角速度ωtb变为沿舰船坐标系的投影形式为：
H
bz
tb
by
tb
bx
tb
0sincoscos
10sin
0cossincos
（式4.6）
对式3.6中矩阵求逆便可得到：
bz
tb
by
tb
bx
tb
H
1
0sincoscos
10sin
0cossincos
bz
tb
by
tb
bx
tb
sincoscossinsin
sincos0coscos
cos0sin
cos
1
（式4.7）
式4.7便是欧拉角方程。解式4.7便可求得H、φ、θ三个参数，将其代入式4.4便
可求得姿态矩阵t
b
T。
三参数法与其他的算法相比，需求解的方程个数少。用欧拉角法求解得到的
姿态矩永远是正交矩阵，用这个矩阵进行加速度信息的坐标变换时，变换后的信
息中不存在非正交误差，这样用欧拉角法得到的姿态矩阵则不需要进行正交变化
处理。但在用计算机进行数值积分时要进行超越函数的运算，计算工作量加大。
.
可编辑
另外，俯仰角=士90。时，该算法出现“奇点”，所以不适合全姿态飞行，同
时该算法漂移误差较大，一般在应用在平台式惯性导航计算机软件中，而捷联式
姿态运算中很少运用。
2.四参数法
经典力学中古老的问题之一是刚体运动和坐标变换。在平台式惯性导航系统
计算机软件中通常采用欧拉角及其方向余弦矩阵。由理论力学的知识可知，绕定
点转动的刚体的角位置可以通过依次转过三个欧拉角的三次转动而获得，也可以
通过绕某一瞬时轴转过某个角度的一次转动而获得。对于前者可以采用方向余弦
法解决定点转动的刚体定位问题，对于后者可以采用四参数法来解决定位问题。
四参数法也就是使用最广泛的四元数法。四元数理论是数学中的一古老分支，是
由哈密而顿于1943年首先提出的，其思想类似平面问题使用复数解的方式。但
是该理论建立之后长期没有得到实际应用，随着空间技术、计算技术、特别是捷
联式惯性导航技术的发展，四元数的优越性日渐引起人们的重视。其先求解姿态
四元数微分方程，再由姿态四元数确定航向角和姿态角。虽然需要四个微分方程，
较欧拉角微分方程多一个方程，但进行数值计算求解时只需要进行加减乘除运
算，所以求解过程的计算量要比欧拉角法减少得多。它的优势体现在，与方向余
弦法比较，计算量小，存储容量少，仅需要进行简单的四元数规范化处理便可以
保证姿态矩阵的正交性，因而成为一种普遍采用的方法。但是不可避免地引入了
有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动
时，该法会产生严重的姿态漂移误差，所以只能用于工作环境平缓和缓慢的运载
体[2]。
3.九参数法[14]
.
可编辑
九参数法，又称方向余弦法。方向余弦法是用矢量的方向余弦来表示姿态矩
阵的方法，也就是绕定点转动的两个坐标系之间的关系可以用方向余弦矩阵来表
示。方向余弦矩阵是随时间变化的，其变换规律用的数学微分方程来描述，即时
值就可以通过求解该微分方程得到。方向余弦法求解姿态矩阵避免了欧拉角法的
方程退化，可以全姿态工作。但方向余弦矩阵具有九个元素，所以解算矩阵微分
方程时，实际是解算九个联合微分方程，一般来说计算工作量比较大。
4.等效转动矢量法
姿态变换算法可借助有限转动理论直接用于刚体的定位，刚体从初始位置转
动至给定位置，可以认为是将刚体的姿态从初始位置固连的定坐标系变换到与给
定位置固连的动坐标系。在捷联式惯性系统中，载体的姿态具有从载体坐标系到
导航坐标系的坐标变换中直接得到的。等效转动矢量法是建立在刚体矢量旋转基
础上，在自然姿态更新周期内，使用了陀螺的角增量信息。该方法求解的是姿态
变化四元数微分方程，而不是姿态四元数微分方程，这是与四元数的根本区别，
在解四元数微分方程时要用到等效旋转矢量的概念，该法计算量与四元数法计算
量接近，但因能够对不可交换性误差做有效补偿，
所以算法漂移比四元数小，是先进捷联式惯导中普遍使用的算法。
4.2.3四元数法
本课题采用姿态更新算法是四元数法，下面详细阐述四元数法的研究结果。
1基本概念
①.定义
一般四元数是一个带实系数的一阶四元数组，是一个四维空间中的矢量，四
元数Q可以写成如下的形式：Q=q0+q1i+q2j+q3k(式
.
可编辑
4.8)
式中q0、q1、q2、q3、为四个实数，i,j,k为四元数的三个虚数单位的基，作为一
个正交的基。四元数的基具有双重性质，因此有些书中又将其称为超复数。
②.两个四元数乘积的几种形式
设两个四元数为：
Q=q0+q1i+q2j+q3k
p=p0+p1i+p2j+p3k
QP=(q0+q1i+q2j+q3k)(p0+p1i+p2j+p3k)
=q0p0-p1q1-q2p2-q3p3+q0(p1i+p2j+p3k)
+p0(q1i+q2j+q3k)+
321
321
ppp
qqq
kji
（式
4.9）
1）矢量形式
四元数Q、P可以分成标量和矢量两部分，即
Q=q0+qP=p0+p
式中q0、p0----Q、P的标量部分
q、p----Q、P的矢量部分
这样，式3.9可以写成矢量形式为：
QP=q0p0-qp+q0p+p0q+qp
(式4.10)
2)四元数形式
.
可编辑
把式3.8展开整理得：
QP=q0p0-q1p1-q2p2-q3p3+(q0p1+q1p0+q2p3-q3p2)i
+(q0p2+q2p0+q3p1-q1p3)j+(q0p3+q3p0+q1p2-q2p1)k
=n0+n1i+n2j+n3k(式
4.11)
式中n0=q0p0-q1p1-q2p2-q3p3
n1=q0p1+q1p0+q2p3-q3p2
n2=q0p2+q2p0+q3p1-q1p3
n3=q0p3+q3p0+q1p2-q2p1(式
4.12)
式3.11即为两个四元数乘积的四元数形式。
3）矩阵形式
若把四元数Q、P、N的四个元写成矢量形式，既表示为：
Q(q)=Tqqqq
3210
P(p)=Tpppp
3210
N(n)=T
nnnn
3210
则式3.12可以写成矩阵形式为：
3
2
1
0
0123
1032
2301
3210
3
2
1
0
p
p
p
p
qqqq
qqqq
qqqq
qqqq
n
n
n
n
或N(n)=M(q)P(p)(式
4.13)
则式3.12可以写成另外一种形式为：
.
可编辑
3
2
1
0
3113
2032
1301
3210
3
2
1
0
q
q
q
q
qppp
qppp
qppp
pppp
n
n
n
n
或N(n)=M*(p)Q(q)(式4.14)
2.四元数在刚体定位中的应用
1)转动的四元数变换[2]
在刚体定点转动理论中，根据欧拉定
理，
动坐标系相对参考坐标的方位，等效于动
坐标系统某一瞬时的欧拉旋转轴On转动
一个角度α（见图4.2），On轴称为欧拉轴，角度α有时也叫欧拉角。
若用n表示欧拉轴向的单位矢量，则动坐图4.2四元数的物理概念
标系的方位完全可以由n和α两个参数来确定，即可用n和α两个参数构成一个四
元数。
Q=cos(α/2)+nsin(α/2)=en)2/1((式4.15)
如果把n写成分量形式，则：
Q=cos(α/2)+nxsin(α/2)i+nysin(α/2)j+nzsin(α/2)k
=q0+q1i+q2j+q3k(式
4.16)
式中q0=cos(α/2),q1=nxsin(α/2),q2=nysin(α/2),q3=nzsin(α/2)
显然，式3.15的四元数是单位四元数，即
h=N=
12
3
2
2
2
1
2
0
qqqq
这样的四元数又叫做“规范数”的四元
.
可编辑
数；用来描述刚体定点转动的四元数，即式3.15被叫作变换四元数。
2).矢量坐标变换的四元数描述
一个矢量r在t坐标系轴向分量形式为：
r=rtxit+rtyjt+rtzkt
式中rtx、rty、rtz为r在地理坐标系轴向的分量；it、jt、kt为地理坐标系轴向的
单位矢量。可用rtx、rty、rtz来表示r的四元数形式为：
Rt=0+rtxi+rtyj+rtzk=0+r
Rt就叫做矢量r在地理坐标系上的四元数映象。i、j、k是四元数的虚数单位，
而r则是四元数的矢量部分，显然，如果认为i、j、k和it、jt、kt重合，则四元数
的矢量部分就是三维空间的矢量r的本身。
假定矢量r绕通过顶点"O"的某一瞬时轴转动了一个角度α，则和矢量固联的
动坐标系和参考坐标系之间的变换四元数为：
Q=cos(α/2)+nsin(α/2)
式中n---转轴方向的单位矢量
转动前的矢量用r表示，转动后的矢量用r′表示，则r′和r的关系可由四元数来
描述，即
r′=Q•r•Q*(式4.17)
下面来研究固定矢量坐标变换的四元数描述。若一个固定参考矢量r在地理坐
标系（t系）和舰船坐标系（b系）内的分量形式为：
r=rbxib+rbyjb+rbzkb
=rtxit+rtyjt+rtzkt
若用变换四元数Q表示b系和t系之间的变换矩阵，则按矢量转动的式4.17有：
.
可编辑
ib=Q•it•Q*
jb=Q•jt•Q*
kb=Q•kt•Q*
故r=rbxib+rbyjb+rbzkb
=rtxit+rtyjt+rtzkt
=rtxQ•it•Q*+rtyQ•jt•Q*+rtzQ•kt•Q*
=Q•(rtxit+rtyjt+rtzkt)•Q*
如果Rt=rtxit+rtyjt+rtzkt
Rb=rbxib+rbyjb+rbzkb
则有
Rt=Q•Rb•Q*(式
4.18)
或
Rb=Q•Rt•Q*(式4.19)
式4.18与式4.19即为固定矢量坐标变换的四元数描述。
3.转动四元数Q的微分方程
四元数的实时值可以通过求解四元数微分方程得到。
图4.3转动四元
数Q的变换
设在t时刻动系OXbYbZb以角速度ω相对定系OXtYtZt转动，则动系b和定系t之间
的变换四元数Q同样是时间的函数，可用Q(t)表示t时刻的四元数，Q(t+Δt)表示
.
可编辑
t+Δt时刻的四元数，如图3.3所示。
从图3.3可以看出，Q(t)和ΔQ(t)之间和为Q(t+Δt),故有：
Q(t+Δt)=ΔQ(t)Q(t)（式4.20）
或
Qc（t+Δt）=Qc(t)•ΔQc(t)（式4.21）
考虑Δt很小，在Δt时间内动坐标系转动的角度为Δα，则有
ΔQ(t)=cos(Δα/2)+nsin(Δα/2)≈1+n(Δα/2)
由于Δα是由ω在Δt时间内转过的角度，故
nΔα=ωΔt
其中ω表示为：
ω=ωbxib+ωbxjb+ωbzkb
=ωtxit+ωtyjt+ωtxkt
=ωt
由于ωb=ωbxit+ωbxjt+ωbzkt为的“映像”，故有：
ΔQc(t)=1+(ωbΔt)/2
Q(t)=Qc(t)
将上式代入式4.21中得：
ΔQc(t+Δt)=Qc(t)•2/)(1t
b
=Qc(t)+Qc(t)(ωbΔt)/2
我们定义
)(tQc≌lim
t
tQttQcc
)()(
=Qc(t)ωb/2(式4.22)
若利用式4.20，则有
ΔQ(t)=1+(ωtΔt)/2
.
可编辑
所以
Q(t+Δt)=(1+
2
1
ωtΔt)•Q(t)
取
)(tQ=lim
t
tQttQ
)()(
=Q(t)ωt/2(式4.23)
式4.22和4.23为四元数微分方程，是四元数相乘。如果写成矩阵的形式就是：
)()(
2
1
)(
b
QqMqQ
或
)()(
2
1
)(*qQMqQ
b
3
2
1
0
3
2
1
0
0
0
0
0
2
1
q
q
q
q
q
q
q
q
bxbybz
bxbzby
bybxbx
bzbybx
(式4.24)
或)()(
2
1
)(qQMqQ
t
即
3
2
1
0
3
2
1
0
0
0
0
0
2
1
q
q
q
q
q
q
q
q
txtytz
txtzty
tytxtx
tztytx
(式4.25)
4姿态更新算法[6]
利用四元数的链乘规则可写出姿态四元数更新的递推式
)1(
)(
)1()(mbn
mbn
n
b
n
b
qmqmq（式4.26）
其中
)(mqn
b
是tm时刻的姿态四元数；
)1(mqn
b
是tm-1时刻的姿态四元数；
.
可编辑
)1(
)(
mbn
mbn
q是以n系为参考坐标系时，b系从tm-1时刻到tm的变换四元数，它的计算
和)(tb
nb
有关。
)1(
)(
mbn
mbn
q的计算利用到载体坐标系相对于导航坐标系的转动角速度)(tb
nb
，记是
从tm-1时刻到tm时刻载体坐标系b相对于导航坐标系n的等效旋转矢量，则：
dtttCtdttn
m
Tn
b
m
m
m
m
b
nb
b
nbm
)())(()()(
11
（式4.27）
由于)(tn
m
相对于b
nb
来说变化很小，若认为Tn
b
tC))((和)(tn
m
在
mm
ttt
1
时间
内基本不变，并取为tm-1时刻的值，则有
m
n
m
Tn
b
m
m
b
nbm
TmmCdtt)1())1(()(
1
（式4.28）
其中)1(mCn
b
是tm-1时刻的姿态矩阵，)1(mn
m
是tm-1时刻导航坐标系相对于惯
性空间的转动角速度，都是已知量，并且记
dttm
m
b
nbm
)(
1
（式4.29）
m
的计算可用不同子样数角增量算法，得到
m
利用变换四元数和等效旋转矢量
之间的转换关系是：
2
sin
2
cosq
（式4.30）
可求得)1(
)(
mbn
mbn
q，完成姿态更新。
4.3捷联式惯性导航系统的初始对准
从惯性导航系统的原理可知，舰船的位置是由系统中的加速度计测得的加速
度经两次积分而求得。要进行积分必须知道初始条件，例如初始速度和初始位置。
而对捷联式惯性导航系统中初始对准的另一个关键问题是要在较短的时间内以
一定的精度确定出从载体坐标系到地理坐标系的初始变换矩阵）（0t
b
T。这是因为
在捷联系统中惯性敏感器直接测量得到的是载体相对惯性坐标系的各轴向的加
速度大小，而捷联系统数学解析平台软件程序中所需的应是载体坐标系相对于地
.
可编辑
理坐标系的加速度信息。
舰船导航用的惯性系统大多选用与地球固联的参考坐标，所以借助于惯性
敏感器测量的两个在空间不共线的矢量，即地球自转速率ωie和地球重力矢量g
来实现自主式对准是很方便的，它包括水平对准和方位对准。
4.3.1捷联惯导系统初始对准的主要特点
1．大的初始不对准角
捷联惯导系统直接安装在基座上，其不对准角由基座的姿态和航向决定，
一般不能将不对准角视为小量，这就带来数学处理上的麻烦。事实上捷联惯导系
统初始对准的目的是测定惯性测量系相对导航坐标系的方向余弦矩阵。
2．对瞬时方向余弦阵的测定
捷联式惯性系统与平台式惯性系统的另一个主要差别就是前者在初始对准
时，必须求出瞬时方向余弦阵，而不是平均方向余弦阵。这是因为对捷联式惯性
系统来讲，基座的角运动不能被隔离，因此必须考虑上述基座运动对误差角速度
测定精度的影响。
3．有利于在对准过程中标定惯性仪表的参数
捷联式惯性系统与平台式惯性系统相比，能够输出更多的可用信息，这样，
捷联式惯性系统就能在对准过程中标定更多的惯性仪表误差系数。以双位置对准
为例，捷联式惯性系统能标定三个陀螺常值漂移和三个加速度计零偏，而对平台
式系统则只能标定两个陀螺漂移和一个法向加速度计零偏。
4．有更多的可用信息。
对于平台系统，只有加速度计输出可直接用于初始对准，陀螺输出要提供
平台稳定回路，而沿平台轴的输出信息只能从同位器获得，但同位器的分辨率太
.
可编辑
低，不能满足初始对准的精度要求。与此相反，捷联陀螺的信息具有极高的角分
辨率，可直接用于初始对准。这是捷联系统优于平台系统的地方，对方位陀螺的
标定尤为有利。
4.3.2初始对准的一般要求
惯性导航系统的初始化包括:给定初始速度和初始位置，惯导平台的初始对
准，陀螺仪的测漂和定标。第一项任务比较简单。第三项任务在陀螺性能比较稳
定的情况下，不一定每次启动都要进行。而平台的初始对准，则每次启动进入导
航工作状态之前都要进行，而且要求对准精度高，对准时间短。对准精度的高低，
直接影响导航性能。对于平台系统，初始对准就是使平台坐标系向导航坐标系对
准，对准过程是一个物理过程。而对于捷联系统，初始对准就是确定初始时刻的
姿态阵)0(t
b
C。对准的方法有两类:一类是将外部参考基准通过光学或机电的办法
引入平台:另一类是利用惯导系统本身的加速度计和陀螺仪测量重力加速度g和
地球自转角速度
ie
进行自主式对准。由于平台系统和捷联系统基本原理相同，
因此，它们的自对准原理也是相同的。不同的仅是实现方法上有差异[19][20]。
初始对准的要求包括精度和快速两个方面。为了满足精度要求，希望惯性敏
感器具有尽可能高的精度和稳定性，并希望系统能对外界干扰不敏感，即整个系
统的鲁棒性要好。为提高捷联系统的精度，还希望初始对准能对陀螺漂移、加速
度计零位误差以及它们的标度系数进行测定和补偿。要使系统具有较好的抗干扰
能力，还应采用频谱技术、滤波技术，将有用信息和干扰信息从时域和频率域上
加以分离。显然上述措施的实现，都需要容量大、速度快的计算机给以保证。很
明显，精度和快速性这两方面的要求是矛盾的，因此需要合理地进行系统设计，
尽可能兼顾这两方面的要求，以期求得满意的效果。一般性的设计原则是在保证
.
可编辑
初始对准精度的前提下设法缩短对准时间。
4.3.3初始对准方法
从目前国内外有关资料和相关报导来看，按照不同的分类方法，捷联惯导系
统初始对准大致有以下几种分类:
1．按是否利用外观测信息来分，有自主对准与非自主对准。自主对准是利用系
统本身的惯性元件，结合系统作用原理，自动进行对准的方法。非自主对准则要
靠外部参考进行对准，这一过程要有地面设施的支持，平时费时、费力，战时缺
乏有效的机动和灵活性。自主对准加强了惯导系统的自主性、隐蔽性，在现代战
争中具有非自主对准不可代替的作用。而在惯导系统中，从其功能的完善性和使
用的方便性出发，都要求在定位的同时具有自主定向，即对准的功能。传统的导
航，即定位的概念正在被新的综合性定位定向概念所取代。未来的导航系统应当
都具有自动对准的能力。因此，自主对准技术以其重要的价值和意义，越来越引
起了广泛的重视和研究.但是，从控制理论观点分析，自主对准技术的基本困难
是系统不完全可观测。
2．按阶段来分，初始对准有粗对准和精对准两个阶段。粗对准阶段用重力矢量
g和地球自转速率ωie的测量值，直接估算载体坐标系到地理坐标系的变换矩阵，
大约需要一分钟，这样对准的精度为:方位角在几度以内，两个水平角在1度以
内。粗对准阶段也可采用传递对准或光学对准的方法。捷联惯导系统精对准是粗
对准的继续，这个阶段的主要任务是:通过处理惯性敏感器的输出信息，精确校
正计算参考坐标系与真实参考坐标系之间的小失准角，从而建立起准确的初始变
换矩阵）（0t
b
T，为导航计算提供精确的初始条件，以便正常地进行导航工作。
.
可编辑
4.3.4对准原理
在静基座情况下，可以直接利用地理坐标系上重力矢量g和地球自转速率
ωie的已知值以及惯性敏感器对g与ωie的量测值gb、b
ie
来计算捷联矩阵）（0t
b
T。
用g、ωie可以构成一个新的矢量E：
E=gωie
根据载体坐标系与地理坐标系的变换矩阵t
b
T可以得：
g′=t
b
Tgb
bt
b
t
b
ie
t
b
t
ie
ETE
T
（式4.31）
考虑到捷联矩阵t
b
T具有正交性，即有：
1)()(t
b
xt
b
TT
故t
b
T可表示成：
t
b
T=
xb
xb
ie
xb
xt
xt
ie
xt
E
g
E
g
)(
)(
)(
)(
)(
)(1
（式4.32）
式中gb、b
ie
是由加速度计、陀螺仪直接测得的，而Eb、Et应由式3.31计算得
到。
实际应用中，捷联系统工作在具有各种噪声扰动的动态环境重，直接利用式
4.32的静基座算法将会产生很大的误差。我们应该采用稳定系统控制原理，以
重力矢量g和地球自转速率ωie为控制信号，使捷联系统稳定在地理坐标系上。
捷联矩阵t
b
T可以形象地理解成数学解析平台，系统中的数学解析平台的水平及
方位失准角作为系统的负反馈以一定的控制算法，使数学解析平台工作在具有较
好的动态特性和稳态特性的工作状态。
.
可编辑
数学解析平台的工作过程与稳定平台式系统类似，当数学解析平台有方位
失准角γ时，则地球自转角速率分量ωiecosφsinγ就耦合到解析平台的东轴上，
从而产生了罗经效应。其中罗经效应项ωiecosφsinγ就使数学解析平台绕东西
轴旋转，从而产生水平倾角α。解析系统利用这一控制信息，一方面控制解析平
台减小水平倾斜角；另一方面又控制解析平台减小方位失准角，从而使数学解析
平台能稳定在地理坐标系上。其稳定过程是以如下数学公式进行的。
bk
tb
t
b
t
b
TTˆˆˆ
（式4.33）
式中t
b
Tˆ表示计算地理坐标系（tˆ系）和载体坐标系（b系）间的方向余弦矩阵，
即为计算的捷联矩阵，它同样起着平台作用。
对式4.33不断积分，形成了解析平台在地理坐标系上的稳定过程。
捷联式惯性导航系统的计算姿态矩阵t
b
Tˆ与真实姿态矩阵t
b
T的误差，可以直
接由数学解析平台的不对准误差角来表示。当误差角较小时，可以通过Φt来表
示t
b
Tˆ与t
b
T之间的误差，称Φt为捷联系统中不对准角Φ反对称矩阵。
Φ=（αβγ）T
Φ
0
0
0
（式4.34）
这里的Φ为计算地理坐标系与真实地理坐标系的误差角，α、β、γ分别为
地理坐标系与真实坐标系的北向、东向、方位的误差角。
捷联系统初始对准的基本思路是：通过处理加速度计和陀螺仪的量测值，产
生修正角速度ωc以供捷联矩阵的更新计算，并驱使不对准角尽可能减为零。与
此同时，以陀螺仪的量测输出估计计算出舰船摇摆角速度，对舰船摇摆的角度进
.
可编辑
行隔离。其原理示意图如图4.4所示。
图4.4对准原理示意图
4.4捷联式惯性导航系统的误差分析
4.4.1系统的主要误差源
1．惯性仪表的安装误差和表读因子误差；
2．陀螺的漂移和加速度计的零位误差；
3．初始条件误差，包括导航参数和姿态航向的初始误差；
4．计算误差，主要考虑姿态航向系统的计算误差，也即数学平台的计算误差；
5．载体角运动所引起的动态误差
4.4.2分类
对上述几种误差源进行分类，则捷联惯导系统的误差可分为四类。
1．数学模型的近似性所引起的误差
当捷联系统的数学模型建立得不够精确时会引起系统误差。数学模型的选取
应达到其近似性可以忽略的程度，否则就应该探讨更精确的数学模型。
2．惯性仪表的误差
惯性仪表（包括陀螺及加速度计）由于原理、加工与装配工艺的不完善等均
.
可编辑
可造成仪表输出的误差，从而导致系统的误差。在实际中，这部分误差在系统误
差中占很大一部分。
3．计算机的算法误差
对于捷联式惯导系统，当加速度计与陀螺的输出被采集到计算机中以后，剩
下的工作由计算机承担，而所有的捷联计算都存在着算法误差，从而导致系统的
误差。
4．初始对准误差
系统初始对准的误差是由惯性仪表的误差及初始对准过程中的算法误差等
所造成。
4.4.3系统的误差方程
1.静基座下的姿态误差方程
由于系统存在着各种误差源，导航计算机实时所计算出的姿态矩阵t
b
T并不
能理想地描述b系相对t系的关系，而只能描述b系相对t系附近的计算地理坐
标系tˆ的关系，这里的tˆ系和t系存在小的误差角α、β、γ，简记为
Φ=（αβγ）T
规定符号“”表示计算值，“∽”表示测量值
由于舰船系泊于码头的纬度可以足够精确的测得，则地球自转角速度在该处
地理坐标系中的投影分量也可以精确的表示出来。
2
1
0
sin
cos
0
ie
ie
t
ie
（式4.35）
由于tˆ系偏离t系一个小角度Φ=（αβγ）T，计算机实际算得地球自转角速
度在b系中的计算值为：
.
可编辑
t
ie
tb
t
t
ie
t
t
b
t
t
ie
b
t
t
ie
t
b
b
ie
ITTTTT)(
ˆˆ
1
ˆ
t
ie
bb
ie
t
ie
tb
t
t
ie
b
t
TT（式4.36）
此式表明，地球自转角速度ωie在b系中的理想值和计算值之间的计算误差，以
δb
ie
表示为：δb
ie
=t
ie
b（式4.37）
根据速度合成定理有：
b
ti
b
te
b
ie
ˆˆ
ˆˆˆ（式4.38）
将式3.36代入上式，则有：
b
te
b
ie
b
ie
b
ti
ˆˆ
ˆˆ（式4.39）
以上是数学解析平台t
b
Tˆ和加速度计的误差。另外在安装于载体上的陀螺仪
输出的测量信号中，除了地球自转角速率之外还包含陀螺漂移误差ε和干扰角速
度误差ωd，
bb
d
b
ie
b
ib
~（式4.40）
令δb
ib
=b
d
+ε
则有b
ie
b
ib
~+δb
ib
（式4.41）
称δb
ib
为测量误差。
然后经过一系列的推导可得静基座下捷联式惯性导航系统的姿态误差方程为：
tz
d
tz
ty
d
ty
tx
d
tx
tx
tx
ty
RtgV
RV
RV
/
ˆ
/
ˆ
/
ˆ
0
0
0
0
2
1
（式4.42）
其中
sin,cos
21ieie
。
2.速度误差方程
由加速度计测得的加速度信息经过数学解析平台变换成计算地理坐标系下
的计算值为b
ib
tb
ib
t
t
b
ib
t
b
t
t
b
ib
t
b
t
ib
aIaTaTTaTa
~
)(
~~~
ˆˆˆˆˆ（式4.43）
加速度计测信息中含有g、
、V
ie
ˆ
sin2、t
d
a等成分。误差项为：
.
可编辑
t
d
t
ie
b
ib
aVa
ˆ
sin2（式4.44）
其中
----加速度计的零位误差；
t
d
a----干扰加速度引起的检测误差；
V
ie
ˆ
sin2---哥式加速度带来的检测误差。
这样，在静基座下，则有：
Ttz
ib
ty
ib
tx
ib
t
ib
gaaaa)(
~
（式4.45）
将式4.45代入式4.43中，可得：
tz
d
tx
ty
d
tx
ie
ty
tx
d
ty
ie
tx
xt
ib
yt
ib
xt
ib
ag
aV
aV
a
a
a
ˆ
sin2
ˆ
sin2
1
1
1
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
（式4.46）
在初始对准中，作为控制信息，常用的是水平加速度计信息，而不是垂直信息，
展开上式，得水平通道速度误差方程为：
tx
d
txty
ie
xtagVa
ˆ
sin2
ˆˆ
ty
d
tytx
ie
ytagVa
ˆ
sin2
ˆˆ（式4.47）
式中
dta
t
t
Vxttx
ˆ
1
2ˆ
ˆ
dta
t
t
Vytty
ˆ
1
2ˆ
ˆ
4.4.4捷联惯导系统的基本误差特性分析
从系统的主要误差源可以看出，捷联惯导系统的基本误差可以概括为两大
类：确定性的和随机性的，下面分别进行讨论[9]。
1.确定性的误差源引起的误差特性
为了简单起见，考虑静基座的情况，由于惯性系统的垂直通道是不稳定的考
.
可编辑
虑。经度误差在系统的回路之外，不影响系统的动态特性，也不予以考虑。统的
误差方程就简化为:
z
y
x
z
y
x
z
y
x
ie
ie
ieie
ieie
ie
ie
z
y
x
y
x
y
x
L
L
g
LL
LL
R
R
g
L
L
R
L
L
0
0
0
sin
0
0
0
0
0
sin
0
0
coscos0
sinsin0
00
1
00
1
00
00sin2
tan
1
0
0
sin2
0
（式
4.48）
写成简化形式为：X(t)=FX(t)+W(t)
对上式取拉氏变换得：sX(s)-X(0)=FX(s)+W(s)
X(s)=(sI-F)-1[X(0)+W(s)]
系统的特征方程为:
0sin4)()()(22222222LsssFSIs
iesie
（式4.49）
式中2
s
=
R
g
,
s
为舒勒角频率。
由特征方程可得：
022
ie
s
（式4.50）
Lss
ies
222222sin4)(由式4.50可得：
ie
js
2,1
这是一个等幅振荡，振荡周期为：hT
ie
24
2
（即地球自转周期）.
对于式4.50，因为s
s
31024.1
,1410729.0s
ie
,即
ies
，故可近似分
解为：
0)sin()sin(2222LsLs
iesies
.
可编辑
所以得：)sin(
4,3
Ljs
ies
)sin(
6,5
Ljs
ies
这四个根说明系统中还包括有角频率为)sin(L
ies
和)sin(L
ies
的
两种振荡运动。由于)sin(L
ies
，说明系统中包括两种频率相近的正弦分量，
它们和在一起产生差频。
例如，
ttLLtL
sieiesies
sin)sincos()0(2)sinsin()0()sinsin()0(
即产生一个角频率为
s
的振荡，其幅值为)sincos()0(2L
ie
。因此合成的振荡
具有调幅性质。对应角频率
s
的振荡叫舒勒振荡，振荡周期为84.4min，而对
应的L
ie
sin
的振荡称为傅科振荡，其振荡周期为
L
T
ie
fsin
2
。
当L=45度时，Tf=34h。
综上所述，惯导系统确定性误差源引起的误差特性包括三种振荡，即舒勒
周期振荡，
地球周期振荡和傅科周期振荡。但需要注意的是有些误差虽然从性质上来说是振
荡的。
但因振荡周期很长，远远大于一次工作时间，此时，在系统工作时间误差是随时
间增长
的。比如t
i
ie
z
z
sin，振荡周期是24小时，显然在工作几小时的期间
z
是
随时间增长的。
2.随机误差源引起的系统误差
对于上面讨论的确定性误差源引起的误差特性可以设法通过补偿加以消除。
.
可编辑
在补偿了确定性误差之后，则随机误差源成为影响系统精度的主要误差源。系统
的随机误差很多，主要讨论陀螺漂移和加速度计的偏差。
a)陀螺漂移的数学模型
陀螺是运载体角运动的测量器件，对惯导系统的姿态误差产生直接的影响。
陀螺的
误差主要体现为随机漂移，随机漂移是十分复杂的随机过程，大致可概述为三种
分量。
(1)逐次启动漂移
它取决于启动时刻的环境条件和电气参数的随机性等因素，一旦启动完成，
这种漂移便保持在某一固定值上，但这一固定值是一个随机变量，所以这种分量
可用随机常数
来描述。
),,(0zyxi
bi
(式4.51)
(2)慢变漂移
陀螺在工作过程中，环境条件、电气参数都在作随机改变，所以陀螺是漂移
在随机常数分量的基础上以较慢的速率变化。由于变化比较缓慢，变化过程中前
后时刻上的漂移值有一定的关联性，即后一时刻的漂移值程度不等地取决于前一
时刻的漂移值，两者的时间点靠的越近，这种依赖关系就越明显。这种分量可用
一阶马尔科夫过程中来描述：
),,(
1
zyxi
riri
G
ri
(式4.52)
(3)快变漂移
表现在上述两种分量基础上的杂乱无章的高频跳变。不管两时间点靠的多
.
可编辑
近，该时
间点上的漂移值依赖关系十分微弱或几乎不存在。这种漂移分量可抽象为白噪声
过程，即
)()]()([tqtE
gigigi
zyxi,,(式4.53)
式4.53中
)(t
为狄拉克函数。
综上所述，陀螺漂移可模型化为：
)()()()(tttt
giribii
(式4.54)
b)加速度计数学模型
与陀螺漂移误差模型的分析类似，加速度计误差模型可分为三种分量。但在
组合导航设计中，一般只考虑随机常值漂移，即偏置误差
i
(zyxi,,)，而忽略
相关误差。这是由于这种分量相对较小，同时也为了使滤波器的维数尽量低些。
所以加速度计误差模一般考虑为:
aibii
zyxi,,(式4.55)
式中
bi
=0，zyxi,,，
ai
（zyxi,,）为噪声过程。
3.举例说明
为了简单起见，考虑一个单轴情况，其误差方程为：
xy
x
R
g
R
(式4.56)
y
x
yR
(式4.57)
把加速度计误差
x
和陀螺漂移都考虑作为一阶马尔科夫过程，分别表示为：
1
2
yyyy
A
(式4.58)
22
2
xxx
MAM
(式4.59)
式中
A是陀螺随机漂移方差，2
A；
A是加速度计随机误差方差，2
A
.
可编辑
1
，
2
是强度为1的白噪声，其协方差
1)](),(cov[t
，把
y
，
x
扩充为状态，
则状态方程为：
)()(tGWtFXX
(式4.60)
式中
x
M
y
R
g
R
F
0
0
1
0
1
0
0
0
0
0
0
1
0
x
y
MA
A
G
2
0
0
0
0
2
0
0
系统噪声协方差阵为：
1
0
0
1
Q
把式4.60离散化得：
)(),1()(),1()1(kWkkkXkkkX
(式4.61)
式中9922
!9
1
...
!2
1
),1(TFTFFTIkk
GTFTFFTITkk)
!10
1
...
!3
1
!2
1
(),1(9922
取下列参数值：
mR310370.62/81.9smg
s
i
/)'('01.012101.0sM
24/)'('101.0s
11101.0sM
在初始时刻，系统的状态是统计独立的，取初始协方差尸(0)为对角矩阵，其对
角元素为：
0)0()0(2
11
P
2422
22
)'('1044.1)''120()0()0(
P
2232
33
)'('101.0)0()0(sP
.
可编辑
4292
44
)'('101.0)0()0(sP
根据随机误差的协方差分析方法计算：
),1()(),1(),1()(),1()1(kkkQkkkkkPkkkPTT(式4.62)
计算结果说明，在随机误差源的作用下，系统误差是随时间振荡增长的。
4.5仿真算法流程图
开始
读取初始姿态，速度，位置参数及其误差
值；读取时间常数；读取陀螺及加速度计
误差参数，初始四元数及姿态矩阵计算
设置常值
角增量计算
四元数更新计算
四元数规范化
姿态矩阵更新计算，姿态角更新计算
速度更新计算
位置更新计算
输出
结束
.
可编辑
5.仿真结果
5.1仿真初始数据设定
陀螺漂移为0.1度/小时，加速度计的零偏为le-4*g，gn只与维度有关而与
高度无关。算法步长（即姿态更新周期）Tm=0.02ms，静态仿真了9000S，每
点间隔0.1S保存一个数据，姿态更新采用四元数算法。
模拟航迹仿真
选取地球参数:
长半轴Re=6378160;
椭圆度e=1/298.3;
自转角速率
ie
=7.292ll51467e-5;
重力g0=9.7803267714;
载体初始状态:
姿态角at=pi/180*[0;0;45];%俯仰角，横滚角，航向角
载体方向速度vb=[0;0;0];
地理位置pos=[pi/180*34.2;pi/180*108.9;0];
龙格库塔法步长th=0.002;%2ms
变量rk的说明:变量rk包含了仿真计算产生的载体和航迹信息，
rk(1:3)姿态航向角
rk(4:6)地速(东向速度、北向速度、天向速度)
rk(7:9)地理位置(纬度、经度、高度)
.
可编辑
rk(10:12)角增量(从仿真零时刻起的累计增量)
rk(13:15)比力增量(累计增量)
捷联算法仿真选取地球参数和生成航迹中的地球参数相同，载体初始状态从生成
航迹中的数据获取。这里采用四元数算法进行仿真。
5.2结果显示
输入指定的初始数据可以得出几个重要参数的结果图
图5.1航向角误差曲线
图5.2纵摇角误差曲线
.
可编辑
图5.3横摇角误差曲线
图5.4纬度误差曲线
.
可编辑
图5.5经度误差曲线
图5.6东向速度误差曲线
图5.7北向速度误差曲线
有了这些数据能使使用者更好的了解惯性导航系统的各种特性，还有及时
.
可编辑
的根据这些数据进行误差调整。
6.总结
通过这段时间不断的了解关于捷联式惯性导航系统的各种知识。更加了解
了捷联式惯性导航系统在自主导航中的重要。
现代社会随着科技日新月异的发展，各种高科技的产品都需要好的自导系
统使产品能更好的为人类服务。这时，捷联式惯性导航系统因为其结构简单和抗
干扰性强的优点，只要能减少系统运行过程中的误差并及时对误差进行修正就能
稳定的进行使用者想要达到的结果。随着各种误差减小和误差修正的算法的发现
与研究，捷联式惯性导航系统必将成为自主导航系统中最重要的部分。
.
可编辑
参考文献
[1]陈建国等，船用捷联惯性导航系统研究[A],海军驻上海地区水声导航系统
军事代表室，2009
[2]刘莉娜等,基于捷联惯性导航的组合导航系统研究[A]，湖南株洲职业技术学
院,2009
[3]张荣辉等，基于四元数法的捷联式惯性导航系统的姿态解算[A]，光学精密
工程，2008
[4]刘勇等，旋转光纤陀螺捷联式惯性导航系统[A]，压电与声光，2009
[5]于青，AUV捷联式惯性导航系统研究，中国海洋大学，2008
[6]吴非，车载激光捷联惯导系统动基座初始对准技术研究，国防科学技术大学，
2007
[7]韩璐等，弹用捷联式惯性导航系统数值仿真，西安工业大学学报2009
[8]张娟妮等，机抖激光陀螺捷联惯性测量组合的标定方法研究[A]宇航计测技
术2008
[9]刘危等，捷联惯性导航系统的姿态算法[A]，北京航空航天大学学报，2005
[10]刘勇等，旋转光纤陀螺捷联式惯性导航系统[A]，压电与声光，2008
[11]张树侠,孙静.捷联式惯性导航系统[M].国防工业出版社,1988.
.
可编辑
[12]李滋刚等.捷联式惯性技术及系统[M].东南大学先进技术与装备研究院,
2009
[13]张天光.捷联惯性导航技术[M].北京:国防工业出版社,2007.
[14]张宗麟.惯性导航与组合导航[M].北京:航空工业出版社,2000.
[15]杨艳娟.捷联惯性导航系统关键技术研究[D].哈尔滨:哈尔滨工程大学,2001.
[19]樊荣.捷联惯性导航系统初始对准方法研究及其仿真.南京理工大学硕士学位
论文,2006.6
[20]杨剑宏.舰载光纤陀螺捷联系统快速初始对准及GPS/sINS组合技术研究.上
海交通大学博士学位论文，2001
[21]Lee，J.G.;Park，C.G.;park，H一WMultipositionalignmentof
aceandElectronic
systems，IEEETransactionson，VOlume:29，Issue:4，Oct1993，
Pages:1323-1328
[22]严恭敏.捷联惯导算法及车载组合导航系统研究.西北工业大学硕士学位论
文，2004.1
[23]姚俊,马松辉.Simulink诫建模与仿真.西安:西安电子科技大学出版社，2002.8
[24]陈亚勇.MATLAB信号处理详解.北京:人民邮电出版社，2001.9
[25]清源计算机工作室.MATLAB6.0.北京：机械工业出版社，2001.5
[26]陈桂明,张明照,戚红雨,张宝俊.应用MATLAB建模与仿真.北京：科学出版社，
2001.9
[27]戴锋.Visualc++程序设计基础.北京：清华大学出版社，2001.4
[28]陈华生,张岳新.VisualC++程序设计基础.苏州：苏州大学出版社，2000
.
可编辑
程序代码
//:implementationfile
#include"stdafx.h"
#include"MyProject.h"
#include"MyDialog6.h"
#include"stdlib.h"
#include"math.h"
#include"stdio.h"
//#include
#include"conio.h"
.
可编辑
#defineTT0.01
#defineii1100
//#defineGrv9.8
//#definePI3.9798
#defineGYROx0.0//GYROx*deg/hconstantdrift
#defineGYROy0.0//constantdrift
#defineGYROz0.0//constantdrift
#defineACCx0.0//ACCx*1e-3g*mgbias
#defineACCy0.0//ACCx*1e-3gbias
#defineACCz0.0//ACCx*1e-3gbias
#defineGYRO_rx0.0//GYROx*deg/hconstantdrift
#defineGYRO_ry0.0//constantdrift
#defineGYRO_rz0.0//constantdrift
#defineACC_rx0.0//ACCx*1e-3g*mgbias
#defineACC_ry0.0//ACCx*1e-3gbias
#defineACC_rz0.0//ACCx*1e-3gbias
#ifdef_DEBUG
#definenewDEBUG_NEW
#undefTHIS_FILE
staticcharTHIS_FILE[]=__FILE__;
#endif
.
可编辑
doublegyro_data[3];
doubleCx,Cy,Cz,q1;
doubleq[4],Tbn[9],angle[3],STbn[9];
doubleRa,Rn,Re,Pai,Dpai,Rpai,Wie,aee;
doubleSH,SP,SR,SL,SE,SV,SVe,SVn,SVu,SdL,SdE;
doubleSAe,SAn,SAu,SFe,SFn,SFu,SFx,SFy,SFz;
doubleSWine,SWinn,SWinu,SWinx,SWiny,SWinz;
doubleSWnb[3];
doubleH,P,R,L,E,Ve,Vn,Vu,dL,dE;
doubleWine,Winn,Winu,Winx,Winy,Winz,Fe,Fn,Fu;
doubleEGx,EGy,EGz,EAx,EAy,EAz;
doubleEH,EP,ER,EL,EE,EVe,EVn,EVu;
doublehead,pitch,roll,t;
doublediff_ang[3];
doubletran[9];
structrocking
{
doublepriod[3];
doubleang_frey[3];
doublephase[3];
doubleam[3];
}rocking;
.
可编辑
intm0,m1,m2;
doubles0,w0,v0,r0,t0;
doubles1,w1,v1,r1,t1;
doubles2,w2,v2,r2,t2;
intma0,ma1,ma2;
doublesa0,wa0,va0,ra0,ta0;
doublesa1,wa1,va1,ra1,ta1;
doublesa2,wa2,va2,ra2,ta2;
doubledeghour_radsecond,gravity_const;
doubledx,dy,dz;
inti,bbb,j;
intMOVE;
longTime,aaa;
voidoptimize_quan();
voidquan_vbn_angle();
voidsample_quan();
/////////////////////////////////////////////////////////////////////////////
//CMyDialog6dialog
voidsample_quan(double*q,doubledx,doubledy,doubledz)
.
可编辑
{
doublecc0,cc1,cc2,qq[4];
inti,j,u,l;
doubleT[16];
//doubleq1[4];
cc0=dx*dx+dy*dy+dz*dz;
cc0=sqrt(cc0);//wqpfinding
if(cc0==0.0)cc1=0.5;
elsecc1=sin(cc0/2.0)/cc0;
cc2=cos(cc0/2.0);
for(i=0;i<=3;i++)
for(j=0;j<=3;j++)
{
u=i*4+j;
if(i==j)
T[u]=cc2;
}
T[1]=T[14]=-cc1*dx;
T[4]=T[11]=-T[1];
T[2]=T[7]=-cc1*dy;
T[8]=T[13]=-T[2];
T[3]=T[9]=-cc1*dz;
.
可编辑
T[6]=T[12]=-T[3];
for(i=0;i<=3;i++)
{
qq[i]=0.0;
for(l=0;l<=3;l++)
qq[i]=qq[i]+T[i*4+l]*q[l];
}
for(i=0;i<=3;i++)
q[i]=qq[i];
return;
}
voidquan_cbn_angle(double*q,double*angle,double*Tbn)
{
//t[0]t[1]t[2]
//t[3]t[4]t[5]=cbn
//t[6]t[7]t[8]
doubleq00,q11,q22,q33,q12,q13,q23,q01,q02,q03;
q00=q[0]*q[0];
q11=q[1]*q[1];
q22=q[2]*q[2];
q33=q[3]*q[3];
.
可编辑
q12=q[1]*q[2];
q13=q[1]*q[3];
q23=q[2]*q[3];
q01=q[0]*q[1];
q02=q[0]*q[2];
q03=q[0]*q[3];
Tbn[0]=q00+q11-q22-q33;
Tbn[1]=2.0*(q12+q03);
Tbn[2]=2.0*(q13-q02);
Tbn[3]=2.0*(q12-q03);
Tbn[4]=q00-q11+q22-q33;
Tbn[5]=2.0*(q23+q01);
Tbn[6]=2.0*(q13+q02);
Tbn[7]=2.0*(q23-q01);
Tbn[8]=q00-q11-q22+q33;
angle[0]=atan2(Tbn[3],Tbn[4]);
//angle[1]=asin(Tbn[5]);//angle[1]=pitch
angle[1]=atan2(Tbn[5],sqrt(Tbn[3]*Tbn[3]+Tbn[4]*Tbn[4]));
angle[2]=-atan2(Tbn[2],Tbn[8]);
return;
}
.
可编辑
voidoptimize_quan(double*q)
{
inti;
doubleqq=0.0;
for(i=0;i<=3;i++)
{
qq=qq+q[i]*q[i];
}
qq=sqrt(qq);
for(i=0;i<=3;i++)
q[i]=q[i]/qq;
return;
}
CMyDialog6::CMyDialog6(CWnd*pParent/*=NULL*/)
:CDialog(CMyDialog6::IDD,pParent)
{
//{{AFX_DATA_INIT(CMyDialog6)
m_ZhouQi=0.01f;
m_Time=0.0;
m_SV=0.0;
m_Frequ=100;
.
可编辑
m_aee=1;
m_SE=0.0;
m_SL=0.0;
m_ExC=0.0;
m_EAx=0.0;
m_EyC=0.0;
m_EAy=0.0;
m_EzC=0.0;
m_xx=0.0;
m_yy=0.0;
m_zz=0.0;
m_Ah=0.0;
m_Ap=0.0;
m_Ar=0.0;
m_Tr=0.0;
m_Tp=0.0;
m_Th=0.0;
m_Pp=0.0;
m_Pr=0.0;
m_Ph=0.0;
m_xxxx=0.0;
m_yyyy=0.0;
.
可编辑
m_EAz=0.0;
m_zzzz=0.0;
//}}AFX_DATA_INIT
}
voidCMyDialog6::DoDataExchange(CDataExchange*pDX)
{
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CMyDialog6)
DDX_Text(pDX,IDC_EDIT3,m_ZhouQi);
DDX_Text(pDX,IDC_EDIT1,m_Time);
DDX_Text(pDX,IDC_EDIT2,m_SV);
DDX_Text(pDX,IDC_EDIT4,m_Frequ);
DDX_Text(pDX,IDC_EDIT5,m_aee);
DDX_Text(pDX,IDC_EDIT6,m_SE);
DDX_Text(pDX,IDC_EDIT7,m_SL);
DDX_Text(pDX,IDC_EDIT8,m_ExC);
DDX_Text(pDX,IDC_EDIT13,m_EAx);
DDX_Text(pDX,IDC_EDIT15,m_EyC);
DDX_Text(pDX,IDC_EDIT20,m_EAy);
DDX_Text(pDX,IDC_EDIT21,m_EzC);
DDX_Text(pDX,IDC_EDIT9,m_xx);
.
可编辑
DDX_Text(pDX,IDC_EDIT16,m_yy);
DDX_Text(pDX,IDC_EDIT22,m_zz);
DDX_Text(pDX,IDC_EDIT12,m_Ah);
DDX_Text(pDX,IDC_EDIT10,m_Ap);
DDX_Text(pDX,IDC_EDIT11,m_Ar);
DDX_Text(pDX,IDC_EDIT18,m_Tr);
DDX_Text(pDX,IDC_EDIT17,m_Tp);
DDX_Text(pDX,IDC_EDIT19,m_Th);
DDX_Text(pDX,IDC_EDIT23,m_Pp);
DDX_Text(pDX,IDC_EDIT24,m_Pr);
DDX_Text(pDX,IDC_EDIT25,m_Ph);
DDX_Text(pDX,IDC_EDIT14,m_xxxx);
DDX_Text(pDX,IDC_EDIT27,m_yyyy);
DDX_Text(pDX,IDC_EDIT26,m_EAz);
DDX_Text(pDX,IDC_EDIT28,m_zzzz);
//}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CMyDialog6,CDialog)
//{{AFX_MSG_MAP(CMyDialog6)
//}}AFX_MSG_MAP
ON_BN_CLICKED(IDOK,&CMyDialog6::OnBnClickedOk)
.
可编辑
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
//CMyDialog6messagehandlers
voidCMyDialog6::OnOK()
{
UpdateData(true);
SV=m_SV;
//SE=m_SE;
//SL=m_SL;
Time=(long)m_Time;
//intce;
//ce=m_aee;
FILE*fp0,*fp1,*fp2,*fp3,*fp4,*fp5,*fp6,*fp7;
//**********constantparameters***********
gravity_const=9.8e-3;
Ra=6.378165e+6;
aee=1.0/298.3;//3.352329e-3;
Pai=3.9798;
Rpai=Pai/180.0;
Dpai=180.0/Pai;
.
可编辑
Wie=15.041088*Rpai/3600.0;
deghour_radsecond=Rpai/3600.0;
for(MOVE=6;MOVE<=6;MOVE++)
{
if(MOVE==4)
{
if((fp0=fopen("Err5.2r","wb"))==NULL)
{
printf("cannotopenErr.2rfilen");
exit(0);
}
if((fp3=fopen("Q.2r","wb"))==NULL)
{
printf("cannotopenQ.2rfilen");
exit(0);
}
if((fp1=fopen("Cnb1.2r","wb"))==NULL)
{
printf("cannotopencnb1.2rfilen");
exit(0);
}
.
可编辑
if((fp2=fopen("Cnb2.2r","wb"))==NULL)
{
printf("cannotopencnb2.2rfilen");
exit(0);
}
if((fp4=fopen("obs2rvs","wb"))==NULL)//Ve,Vn
{
printf("cannotopenobs2rvsfilen");
exit(0);
}
if((fp5=fopen("","wb"))==NULL)
//Fe,Fn,Fu,Ve,Vn
{
printf("en");
exit(0);
}
if((fp6=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
.
可编辑
if((fp7=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
}
if(MOVE==5)
{
if((fp0=fopen("Err5.3r","wb"))==NULL)
{
printf("cannotopenErr.3rfilen");
exit(0);
}
if((fp3=fopen("Q.3r","wb"))==NULL)
{
printf("cannotopenQ.3rfilen");
exit(0);
}
if((fp1=fopen("Cnb1.3r","wb"))==NULL)
{
.
可编辑
printf("cannotopencnb1.3rfilen");
exit(0);
}
if((fp2=fopen("Cnb2.3r","wb"))==NULL)
{
printf("cannotopencnb2.3rfilen");
exit(0);
}
if((fp4=fopen("obs3rvs","wb"))==NULL)
{
printf("cannotopenobs3rvsfilen");
exit(0);
}
if((fp5=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
if((fp6=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
.
可编辑
}
if((fp7=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
}
if(MOVE==6)
{
if((fp0=fopen("","wb"))==NULL)
{
printf("n");
exit(0);
}
if((fp3=fopen("","wb"))==NULL)
{
printf("n");
exit(0);
}
if((fp1=fopen("","wb"))==NULL)
{
.
可编辑
printf("en");
exit(0);
}
if((fp2=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
if((fp4=fopen("obsrmvs","wb"))==NULL)
{printf("cannotopenobsrmvsfilen");
exit(0);
}
if((fp5=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
if((fp6=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
.
可编辑
if((fp7=fopen("","wb"))==NULL)
{
printf("en");
exit(0);
}
}
/********************initialvalue
t=0***********************************/
//SL=32.0*Rpai;
//SE=118.0*Rpai;
SL=m_SL*Rpai;
SE=m_SE*Rpai;
L=SL;
E=SE;
//Rn=Ra*(1.0-2.0*aee+3.0*aee*sin(SL)*sin(SL));
//Re=Ra*(1.0+aee*sin(SL)*sin(SL));
Rn=Ra;
Re=Ra;
SH=0.0;
SP=0.0;
.
可编辑
SR=0.0;
H=0.0;
P=0.0;
R=0.0;
if(MOVE==6)
{
//SV=0.0;}
}
if((MOVE==4)||(MOVE==5))
{
//SV=0.0;}
}
SVe=SV*sin(SH);
SVn=SV*cos(SH);
SVu=0.0;
Ve=SV*sin(H);
Vn=SV*cos(H);
dL=Vn/Rn;
dE=Ve/(Re*cos(L));
SdL=SVn/Rn;
SdE=SVe/(Re*cos(SL));
/********************largerocking************************/
.
可编辑
/*[0]=20.0;
[1]=6.0;
[2]=9.0;
[0]=45.0*Rpai;//45.0
[1]=7.0*Rpai;
[2]=15.0*Rpai;*/
/********************smallrocking************************/
/*[0]=12.0;
[1]=7.0;
[2]=9.0;
[0]=10.0*Rpai;//10.0deg/heading
[1]=10.0*Rpai;//pitch
[2]=40.0*Rpai;//roll
_frey[0]=2.0*Pai/[0];
_frey[1]=2.0*Pai/[1];
_frey[2]=2.0*Pai/[2];*/
[0]=m_Th;//6.0;
[1]=m_Tp;//6.0;
[2]=m_Tr;//7.0;
//?[0]=7.5*Rpai;//10.0deg/heading
[0]=m_Ah*Rpai;
.
可编辑
//?[1]=10*Rpai;//pitch
[1]=m_Ap*Rpai;
//?[2]=15.0*Rpai;//roll*/
[2]=m_Ar*Rpai;
/*[0]=6.0;
[1]=4.83;
[2]=7.66;
[0]=3.0*Rpai;//10.0deg/heading
[1]=3.0*Rpai;//pitch
[2]=15.0*Rpai;//roll*/
for(i=0;i<=2;i++)
{
if(fabs([i])<0.1)
{
_frey[0]=0;
_frey[1]=0;
_frey[2]=0;
}
else
{
_frey[0]=2.0*Pai/[0];
.
可编辑
_frey[1]=2.0*Pai/[1];
_frey[2]=2.0*Pai/[2];
}
}
//_frey[0]=m_Th;
//_frey[1]=m_Tp;
//_frey[2]=m_Tr;
[0]=m_Ph*Rpai;//0.0;
[1]=m_Pp*Rpai;//0.0;
[2]=m_Pr*Rpai;//0.0;
if(MOVE==4)
{
_frey[0]=0.0;
}
//*************InitialattitudematrixandQ(4)*************
q[0]=cos(H/2.0)*cos(P/2.0)*cos(R/2.0)+sin(H/2.0)*sin(P/2.0)*sin(R/2.0
);
q[1]=cos(H/2.0)*sin(P/2.0)*cos(R/2.0)+sin(H/2.0)*cos(P/2.0)*sin(R/2.0
.
可编辑
);
q[2]=cos(H/2.0)*cos(P/2.0)*sin(R/2.0)-sin(H/2.0)*sin(P/2.0)*cos(R/2.0)
;
q[3]=cos(H/2.0)*sin(P/2.0)*sin(R/2.0)-sin(H/2.0)*cos(P/2.0)*cos(R/2.0)
;
Tbn[0]=cos(H)*cos(R)+sin(H)*sin(P)*sin(R);
Tbn[1]=cos(H)*sin(P)*sin(R)-sin(H)*cos(R);
Tbn[2]=-cos(P)*sin(R);
Tbn[3]=sin(H)*cos(P);
Tbn[4]=cos(H)*cos(P);
Tbn[5]=sin(P);
Tbn[6]=cos(H)*sin(R)-sin(H)*sin(P)*cos(R);
Tbn[7]=-sin(H)*sin(R)-cos(H)*sin(P)*cos(R);
Tbn[8]=cos(P)*cos(R);
//********Attitudeandnavigationcalculation*********
//***Varyingwithinitialcondition
s0=65536.0;w0=2053.0;v0=13849.0;r0=0.0;
s1=65536.0;w1=2053.0;v1=13849.0;r1=0.0;
.
可编辑
s2=65536.0;w2=2053.0;v2=13849.0;r2=0.0;
sa0=65536.0;wa0=2053.0;va0=13849.0;ra0=0.0;
sa1=65536.0;wa1=2053.0;va1=13849.0;ra1=0.0;
sa2=65536.0;wa2=2053.0;va2=13849.0;ra2=0.0;
t=0.0;
for(aaa=0;aaa<=Time-1;aaa++)
{
for(bbb=0;bbb<=ii1-1;bbb++)
//********inertialsensorparmeters********
{
{
t0=0.0;
for(j=0;j<=11;j++)
{
r0=w0*r0+v0;m0=r0/s0;r0=r0-m0*s0;t0=t0+r0/s0;
}
//EGx=(5.0*(t0-6.0)+2.0)*deghour_radsecond;
//EGx=(2.0*(t0-6.0)+2.0)*deghour_radsecond;
//EGx=(GYRO_rx*(t0-6.0)+GYROx)*deghour_radsecond;
EGx=(m_xx*(t0-6.0)+m_ExC)*deghour_radsecond;
.
可编辑
//EGx=0.0;
}
{
t1=0.0;
for(j=0;j<=11;j++)
{
r1=w1*r1+v1;m1=r1/s1;r1=r1-m1*s1;t1=t1+r1/s1;
}
//EGy=(5.0*(t1-6.0)+2.0)*deghour_radsecond;
EGy=(m_yy*(t1-6.0)+m_EyC)*deghour_radsecond;
//EGy=(GYRO_ry*(t1-6.0)+GYROy)*deghour_radsecond;
//EGy=0.0;
}
{
t2=0.0;
for(j=0;j<=11;j++)
{
r2=w2*r2+v2;m2=r2/s2;r2=r2-m2*s2;t2=t2+r2/s2;
}
//EGz=(5.0*(t2-6.0)+2.0)*deghour_radsecond;
.
可编辑
//EGz=(2.0*(t2-6.0)+2.0)*deghour_radsecond;
EGz=(m_zz*(t2-6.0)+m_EzC)*deghour_radsecond;
//EGz=0.0;
}
{
ta0=0.0;
for(j=0;j<=11;j++)
{
ra0=wa0*ra0+va0;ma0=ra0/sa0;ra0=ra0-ma0*sa0;ta0=ta0+ra0/sa0;
}
//EAx=(0.04*(t0-6.0)+0.3)*gravity_const;
EAx=(m_xxxx*(t0-6.0)+m_EAx)*gravity_const;
//EAx=0.0;
}
{
ta1=0.0;
for(j=0;j<=11;j++)
{
ra1=wa1*ra1+va1;ma1=ra1/sa1;ra1=ra1-ma1*sa1;ta1=ta1+ra1/sa1;
.
可编辑
}
//EAy=(0.04*(ta1-6.0)+0.3)*gravity_const;
//EAy=0.0;
EAy=(m_yyyy*(ta1-6.0)+m_EAy)*gravity_const;
}
{
ta2=0.0;
for(j=0;j<=11;j++)
{
ra2=wa2*ra2+va2;ma2=ra2/sa2;ra2=ra2-ma2*sa2;ta2=ta2+ra2/sa2;
}
//EAz=(0.04*(ta2-6.0)+0.3)*gravity_const;
EAz=(m_zzzz*(ta2-6.0)+m_EAz)*gravity_const;
//EAz=0.0;
}
if((aaa==0)&&(bbb==0))
t=TT;
else
t=t+TT;
.
可编辑
SH=[0]*sin(_frey[0]*t+[0]);
SP=[1]*sin(_frey[1]*t+[1]);
SR=[2]*sin(_frey[2]*t+[2]);
diff_ang[0]=[0]*_frey[0]*cos(_frey[
0]*t+[0]);
diff_ang[1]=[1]*_frey[1]*cos(_frey[
1]*t+[1]);
diff_ang[2]=[2]*_frey[2]*cos(_frey[
2]*t+[2]);
if(MOVE==4)
{
SH=0.0;
diff_ang[0]=0.0;
}
tran[0]=0.0;
tran[1]=cos(SR);
.
可编辑
tran[2]=sin(SR)*cos(SP);
tran[3]=1.0;
tran[4]=0.0;
tran[5]=-sin(SP);
tran[6]=0.0;
tran[7]=sin(SR);
tran[8]=-cos(SR)*cos(SP);
//***********Varyingwithinitialcondition*****************
for(i=0;i<=2;i++)
{
SWnb[i]=0.0;
for(j=0;j<=2;j++)
SWnb[i]=SWnb[i]+tran[i*3+j]*diff_ang[2-j];
}
SWine=-SVn/Rn;
SWinn=Wie*cos(SL)+SVe/Re;
SWinu=Wie*sin(SL)+SVe*tan(SL)/Re;
STbn[0]=cos(SH)*cos(SR)+sin(SH)*sin(SP)*sin(SR);
STbn[1]=cos(SH)*sin(SP)*sin(SR)-sin(SH)*cos(SR);
STbn[2]=-cos(SP)*sin(SR);
STbn[3]=sin(SH)*cos(SP);
.
可编辑
STbn[4]=cos(SH)*cos(SP);
STbn[5]=sin(SP);
STbn[6]=cos(SH)*sin(SR)-sin(SH)*sin(SP)*cos(SR);
STbn[7]=-sin(SH)*sin(SR)-cos(SH)*sin(SP)*cos(SR);
STbn[8]=cos(SP)*cos(SR);
SWinx=SWine*STbn[0]+SWinn*STbn[1]+SWinu*STbn[2];
SWiny=SWine*STbn[3]+SWinn*STbn[4]+SWinu*STbn[5];
SWinz=SWine*STbn[6]+SWinn*STbn[7]+SWinu*STbn[8];
//*******Calculatingparamertersofangle
increment*******
Wine=-Vn/Rn;
Winn=Wie*cos(L)+Ve/Re;
Winu=Wie*sin(L)+Ve*tan(L)/Re;
Winx=Wine*Tbn[0]+Winn*Tbn[1]+Winu*Tbn[2];
Winy=Wine*Tbn[3]+Winn*Tbn[4]+Winu*Tbn[5];
Winz=Wine*Tbn[6]+Winn*Tbn[7]+Winu*Tbn[8];
Cx=(SWinx+SWnb[0]+EGx-Winx)*TT;
Cy=(SWiny+SWnb[1]+EGy-Winy)*TT;
Cz=(SWinz+SWnb[2]+EGz-Winz)*TT;
.
可编辑
gyro_data[0]=SWinx+SWnb[0]+EGx;
gyro_data[1]=SWiny+SWnb[1]+EGy;
gyro_data[2]=SWinz+SWnb[2]+EGz;
sample_quan(q,Cx,Cy,Cz);
if(bbb%10==0)
{
optimize_quan(q);
}
quan_cbn_angle(q,angle,Tbn);
H=angle[0];
P=angle[1];
R=angle[2];
//********Varyingwithinitialcondition********
//------turecalculatingaccelarate------
SAe=SV*cos(SH)*diff_ang[0];
SAn=-SV*sin(SH)*diff_ang[0];
SAu=0.0;
SFe=SAe-(2.0*Wie+SdE)*sin(SL)*SVn+(2.0*Wie+SdE)*cos(SL)*SVu;
.
可编辑
SFn=SAn+(2.0*Wie+SdE)*sin(SL)*SVe+SdL*SVu;
SFu=SAu+9.8-(2.0*Wie+SdE)*cos(SL)*SVe-SdL*SVn;
/**0731***/
SFx=SFe*STbn[0]+SFn*STbn[1]+SFu*STbn[2];
SFy=SFe*STbn[3]+SFn*STbn[4]+SFu*STbn[5];
SFz=SFe*STbn[6]+SFn*STbn[7]+SFu*STbn[8];
SVe=SV*sin(SH);
SVn=SV*cos(SH);
SVu=0.0;
SdL=SVn/Rn;
SdE=SVe/(Re*cos(SL));
SL=SL+(SVn/Rn)*TT;
SE=SE+(SVe/(Re*cos(SL)))*TT;
/**********--calculatingvocility--***********************/
Fe=(SFx+EAx)*Tbn[0]+(SFy+EAy)*Tbn[3]+(SFz+EAz)*Tbn[6];
Fn=(SFx+EAx)*Tbn[1]+(SFy+EAy)*Tbn[4]+(SFz+EAz)*Tbn[7];
Fu=(SFx+EAx)*Tbn[2]+(SFy+EAy)*Tbn[5]+(SFz+EAz)*Tbn[8];
.
可编辑
Ve=Ve+(Fe+(2.0*Wie+dE)*sin(L)*Vn-(2.0*Wie+dE)*cos(L)*Vu)*TT;
Vn=Vn+(Fn-(2.0*Wie+dE)*sin(L)*Ve-dL*Vu)*TT;
Vu=Vu+(Fu+(2.0*Wie+dE)*cos(L)*Ve+dL*Vn-9.8)*TT;
dL=Vn/Rn;
dE=Ve/(Re*cos(L));
L=L+dL*TT;
E=E+dE*TT;
EH=(SH-H)*Dpai*60.0;
EP=(SP-P)*Dpai*60.0;//angledegreethatis'
ER=(SR-R)*Dpai*60.0;
EL=(SL-L)*Dpai*60.0;
EE=(SE-E)*Dpai*60.0;
EVe=SVe-Ve;
EVn=SVn-Vn;
//fprintf(fp6,"%e%e%en",gyro_data[0],gyro_data[1],gyro_data[2]);
if(bbb%10==0)
{
.
可编辑
//fprintf(fp0,"%e%e%e%e%e%e%en",EH,EP,ER,EL,EE,EVe,EVn);
//fprintf(fp1,"%e%e%e%e%en",Tbn[0],Tbn[3],Tbn[6],Tbn[1],Tbn[4
]);
//fprintf(fp2,"%e%e%e%en",Tbn[7],Tbn[2],Tbn[5],Tbn[8]);
//fprintf(fp5,"%e%e%e%e%en",Fe,Fn,Fu,Ve,Vn);
//fprintf(fp7,"%e%e%en",P*Dpai,R*Dpai,H*Dpai);
}
}
if(aaa%m_aee==0)
{
//fprintf(fp6,"%e%e%en",gyro_data[0],gyro_data[1],gyro_data[2]);
fprintf(fp0,"%e%e%e%e%e%e%en",EH,EP,ER,EL,EE,EVe,EVn);
//fprintf(fp3,"%e%e%e%en",q[0],q[1],q[2],q[3]);
//if(aaa!=0){fprintf(fp4,"%e%en",Ve,Vn);}
printf("L=10*%ldn",aaa/10);
}
}
.
可编辑
fclose(fp0);
fclose(fp1);
fclose(fp2);
fclose(fp3);
fclose(fp4);
fclose(fp5);
fclose(fp6);
fclose(fp7);
}
CDialog::OnOK();
}