捷联惯导算法与组合导航原理讲义
严恭敏，翁浚 编著
西北工业大学 2016-9
1
前 言
近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。
惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。
本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：
西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军；网友：Zoro；山东科技大学：王云鹏。
书中缺点和错误在所难免，望读者不吝批评指正。
作 者 2016年9月
2
目 录
第1章 概 述 .................................................................................................................. 6
1.1捷联惯导算法简介 ....................................................................................................................... 6
1.2 Kalman滤波与组合导航原理简介 .............................................................................................. 7
第2章 捷联惯导姿态解算基础 ....................................................................................... 10
2.1反对称阵及其矩阵指数函数 ...................................................................................................... 10
2.1.1 反对称阵 .................................................................................................................................... 10 2.1.2 反对称阵的矩阵指数函数 ......................................................................................................... 12 2.2方向余弦阵与等效旋转矢量 ...................................................................................................... 13
2.2.1 方向余弦阵 ................................................................................................................................ 13 2.2.2 等效旋转矢量 ............................................................................................................................ 14 2.3方向余弦阵微分方程及其求解 .................................................................................................. 17
2.3.1 方向余弦阵微分方程 ................................................................................................................. 17 2.3.2 方向余弦阵微分方程的求解 ..................................................................................................... 17 2.4姿态更新的四元数表示 ............................................................................................................. 20
2.4.1 四元数的基本概念..................................................................................................................... 20 2.4.2 四元数微分方程......................................................................................................................... 23 2.4.3 四元数微分方程的求解 ............................................................................................................. 25 2.5等效旋转矢量微分方程及其泰勒级数解 ................................................................................... 26
2.5.1 等效旋转矢量微分方程 ............................................................................................................. 26 2.5.2 等效旋转矢量微分方程的泰勒级数解 ..................................................................................... 29 2.6圆锥运动条件下的等效旋转矢量算法 ....................................................................................... 31
2.6.1 圆锥运动的描述......................................................................................................................... 31 2.6.2 圆锥误差补偿算法..................................................................................................................... 33
第3章 地球形状与重力场基础 ....................................................................................... 40
3.1地球的形状描述 ........................................................................................................................ 40 3.2地球的正常重力场 ..................................................................................................................... 46 3.3地球重力场的球谐函数模型 ...................................................................................................... 50
3.3.1 球谐函数的基本概念 ................................................................................................................. 50 3.3.2 地球引力位函数......................................................................................................................... 58 3.3.3 重力位及重力计算..................................................................................................................... 63
第4章 捷联惯导更新算法及误差分析 .......................................................................... 70
4.1捷联惯导数值更新算法 ............................................................................................................. 70 4.1.1 姿态更新算法 ............................................................................................................................ 70 4.1.2 速度更新算法 ............................................................................................................................ 71 4.1.3 位置更新算法 ............................................................................................................................ 77 4.2捷联惯导误差方程 .................................................................................................................... 77
4.2.1惯性传感器测量误差 .................................................................................................................. 77 4.2.2姿态误差方程 ............................................................................................................................. 79 4.2.3速度误差方程 ............................................................................................................................. 80 4.2.4位置误差方程 ............................................................................................................................. 80 4.2.5误差方程的整理.......................................................................................................................... 81 4.3静基座误差特性分析 ................................................................................................................. 83
4.3.1 静基座误差方程......................................................................................................................... 83 4.3.2 高度通道 .................................................................................................................................... 84 4.3.3 水平通道 .................................................................................................................................... 84 4.3.4 水平通道的简化......................................................................................................................... 89 4.3.5 水平通道误差方程的仿真 ......................................................................................................... 91
3