捷联惯导算法与组合导航原理
讲义
严恭敏，翁浚 编著
西北工业大学 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章 捷联惯导更新算法及误差分析 .......................................................................... 69
4.1捷联惯导数值更新算法 ............................................................................................................. 69 4.1.1 姿态更新算法 ............................................................................................................................ 69 4.1.2 速度更新算法 ............................................................................................................................ 70 4.1.3 位置更新算法 ............................................................................................................................ 76 4.2捷联惯导误差方程 .................................................................................................................... 76
4.2.1惯性传感器测量误差 .................................................................................................................. 76 4.2.2姿态误差方程 ............................................................................................................................. 78 4.2.3速度误差方程 ............................................................................................................................. 79 4.2.4位置误差方程 ............................................................................................................................. 79 4.2.5误差方程的整理.......................................................................................................................... 80 4.3静基座误差特性分析 ................................................................................................................. 82
4.3.1 静基座误差方程......................................................................................................................... 82 4.3.2 高度通道 .................................................................................................................................... 83 4.3.3 水平通道 .................................................................................................................................... 83 4.3.4 水平通道的简化......................................................................................................................... 88 4.3.5 水平通道误差方程的仿真 ......................................................................................................... 90
3
第5章 卡尔曼滤波基本理论 .......................................................................................... 92
5.1递推最小二乘法 ........................................................................................................................ 92 5.2 Kalman滤波方程的推导 ........................................................................................................... 94 5.3连续时间随机系统的离散化与连续时间Kalman滤波 ............................................................. 101 5.4噪声相关条件下的Kalman滤波 .............................................................................................. 107 5.5序贯滤波 ................................................................................................................................. 111 5.6信息滤波与信息融合 ............................................................................................................... 114 5.7平方根滤波 ............................................................................................................................. 116 5.8遗忘滤波 ................................................................................................................................. 124 5.9 Sage-Husa自适应滤波 ......................................................................................................... 125 5.10最优平滑算法 ........................................................................................................................ 127 5.11非线性系统的EKF滤波、二阶滤波与迭代滤波 ..................................................................... 130 5.12间接滤波与滤波校正 ............................................................................................................. 136 5.13联邦滤波（待完善） ............................................................................................................. 136 5.14滤波的稳定性与可观测度分析 .............................................................................................. 141
第6章 初始对准及组合导航技术 ............................................................................... 147
6.1捷联惯导粗对准 ...................................................................................................................... 147
6.1.1矢量定姿原理 ........................................................................................................................... 147 6.1.2解析粗对准方法........................................................................................................................ 149 6.1.3间接粗对准方法........................................................................................................................ 152 6.2捷联惯导精对准 ...................................................................................................................... 153 6.3惯性/卫星组合导航 ................................................................................................................ 157
6.3.1空间杆臂误差 ........................................................................................................................... 157 6.3.2时间不同步误差........................................................................................................................ 158 6.3.3状态空间模型 ........................................................................................................................... 159 6.4车载惯性/里程仪组合导航 ...................................................................................................... 159
6.4.1航位推算算法 ........................................................................................................................... 159 6.4.2航位推算误差分析.................................................................................................................... 161 6.4.3惯性/里程仪组合 ...................................................................................................................... 164 6.5低成本姿态航向参考系统（AHRS） ......................................................................................... 167
6.5.1简化的惯导算法及误差方程 .................................................................................................... 168 6.5.2地磁场测量及误差方程 ............................................................................................................ 169 6.5.3低成本组合导航系统模型 ........................................................................................................ 170 6.5.4低成本惯导的姿态初始化 ........................................................................................................ 171 6.5.5捷联式地平仪的工作原理 ........................................................................................................ 173
第7章 捷联惯导与组合导航仿真 ................................................................................. 176
7.1飞行轨迹和惯性器件信息仿真 ................................................................................................ 176 7.1.1飞行轨迹设计 ........................................................................................................................... 176 7.1.2 捷联惯导反演算法................................................................................................................... 177 7.1.3 仿真 .......................................................................................................................................... 178 7.2捷联惯导仿真 .......................................................................................................................... 180
7.2.1 Matlab子函数 ........................................................................................................................... 180 7.2.2捷联惯导仿真主程序 ................................................................................................................ 185 7.3惯导/卫星组合导航仿真 .......................................................................................................... 186
7.3.1Matlab子函数 ............................................................................................................................ 186 7.3.2组合导航仿真主程序 ................................................................................................................ 187
附 录 ............................................................................................................................ 190
A一些重要的三维矢量运算关系 .................................................................................................... 190 B 运载体姿态的欧拉角描述 .......................................................................................................... 192 C 姿态更新的毕卡算法、龙格—库塔算法及精确数值解法 ........................................................... 199
4
D 从非直角坐标系到直角坐标系的矩阵变换 ................................................................................. 207 E 线性系统基本理论 ..................................................................................................................... 211 F 加权最小二乘估计 ..................................................................................................................... 216 G 矩阵求逆引理 ............................................................................................................................ 217 H 几种矩阵分解方法（QR、Cholesky与UD） ............................................................................... 219 I 二阶滤波中的引理证明 .............................................................................................................. 223 J 方差阵上界的证明 ..................................................................................................................... 225 K 三阶非奇异方阵的奇异值分解 ................................................................................................... 226 L Matlab仿真程序 ........................................................................................................................ 231 M 练习题 ....................................................................................................................................... 237
参考文献 ....................................................................................................................... 241
5