《捷联惯导算法与组合导航基本原理讲义(20170220)》由会员分享，可在线阅读，更多相关《捷联惯导算法与组合导航基本原理讲义(20170220)（240页珍藏版）》请在金锄头文库上搜索。
1、捷联惯导算法与组合导航原理讲义严恭敏，翁浚 编著西北工业大学2016-9前 言近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括
2、姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大
3、学：董翠军；网友：Zoro；山东科技大学：王云鹏。书中缺点和错误在所难免，望读者不吝批评指正。作 者 2016年9月目 录第1章 概 述61.1捷联惯导算法简介61.2 Kalman滤波与组合导航原理简介7第2章 捷联惯导姿态解算基础102.1反对称阵及其矩阵指数函数102.1.1 反对称阵102.1.2 反对称阵的矩阵指数函数122.2方向余弦阵与等效旋转矢量132.2.1 方向余弦阵132.2.2 等效旋转矢量142.3方向余弦阵微分方程及其求解172.3.1 方向余弦阵微分方程172.3.2 方向余弦阵微分方程的求解172.4姿态更新的四元数表示202.4.1 四元数的基本概念202.4
4、.2 四元数微分方程232.4.3 四元数微分方程的求解252.5等效旋转矢量微分方程及其泰勒级数解262.5.1 等效旋转矢量微分方程262.5.2 等效旋转矢量微分方程的泰勒级数解292.6圆锥运动条件下的等效旋转矢量算法312.6.1 圆锥运动的描述312.6.2 圆锥误差补偿算法33第3章 地球形状与重力场基础403.1地球的形状描述403.2地球的正常重力场463.3地球重力场的球谐函数模型503.3.1 球谐函数的基本概念503.3.2 地球引力位函数583.3.3 重力位及重力计算63第4章 捷联惯导更新算法及误差分析694.1捷联惯导数值更新算法694.1.1 姿态更新算法69
5、4.1.2 速度更新算法704.1.3 位置更新算法764.2捷联惯导误差方程764.2.1惯性传感器测量误差764.2.2姿态误差方程784.2.3速度误差方程794.2.4位置误差方程794.2.5误差方程的整理804.3静基座误差特性分析824.3.1 静基座误差方程824.3.2 高度通道834.3.3 水平通道834.3.4 水平通道的简化884.3.5 水平通道误差方程的仿真90第5章 卡尔曼滤波基本理论925.1递推最小二乘法925.2 Kalman滤波方程的推导945.3连续时间随机系统的离散化与连续时间Kalman滤波1015.4噪声相关条件下的Kalman滤波1075.5序
6、贯滤波1115.6信息滤波与信息融合1145.7平方根滤波1165.8遗忘滤波1245.9 Sage-Husa自适应滤波1255.10最优平滑算法1275.11非线性系统的EKF滤波、二阶滤波与迭代滤波1305.12间接滤波与滤波校正1365.13联邦滤波（待完善）1365.14滤波的稳定性与可观测度分析141第6章 初始对准及组合导航技术1476.1捷联惯导粗对准1476.1.1矢量定姿原理1476.1.2解析粗对准方法1496.1.3间接粗对准方法1526.2捷联惯导精对准1536.3惯性/卫星组合导航1576.3.1空间杆臂误差1576.3.2时间不同步误差1586.3.3状态空间模型1
7、596.4车载惯性/里程仪组合导航1596.4.1航位推算算法1596.4.2航位推算误差分析1616.4.3惯性/里程仪组合1646.5低成本姿态航向参考系统（AHRS）1676.5.1简化的惯导算法及误差方程1686.5.2地磁场测量及误差方程1696.5.3低成本组合导航系统模型1706.5.4低成本惯导的姿态初始化1716.5.5捷联式地平仪的工作原理173第7章 捷联惯导与组合导航仿真1767.1飞行轨迹和惯性器件信息仿真1767.1.1飞行轨迹设计1767.1.2 捷联惯导反演算法1777.1.3 仿真1787.2捷联惯导仿真1807.2.1 Matlab子函数1807.2.2捷联
8、惯导仿真主程序1857.3惯导/卫星组合导航仿真1867.3.1Matlab子函数1867.3.2组合导航仿真主程序187附 录190A一些重要的三维矢量运算关系190B 运载体姿态的欧拉角描述192C 姿态更新的毕卡算法、龙格库塔算法及精确数值解法199D 从非直角坐标系到直角坐标系的矩阵变换207E 线性系统基本理论211F 加权最小二乘估计216G 矩阵求逆引理217H 几种矩阵分解方法（QR、Cholesky与UD）219I 二阶滤波中的引理证明223J 方差阵上界的证明225K 三阶非奇异方阵的奇异值分解226L Matlab仿真程序231M 练习题237参考文献241第1章 概 述
9、第1章 概 述11.1捷联惯导算法简介11.2 Kalman滤波与组合导航原理简介31.1捷联惯导算法简介在捷联惯导系统（SINS）中惯性测量器件（陀螺和加速度计）直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进行数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。特别在恶劣的高动态环境下，高精度的SINS对惯性器件性能和导航算法精度的要求都非常苛刻，由于高精度惯性器件往往价格昂贵并且进一步提升精度异常困难，所以在影响SINS精度的所有误差源中要求因导航算法引起的误差比重必须很小，一般认为应小于5%。姿态更新算法是SI
10、NS算法的核心，对整个系统的解算精度影响最为突出，具有重要的研究和应用价值。传统的姿态更新算法有欧拉角法、方向余弦阵法和四元数法等方法，这些方法直接以陀螺采样输出作为输入，使用泰勒级数展开或龙格库塔等方法求解姿态微分方程，未充分考虑转动的不可交换性误差问题。传统姿态更新算法在理论上可以通过提高采样和更新频率来提高解算精度，但实际陀螺采样频率又受限于传感器的带宽和噪声水平，因此传统算法的精度提升空间相对有限，仅适用于对解算精度要求不太高的场合。早在1775年，欧拉就提出了等效旋转矢量的概念，指出刚体的定点转动（即绕固定点的任何有限角位移）均可用绕经过该固定点的某轴的一次转动来实现，建立了刚体上单
11、位矢量在转动前后的变换公式。1840年，罗德里格使用后人称之为罗德里格参数的表示方法，推导了相继两次转动的合成公式，它与W. R. Hamilton在1843年发明的四元数乘法表示是一致的。研究表明，相继多次的定点转动问题可用一系列的姿态变化量（变化四元数或变化矩阵）相乘来描述，每个姿态变化量与对应转动的等效旋转矢量之间存在转换公式，使用等效旋转矢量计算姿态变化量不存在任何原理上的误差。因此，现代的SINS姿态更新算法研究的关键就在于如何使用陀螺输出构造等效旋转矢量，以尽量减小和避免不可交换性误差，后续再使用等效旋转矢量计算姿态变化量和进行姿态更新将变得非常简单，而不像传统方法那样，直接使用陀
12、螺输出进行姿态更新容易引起不可交换性误差。1949年，J. H. Laning在研究火控系统的过程中详细地分析了空间转动合成的性质，推导了由等效旋转矢量确定转动角速度的公式，但是由于缺少更好的应用背景驱动（比如后来SINS发展的迫切需求），未能获得广泛的研究重视。20世纪50年代是机械陀螺仪飞速发展的一个重要时期，也正是在那时发现了著名的圆锥运动现象，即当陀螺仪在其旋转轴和输出轴出现同频不同相的角振动时，尽管其输入轴净指向不变（在整体上没有随时间改变的趋势项），但陀螺仪还是会敏感到并输出常值角速率。1958年，为揭示圆锥运动现象产生的根源，L. E. Goodman建立了刚体转动的等效旋转矢量
13、与角速度之间的关系式，后人称之为Goodman-Robinson定理，该定理从几何上将转动不可交换性误差的坐标分量描述为单位球面上的一块有向面积，其面积由对应动坐标轴在单位球面上扫过的曲线与连接该曲线端点的大圆围成，Goodman借助二维Green积分理论获得了不可交换性误差的近似公式。1969年，基于Goodman近似公式，J. W. Jordan在假设陀螺角增量输出为二次多项式条件下提出了等效旋转矢量的“pre-processor”算法，它与后来发展的等效旋转矢量二子样算法完全一致。1969年，J. E. Bortz在其博士论文中详细推导了等效旋转矢量微分方程（1971年正式发表，后人称之
14、为Bortz方程），它是利用陀螺输出求解等效旋转矢量的基本公式，奠定了等效旋转矢量多子样算法的理论基础。在实际应用时一般需对较复杂的Bortz方程做近似处理，事实上，其简化结果与Goodman公式完全一致，它也可以根据Laning公式简化获得。1983年，R. B. Miller采用在圆锥运动条件下使算法漂移误差最小作为评价标准，推导了等效旋转矢量三子样优化算法。1990年，J. E. Lee研究了四子样优化算法。1992年，Y. F. Jiang研究了利用本更新周期内的三子样及前更新周期内的角增量计算旋转矢量的优化算法。1996年，M. B. Ignagni提出了由陀螺角增量构造等效旋转矢量
15、的通式，并给出了多达10种类型的等效旋转矢量算法。1999年，C. G. Park总结提出了各子样下求解圆锥误差补偿系数和算法漂移误差估计的通用公式。至此，从理论上看，在理想的圆锥运动条件下的不可交换性误差补偿问题得到了比较完美的解决。捷联惯导的基本概念在20世纪50年代就已经提出了，但是由于当时计算机的运算能力极其有限，在算法发展的早期阶段姿态更新通常采用双速回路算法方案：高速回路（e.g.,400Hz-10kHz）使用简单的一阶算法补偿由角振动引起的姿态不可交换性误差；中速回路（e.g.,50Hz-200Hz）以高速回路的处理结果作为输入再使用相对复杂的高阶算法进行姿态矩阵或四元数更新。双速回路算法的结构设计和实现过程都稍显繁琐，它只是在计算机运算能力低下时期所采取
