捷联惯导算法与组合导航原理讲义.doc

捷联惯导算法与组合导航原理讲义.doc

《捷联惯导算法与组合导航原理讲义.doc》由会员分享，可在线阅读，更多相关《捷联惯导算法与组合导航原理讲义.doc（242页珍藏版）》请在冰豆网上搜索。

捷联惯导算法与组合导航原理

讲义

严恭敏，翁浚编著

西北工业大学

2016-9

前言

近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。

相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。

惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。

希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。

本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：

西北工业大学自动化学院：

梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：

王亚军；辽宁工程技术大学：

丁伟；北京腾盛科技有限公司：

刘兴华；东南大学：

童金武；中国农业大学：

包建华；南京航空航天大学：

赵宣懿；武汉大学：

董翠军；网友：

Zoro；山东科技大学：

王云鹏。

书中缺点和错误在所难免，望读者不吝批评指正。

作者

2016年9月

目录

第1章概述 6

1.1捷联惯导算法简介 6

1.2Kalman滤波与组合导航原理简介 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

第5章卡尔曼滤波基本理论 92

5.1递推最小二乘法 92

5.2Kalman滤波方程的推导 94

5.3连续时间随机系统的离散化与连续时间Kalman滤波 101

5.4噪声相关条件下的Kalman滤波 107

5.5序贯滤波 111

5.6信息滤波与信息融合 114

5.7平方根滤波 116

5.8遗忘滤波 124

5.9Sage-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.1Matlab子函数 180

7.2.2捷联惯导仿真主程序 185

7.3惯导/卫星组合导航仿真 186

7.3.1Matlab子函数 186

7.3.2组合导航仿真主程序 187

附录 190

A一些重要的三维矢量运算关系 190

B运载体姿态的欧拉角描述 192

C姿态更新的毕卡算法、龙格—库塔算法及精确数值解法 199

D从非直角坐标系到直角坐标系的矩阵变换 207

E线性系统基本理论 211

F加权最小二乘估计 216

G矩阵求逆引理 217

H几种矩阵分解方法（QR、Cholesky与UD） 219

I二阶滤波中的引理证明 223

J方差阵上界的证明 225

K三阶非奇异方阵的奇异值分解 226

LMatlab仿真程序 231

M练习题 237

参考文献 241

第1章概述

第1章概述 1

1.1捷联惯导算法简介 1

1.2Kalman滤波与组合导航原理简介 3

1.1捷联惯导算法简介

在捷联惯导系统（SINS）中惯性测量器件（陀螺和加速度计）直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进行数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。

特别在恶劣的高动态环境下，高精度的SINS对惯性器件性能和导航算法精度的要求都非常苛刻，由于高精度惯性器件往往价格昂贵并且进一步提升精度异常困难，所以在影响SINS精度的所有误差源中要求因导航算法引起的误差比重必须很小，一般认为应小于5%。

姿态更新算法是SINS算法的核心，对整个系统的解算精度影响最为突出，具有重要的研究和应用价值。

传统的姿态更新算法有欧拉角法、方向余弦阵法和四元数法等方法，这些方法直接以陀螺采样输出作为输入，使用泰勒级数展开或龙格—库塔等方法求解姿态微分方程，未充分考虑转动的不可交换性误差问题。

传统姿态更新算法在理论上可以通过提高采样和更新频率来提高解算精度，但实际陀螺采样频率又受限于传感器的带宽和噪声水平，因此传统算法的精度提升空间相对有限，仅适用于对解算精度要求不太高的场合。

早在1775年，欧拉就提出了等效旋转矢量的概念，指出刚体的定点转动（即绕固定点的任何有限角位移）均可用绕经过该固定点的某轴的一次转动来实现，建立了刚体上单位矢量在转动前后的变换公式。

1840年，罗德里格使用后人称之为罗德里格参数的表示方法，推导了相继两次转动的合成公式，它与W.R.Hamilton在1843年发明的四元数乘法表示是一致的。

研究表明，相继多次的定点转动问题可用一系列的姿态变化量（变化四元数或变化矩阵）相乘来描述，每个姿态变化量与对应转动的等效旋转矢量之间存在转换公式，使用等效旋转矢量计算姿态变化量不存在任何原理上的误差。

因此，现代的SINS姿态更新算法研究的关键就在于如何使用陀螺输出构造等效旋转矢量，以尽量减小和避免不可交换性误差，后续再使用等效旋转矢量计算姿态变化量和进行姿态更新将变得非常简单，而不像传统方法那样，直接使用陀螺输出进行姿态更新容易引起不可交换性误差。

1949年，J.H.Laning在研究火控系统的过程中详细地分析了空间转动合成的性质，推导了由等效旋转矢量确定转动角速度的公式，但是由于缺少更好的应用背景驱动（比如后来SINS发展的迫切需求），未能获得广泛的研究重视。

20世纪50年代是机械陀螺仪飞速发展的一个重要时期，也正是在那时发现了著名的圆锥运动现象，即当陀螺仪在其旋转轴和输出轴出现同频不同相的角振动时，尽管其输入轴净指向不变（在整体上没有随时间改变的趋势项），但陀螺仪还是会敏感到并输出常值角速率。

1958年，为揭示圆锥运动现象产生的根源，L.E.Goodman建立了刚体转动的等效旋转矢量与角速度之间的关系式，后人称之为Goodman-Robinson定理，该定理从几何上将转动不可交换性误差的坐标分量描述为单位球面上的一块有向面积，其面积由对应动坐标轴在单位球面上扫过的曲线与连接该曲线端点的大圆围成，Goodman借助二维Green积分理论获得了不可交换性误差的近似公式。

1969年，基于Goodman近似公式，J.W.Jordan在假设陀螺角增量输出为二次多项式条件下提出了等效旋转矢量的“pre-processor”算法，它与后来发展的等效旋转矢量二子样算法完全一致。

1969年，J.E.Bortz在其博士论文中详细推导了等效旋转矢量微分方程（1971年正式发表，后人称之为Bortz方程），它是利用陀螺输出求解等效旋转矢量的基本公式，奠定了等效旋转矢量多子样算法的理论基础。

在实际应用时一般需对较复杂的Bortz方程做近似处理，事实上，其简化结果与Goodman公式完全一致，它也可以根据Laning公式简化获得。

1983年，R.B.Miller采用在圆锥运动条件下使算法漂移误差最小作为评价标准，推导了等效旋转矢量三子样优化算法。

1990年，J.E.Lee研究了四子样优化算法。

1992年，Y.F.Jiang研究了利用本更新周期内的三子样及前更新周期内的角增量计算旋转矢量的优化算法。

1996年，M.B.Ignagni提出了由陀螺角增量构造等效旋转矢量的通式，并给出了多达10种类型的等效旋转矢量算法。

1999年，C.G.Park总结提出了各子样下求解圆锥误差补偿系数和算法漂移误差估计的通用公式。

至此，从理论上看，在理想的圆锥运动条件下的不可交换性误差补偿问题得到了比较完美的解决。

捷联惯导的基本概念在20世纪50年代就已经提出了，但是由于当时计算机的运算能力极其有限，在算法发展的早期阶段姿态更新通常采用双速回路算法方案：

高速回路（e.g.,400Hz-10kHz）使用简单的一阶算法补偿由角振动引起的姿态不可交换性误差；中速回路（e.g.,50Hz-200Hz）以高速回路的处理结果作为输入再使用相对复杂的高阶算法进行姿态矩阵或四元数更新。

双速回路算法的结构设计和实现过程都稍显繁琐，它只是在计算机运算能力低下时期所采取的权宜之策，随