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

发布时间：2022-06-13 发布人：admin 分类：说明书资料大小：27.62M 资料格式：doc 举报版权申诉

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第1页.png

第1页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第2页.png

第2页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第3页.png

第3页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第4页.png

第4页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第5页.png

第5页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第6页.png

第6页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第7页.png

第7页 / 共283页

d52c80d8-5123-4537-83db-ef69054bd562.doc.pdf-第8页.png

第8页 / 共283页

第1章概述

1.1捷联惯导算法简介

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

第2章捷联惯导姿态解算基础

2.1反对称阵及其矩阵指数函数

2.1.1 反对称阵

2.1.2 反对称阵的矩阵指数函数

2.2方向余弦阵与等效旋转矢量

2.2.1 方向余弦阵

2.2.2 等效旋转矢量

2.3方向余弦阵微分方程及其求解

2.3.1 方向余弦阵微分方程

2.3.2 方向余弦阵微分方程的求解

2.4姿态更新的四元数表示

2.4.1 四元数的基本概念

2.4.2 四元数微分方程

2.4.3 四元数微分方程的求解

2.5等效旋转矢量微分方程及其泰勒级数解

2.5.1 等效旋转矢量微分方程

2.5.2 等效旋转矢量微分方程的泰勒级数解

2.6圆锥运动条件下的等效旋转矢量算法

2.6.1 圆锥运动的描述

2.6.2 圆锥误差补偿算法

2.7多项式角运动条件下的等效旋转矢量精确数值解

2.7.1 角运动的多项式描述

2.7.2 等效旋转矢量的迭代求解

第3章地球形状与重力场基础

3.1地球的形状描述

地球旋转椭球体的基本概念

子午圈椭圆与纬度的计算

旋转椭球表面的曲率半径

大地坐标与位置矩阵

大地坐标与地心直角坐标转换

3.2地球的正常重力场

圆球假设下的地球重力

旋转椭球假设下的地球重力

重力与高度的关系

3.3地球重力场的球谐函数模型

球谐函数的基本概念

引力位函数

重力位及重力计算

第4章捷联惯导更新算法及误差分析

4.1捷联惯导数值更新算法

常用坐标系的定义

姿态更新算法

比力方程、速度更新与划桨误差补偿算法

位置更新算法

4.2捷联惯导误差方程

惯性传感器测量误差模型

姿态误差方程

速度误差方程

位置误差方程

误差方程的整理

4.3静基座误差特性分析

静基座误差方程

高度通道

水平通道

水平通道的简化

水平通道误差特性仿真

第5章最优估计与卡尔曼滤波基本原理

5.1 几种最优估计方法

5.1.1 随机向量的概率密度函数

5.1.2 最优估计的基本概念

5.1.3 最小方差估计和线性最小方差估计

5.1.4 极大似然估计

5.1.5 极大验后估计

5.1.6 加权最小二乘估计

5.1.7 贝叶斯估计

5.1.8 维纳滤波

5.2 递推最小二乘估计

5.3 Kalman滤波方程的推导

5.3.1 随机系统状态空间模型

5.3.2 滤波方程的推导

5.3.3 Kalman滤波的几何解释

5.3.4 滤波流程框图与滤波初值的选择

5.3.5 带确定性输入时的滤波方程

5.3.6 Kalman滤波举例

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

5.4.1 系统方程的离散化

5.4.2 量测方程的离散化

5.4.3 连续时间卡尔曼滤波方程

第6章卡尔曼滤波的一些技术处理方法

6.1 噪声相关条件下的Kalman滤波

6.1.1系统噪声与量测噪声相关

6.1.2系统噪声为有色噪声

6.1.3量测噪声为有色噪声

6.2 序贯滤波

6.3 信息滤波与信息融合

6.3.1 信息滤波

6.3.2 信息融合

6.4 平方根滤波

6.4.1 Potter平方根滤波

6.4.2奇异值分解滤波

6.4.3 UD分解滤波

6.4.4平方根信息滤波

6.5 遗忘滤波

6.6 自适应滤波

6.7 量测故障检测与强跟踪滤波

6.7.1量测故障的检测与隔离

6.7.2量测方差自适应滤波

6.7.3单重渐消因子自适应滤波

6.7.4多重渐消因子自适应滤波

6.7.5基于序贯算法的多重渐消因子自适应滤波

6.8 最优平滑算法

6.9 非线性系统的滤波方法

6.9.1 向量函数的泰勒级数展开

6.9.2 EKF滤波

6.9.3 二阶滤波

6.9.4 迭代滤波

6.10 间接滤波与滤波校正

6.11 联邦滤波

6.11.1从序贯滤波到分散滤波

6.11.2联邦滤波

6.12 随机系统的可控性、可观性与滤波器的稳定性分析

6.12.1随机可控性与随机可观性

6.12.2滤波器的稳定性分析

6.13 状态估计的误差分配与可观测度分析

6.13.1误差分配基本原理

6.13.2可观测度分析

第7章初始对准与组合导航技术

7.1捷联惯导粗对准

矢量定姿原理

解析粗对准方法及其误差分析

间接粗对准方法

7.2捷联惯导精对准

7.3惯性/卫星组合导航

空间杆臂误差

时间不同步误差

状态空间模型

7.4车载惯导/里程仪组合导航

航位推算算法

航位推算误差分析

惯性里程仪组合

7.5低成本姿态航向参考系统

简化的惯导算法及误差方程

地磁场测量及误差方程

低成本组合导航系统模型

低成本惯导的姿态初始化

捷联式地平仪的工作原理

第8章捷联惯导与组合导航仿真

8.1飞行轨迹和惯性传感器仿真

飞行轨迹设计

捷联惯导反演算法

仿真

8.2捷联惯导算法仿真

子函数

捷联惯导算法仿真主程序

8.3惯导/卫星组合导航仿真

子函数

组合导航仿真主程序

附录

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

B 运载体姿态的欧拉角与罗德里格参数描述

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

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

E 线性系统基本理论

F 矩阵求逆引理

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

H 二阶滤波中的引理证明

I 方差阵上界的证明

J 三阶非奇异方阵的奇异值分解

K Matlab仿真程序

练习题

参考文献

捷联惯导算法与组合导航原理严恭敏，翁浚编著西北工业大学出版社 2019-3 1

内容简介本书是作者在总结多年研究生“卡尔曼滤波与组合导航原理”课程的教学经验，吸收十余年从事惯性导航与组合导航技术研究的科研成果，以及参阅国内外众多文献资料的基础上编写的，注重基础理论与工程实践相结合，实用性与可操作性强。全书共八章，主要包括捷联惯导算法及其误差分析、地球重力场基础、卡尔曼滤波基本原理、初始对准与组合导航技术、捷联惯导与组合导航仿真等内容。书中附有丰富的 Matlab 仿真程序可供参考，还有练习题可供读者拓展学习或学生练习使用。本书可作为导航制导与控制、仪器仪表及相关专业的高年级本科生、研究生的教学用书和参考书，也可供从事相关专业的科研和工程技术人员阅读参考。 2

前言近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法及其组合导航应用方面的有关问题，包括捷联惯导姿态解算基础、地球形状与重力场基础、捷联惯导更新算法及误差分析、最优估计与卡尔曼滤波基本原理、卡尔曼滤波的一些技术处理方法、初始对准与组合导航技术、捷联惯导与组合导航仿真等内容。希望读者参阅之后能够对捷联惯导算法与组合导航技术有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华、杨小康、李思锦、邓瑀、田野；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武、冯家政；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军、陈威、陈起金；山东科技大学：王云鹏；陕西中天火箭技术股份有限公司：王斯亮；火箭军工程大学：陈河；中国电子科技集团公司第十研究所：张保群；中国矿业大学：陈宇鸣；匿名网友：Zoro。书中缺点和错误在所难免，望读者不吝批评指正。作者 2019 年 3 月 3

目录第 1 章概述.................................................................................................................................. 8 1.1 捷联惯导算法简介 ........................................................................................................................8 1.2 Kalman 滤波与组合导航原理简介 ............................................................................................... 9 第 2 章捷联惯导姿态解算基础.......................................................................................................12 2.1 反对称阵及其矩阵指数函数 ...................................................................................................... 12 2.1.1 反对称阵 ....................................................................................................................................  2.1.2 反对称阵的矩阵指数函数 ........................................................................................................ 2.2 方向余弦阵与等效旋转矢量 ...................................................................................................... 16 2.2.1 方向余弦阵 ................................................................................................................................ 2.2.2 等效旋转矢量 ............................................................................................................................ 2.3 方向余弦阵微分方程及其求解 ...................................................................................................19 2.3.1 方向余弦阵微分方程 ................................................................................................................ 2.3.2 方向余弦阵微分方程的求解 .................................................................................................... 2.4 姿态更新的四元数表示 ..............................................................................................................22 2.4.1 四元数的基本概念 .................................................................................................................... 2.4.2 四元数微分方程 ........................................................................................................................ 2.4.3 四元数微分方程的求解 ............................................................................................................ 2.5 等效旋转矢量微分方程及其泰勒级数解 ....................................................................................29 2.5.1 等效旋转矢量微分方程 ............................................................................................................ 2.5.2 等效旋转矢量微分方程的泰勒级数解 .................................................................................... 2.6 圆锥运动条件下的等效旋转矢量算法 ........................................................................................34 2.6.1 圆锥运动的描述 ........................................................................................................................ 2.6.2 圆锥误差补偿算法 .................................................................................................................... 2.7 多项式角运动条件下的等效旋转矢量精确数值解 ..................................................................... 43 2.7.1 角运动的多项式描述 ................................................................................................................ 2.7.2 等效旋转矢量的迭代求解 ........................................................................................................ 第 3 章地球形状与重力场基础...................................................................................................... 46 3.1 地球的形状描述 ......................................................................................................................... 46 ..地球旋转椭球体的基本概念 ....................................................................................................... ..子午圈椭圆与纬度的计算 ........................................................................................................... ..旋转椭球表面的曲率半径 ........................................................................................................... ..大地坐标与位置矩阵 ................................................................................................................... ..大地坐标与地心直角坐标转换 ................................................................................................... 3.2 地球的正常重力场 ......................................................................................................................52 ..圆球假设下的地球重力 ............................................................................................................... ..旋转椭球假设下的地球重力 ....................................................................................................... ..重力与高度的关系 ....................................................................................................................... 3.3 地球重力场的球谐函数模型 .......................................................................................................57 ..球谐函数的基本概念 ................................................................................................................... ..引力位函数 ...................................................................................................................................  ..重力位及重力计算 ....................................................................................................................... 第 4 章捷联惯导更新算法及误差分析......................................................................................... 77 4.1 捷联惯导数值更新算法 ..............................................................................................................77 ..常用坐标系的定义 ....................................................................................................................... ..姿态更新算法 ............................................................................................................................... ..比力方程、速度更新与划桨误差补偿算法 ............................................................................... 4

..位置更新算法 ............................................................................................................................... 4.2 捷联惯导误差方程 ..................................................................................................................... 86 ..惯性传感器测量误差模型 ........................................................................................................... ..姿态误差方程 ............................................................................................................................... ..速度误差方程 ............................................................................................................................... ..位置误差方程 ............................................................................................................................... ..误差方程的整理 ........................................................................................................................... 4.3 静基座误差特性分析 ................................................................................................................. 92 ..静基座误差方程 ........................................................................................................................... ..高度通道 .......................................................................................................................................  ..水平通道 .......................................................................................................................................  ..水平通道的简化 ........................................................................................................................... ..水平通道误差特性仿真 ............................................................................................................. 第 5 章最优估计与卡尔曼滤波基本原理......................................................................................103 5.1 几种最优估计方法 .................................................................................................................. 103 5.1.1 随机向量的概率密度函数 ........................................................................................................ 5.1.2 最优估计的基本概念 ................................................................................................................ 5.1.3 最小方差估计和线性最小方差估计 ........................................................................................ 5.1.4 极大似然估计 ............................................................................................................................ 5.1.5 极大验后估计 ............................................................................................................................ 5.1.6 加权最小二乘估计 .................................................................................................................... 5.1.7 贝叶斯估计 ................................................................................................................................ 5.1.8 维纳滤波 .................................................................................................................................... 5.2 递推最小二乘估计 .................................................................................................................. 115 5.3 Kalman 滤波方程的推导 .......................................................................................................... 117 5.3.1 随机系统状态空间模型 ............................................................................................................ 5.3.2 滤波方程的推导 ........................................................................................................................ 5.3.3 Kalman 滤波的几何解释 ............................................................................................................  5.3.4 滤波流程框图与滤波初值的选择 ............................................................................................ 5.3.5 带确定性输入时的滤波方程 .................................................................................................... 5.3.6 Kalman 滤波举例 ........................................................................................................................  5.4 连续时间随机系统的离散化与连续时间 Kalman 滤波 .............................................................126 5.4.1 系统方程的离散化 .................................................................................................................... 5.4.2 量测方程的离散化 .................................................................................................................... 5.4.3 连续时间卡尔曼滤波方程 ........................................................................................................ 第 6 章卡尔曼滤波的一些技术处理方法......................................................................................134 6.1 噪声相关条件下的 Kalman 滤波 ..............................................................................................135 6.1.1 系统噪声与量测噪声相关 ......................................................................................................... 6.1.2 系统噪声为有色噪声 ................................................................................................................. 6.1.3 量测噪声为有色噪声 ................................................................................................................. 6.2 序贯滤波 .................................................................................................................................139 6.3 信息滤波与信息融合 ...............................................................................................................141 6.3.1 信息滤波 .................................................................................................................................... 6.3.2 信息融合 .................................................................................................................................... 6.4 平方根滤波 ............................................................................................................................. 145 6.4.1 Potter 平方根滤波 ......................................................................................................................  6.4.2 奇异值分解滤波 ......................................................................................................................... 6.4.3 UD 分解滤波 ...............................................................................................................................  6.4.4 平方根信息滤波 ......................................................................................................................... 6.5 遗忘滤波 .................................................................................................................................152 6.6 自适应滤波 ............................................................................................................................. 154 5

6.7 量测故障检测与强跟踪滤波 ................................................................................................... 156 6.7.1 量测故障的检测与隔离 ............................................................................................................. 6.7.2 量测方差自适应滤波 ................................................................................................................. 6.7.3 单重渐消因子自适应滤波 ......................................................................................................... 6.7.4 多重渐消因子自适应滤波 ......................................................................................................... 6.7.5 基于序贯算法的多重渐消因子自适应滤波 ............................................................................  6.8 最优平滑算法 ..........................................................................................................................159 6.9 非线性系统的滤波方法 ...........................................................................................................162 6.9.1 向量函数的泰勒级数展开 ........................................................................................................ 6.9.2 EKF 滤波 ......................................................................................................................................  6.9.3 二阶滤波 .................................................................................................................................... 6.9.4 迭代滤波 .................................................................................................................................... 6.10 间接滤波与滤波校正 .............................................................................................................168 6.11 联邦滤波 ............................................................................................................................... 171 6.11.1 从序贯滤波到分散滤波 ........................................................................................................... 6.11.2 联邦滤波 ................................................................................................................................... 6.12 随机系统的可控性、可观性与滤波器的稳定性分析 .............................................................177 6.12.1 随机可控性与随机可观性 ....................................................................................................... 6.12.2 滤波器的稳定性分析 ............................................................................................................... 6.13 状态估计的误差分配与可观测度分析 ...................................................................................181 6.13.1 误差分配基本原理 ................................................................................................................... 6.13.2 可观测度分析 ........................................................................................................................... 第 7 章初始对准与组合导航技术...............................................................................................186 7.1 捷联惯导粗对准 .......................................................................................................................186 ..矢量定姿原理 .............................................................................................................................. ..解析粗对准方法及其误差分析 .................................................................................................. ..间接粗对准方法 .......................................................................................................................... 7.2 捷联惯导精对准 .......................................................................................................................192 7.3 惯性/卫星组合导航 ................................................................................................................. 196 ..空间杆臂误差 .............................................................................................................................. ..时间不同步误差 .......................................................................................................................... ..状态空间模型 .............................................................................................................................. 7.4 车载惯导/里程仪组合导航 ...................................................................................................... 198 ..航位推算算法 .............................................................................................................................. ..航位推算误差分析 ...................................................................................................................... ..惯性里程仪组合 ......................................................................................................................... 7.5 低成本姿态航向参考系统 ........................................................................................................ 207 ..简化的惯导算法及误差方程 ...................................................................................................... ..地磁场测量及误差方程 .............................................................................................................. ..低成本组合导航系统模型 .......................................................................................................... ..低成本惯导的姿态初始化 .......................................................................................................... ..捷联式地平仪的工作原理 .......................................................................................................... 第 8 章捷联惯导与组合导航仿真.................................................................................................215 8.1 飞行轨迹和惯性传感器仿真 .....................................................................................................215 ..飞行轨迹设计 .............................................................................................................................. ..捷联惯导反演算法 ..................................................................................................................... ..仿真 .............................................................................................................................................  8.2 捷联惯导算法仿真 ....................................................................................................................220 ..子函数 ..........................................................................................................................  ..捷联惯导算法仿真主程序 .......................................................................................................... 8.3 惯导/卫星组合导航仿真 ........................................................................................................... 225 6

..子函数 ..........................................................................................................................  ..组合导航仿真主程序 .................................................................................................................. 附录.............................................................................................................................................229 A 一些重要的三维矢量运算关系 .................................................................................................... 229 B 运载体姿态的欧拉角与罗德里格参数描述 ................................................................................. 231 C 姿态更新的毕卡算法、龙格—库塔算法及精确数值解法 ...........................................................240 D 从非直角坐标系到直角坐标系的矩阵变换 ................................................................................. 247 E 线性系统基本理论 ......................................................................................................................251 F 矩阵求逆引理 ............................................................................................................................. 256 G 几种矩阵分解方法（QR、Cholesky 与 UD） ............................................................................... 258 H 二阶滤波中的引理证明 ...............................................................................................................262 I 方差阵上界的证明 ......................................................................................................................264 J 三阶非奇异方阵的奇异值分解 ................................................................................................... 265 K Matlab 仿真程序 .........................................................................................................................271 练习题........................................................................................................................................... 277 参考文献....................................................................................................................................... 282 7

第 1 章概述.................................................................................................................................. 8 1.1 捷联惯导算法简介 ........................................................................................................................8 1.2 Kalman 滤波与组合导航原理简介 ............................................................................................... 9 第 1 章概述 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 积分理论获得了不可 8

分享到：

赞收藏

资料库

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

相关推荐

课程资源

热门标签

最新资料