下载文档原格式
1 绪论随着计算机和微电子技术的迅猛发展，利用计算机的强大解算和控制功能代替机电稳定系统成为可能。
于是，一种新型惯导系统--捷联惯导系统从20世纪60年代初开始发展起来，尤其在1969年，捷联惯导系统作为"阿波罗"-13号登月飞船的应急备份装置，在其服务舱发生爆炸时将飞船成功地引导到返回地球的轨道上时起到了决定性作用，成为捷联式惯导系统发展中的一个里程碑。
捷联式惯性导航(strap-down inertial navigation)，捷联（strap-down）的英语原义是“捆绑”的意思。
因此捷联式惯性导航也就是将惯性测量元件（陀螺仪和加速度计）直接装在飞行器、舰艇、导弹等需要诸如姿态、速度、航向等导航信息的主体上，用计算机把测量信号变换为导航参数的一种导航技术。
现代电子计算机技术的迅速发展为捷联式惯性导航系统创造了条件。
惯性导航系统是利用惯性敏感器、基准方向及最初的位置信息来确定运载体的方位、位置和速度的自主式航位推算导航系统。
在工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰破坏。
它完全是依靠载体自身设备独立自主地进行导航，它与外界不发生任何光、声、磁、电的联系，从而实现了与外界条件隔绝的假想的“封闭”空间内实现精确导航。
所以它具有隐蔽性好，工作不受气象条件和人为的外界干扰等一系列的优点，这些优点使得惯性导航在航天、航空、航海和测量上都得到了广泛的运用[1]1.1 捷联惯导系统工作原理及特点惯导系统主要分为平台式惯导系统和捷联式惯导系统两大类。
惯导系统（INS）是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统，具有隐蔽性好，可在空中、地面、水下等各种复杂环境下工作的特点。
捷联惯导系统（SINS）是在平台式惯导系统基础上发展而来的，它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型计算机组成。
平台式惯导系统和捷联式惯导系统的主要区别是：前者有实体的物理平台，陀螺和加速度计置于陀螺稳定的平台上，该平台跟踪导航坐标系，以实现速度和位置解算，姿态数据直接取自于平台的环架；后者的陀螺和加速度计直接固连在载体上作为测量基准，它不再采用机电平台，惯性平台的功能由计算机完成，即在计算机内建立一个数学平台取代机电平台的功能，其飞行器姿态数据通过计算机计算得到，故有时也称其为"数学平台"，这是捷联惯导系统区别于平台式惯导系统的根本点。
捷联惯导算法与组合导航原理讲义一、捷联惯导算法捷联惯导（Inertial Navigation System，INS）是一种通过测量惯性传感器的运动参数实现导航定位的技术。
惯性导航系统中包括了加速度计和陀螺仪等传感器，通过测量物体的加速度和角速度，可以推算出物体的位置、速度和姿态等信息。
1.1加速度计加速度计是一种测量物体加速度的传感器。
常见的加速度计有基于压电效应的传感器和基于微机电系统（Microelectromechanical System，MEMS）的传感器。
加速度计的原理是通过测量物体受到的惯性力，推算出物体的加速度。
由于加速度是速度对时间的导数，因此通过对加速度的积分操作，可以计算出物体的速度和位移。
1.2陀螺仪陀螺仪是一种测量物体角速度的传感器。
常见的陀螺仪有机械陀螺仪和MEMS陀螺仪等。
陀螺仪的原理是基于角动量守恒定律，通过测量转动惯量的变化，推算出物体的角速度。
与加速度计类似，通过对角速度的积分操作，可以计算物体的姿态。
1.3捷联惯导算法离散时间模型中，位置、速度和姿态等状态变量通过积分加速度和角速度来更新。
由于加速度计和陀螺仪测量结果存在噪声，因此在积分操作时需要加入误差补偿算法来消除误差。
常见的误差补偿算法有零偏校正和比例积分修正等。
连续时间模型中，位置、速度和姿态等状态变量通过微分方程来描述，并通过求解微分方程来更新状态。
由于计算量较大，通常需要使用数值积分方法来求解微分方程。
常见的数值积分方法有欧拉法、中点法和四阶龙格-库塔法等。
二、组合导航原理组合导航是一种融合多种导航技术的导航方式。
常见的组合导航方式有捷联惯导与GPS组合导航。
组合导航通过融合多种导航系统的测量结果，可以提高导航定位的精度和可靠性。
2.1捷联惯导与GPS组合导航捷联惯导与GPS组合导航是一种常见的组合导航方式。
在这种方式下，捷联惯导提供了高频率的惯导数据，可以提供较高的定位精度，但是由于其测量结果累积误差较大，会逐渐偏离真实轨迹。
捷联惯导算法心得1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。
地理坐标系：东、北、天，以下简称地理。
在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。
机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。
换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。
换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。
系数就是描述机体姿态的表达方法的具体数值。
姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。
2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。
3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。
狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。
理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。
所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。
5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。
只留下地理水平面上的向量。
误差可以用来纠正公式的系数（航向）。
6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。
捷联惯导算法与组合导航原理讲义严恭敏，翁浚编著西北工业大学2021-9前言近年来，惯性技术不管在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。
相应地，惯性技术的研究和开发也获得前所未有的蓬勃开展，越来越多的高校学生、爱好者和工程技术人员参加到惯性技术的研发队伍中来。
惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法根本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。
希望读者参阅之后可以对捷联惯导算法有个系统而深化的理解，并能快速而有效地将根本算法应用于解决实际问题。
本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多珍贵的修改建议，深表谢意：西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军；网友：Zoro；山东科技大学：王云鹏。
书中缺点和错误在所难免，望读者不吝批评指正。
作者2021年9月目录第1章概述 (6)捷联惯导算法简介 (6)1.2 Kalman滤波与组合导航原理简介 (7)第2章捷联惯导姿态解算根底 (10)反对称阵及其矩阵指数函数 (10)2.1.1 反对称阵 (10)2.1.2 反对称阵的矩阵指数函数 (12)方向余弦阵与等效旋转矢量 (13)2.2.1 方向余弦阵 (13)2.2.2 等效旋转矢量 (14)方向余弦阵微分方程及其求解 (16)2.3.1 方向余弦阵微分方程 (16)2.3.2 方向余弦阵微分方程的求解 (17)姿态更新的四元数表示 (19)2.4.1 四元数的根本概念 (19)2.4.2 四元数微分方程 (23)2.4.3 四元数微分方程的求解 (24)等效旋转矢量微分方程及其泰勒级数解 (26)2.5.1 等效旋转矢量微分方程 (26)2.5.2 等效旋转矢量微分方程的泰勒级数解 (29)圆锥运动条件下的等效旋转矢量算法 (31)2.6.1 圆锥运动的描绘 (31)2.6.2 圆锥误差补偿算法 (33)第3章地球形状与重力场根底 (40)地球的形状描绘 (40)地球的正常重力场 (46)地球重力场的球谐函数模型 (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)捷联惯导误差方程 (76)惯性传感器测量误差 (76)姿态误差方程 (78)速度误差方程 (79)位置误差方程 (79)误差方程的整理 (80)静基座误差特性分析 (82)4.3.1 静基座误差方程 (82)4.3.2 高度通道 (83)4.3.3 程度通道 (83)4.3.4 程度通道的简化 (88)4.3.5 程度通道误差方程的仿真 (90)第5章卡尔曼滤波根本理论 (92)递推最小二乘法 (92)5.2 Kalman滤波方程的推导 (94)连续时间随机系统的离散化与连续时间Kalman滤波 (101)噪声相关条件下的Kalman滤波 (107)序贯滤波 (111)信息滤波与信息交融 (113)平方根滤波 (116)遗忘滤波 (123)5.9 Sage-Husa自适应滤波 (125)最优平滑算法 (126)非线性系统的EKF滤波、二阶滤波与迭代滤波 (129)间接滤波与滤波校正 (135)联邦滤波〔待完善〕 (135)滤波的稳定性与可观测度分析 (140)第6章初始对准及组合导航技术 (146)捷联惯导粗对准 (146)矢量定姿原理 (146)解析粗对准方法 (148)间接粗对准方法 (151)捷联惯导精对准 (152)惯性/卫星组合导航 (156)空间杆臂误差 (156)时间不同步误差 (157)状态空间模型 (157)车载惯性/里程仪组合导航 (158)航位推算算法 (158)航位推算误差分析 (160)6.惯性/里程仪组合 (163)低本钱姿态航向参考系统〔AHRS〕 (166)简化的惯导算法及误差方程 (166)6地磁场测量及误差方程 (168)低本钱组合导航系统模型 (169)低本钱惯导的姿态初始化 (170).5捷联式地平仪的工作原理 (172)第7章捷联惯导与组合导航仿真 (175)飞行轨迹和惯性器件信息仿真 (175)飞行轨迹设计 (175)7.1.2 捷联惯导反演算法 (176)7.1.3 仿真 (177)捷联惯导仿真 (179)7.2.1 Matlab子函数 (179)捷联惯导仿真主程序 (184)惯导/卫星组合导航仿真 (184)子函数 (184)组合导航仿真主程序 (186)附录 (188)A一些重要的三维矢量运算关系 (188)B 运载体姿态的欧拉角描绘 (190)C 姿态更新的毕卡算法、龙格—库塔算法及准确数值解法 (197)D 从非直角坐标系到直角坐标系的矩阵变换 (205)E 线性系统根本理论 (209)F 加权最小二乘估计 (214)G 矩阵求逆引理 (215)H 几种矩阵分解方法〔QR、Cholesky与UD〕 (217)I 二阶滤波中的引理证明 (221)J 方差阵上界的证明 (223)K 三阶非奇异方阵的奇异值分解 (224)L Matlab仿真程序 (229)M 练习题 (235)参考文献 (239)第1章概述第1章概述 (6)捷联惯导算法简介 (6)1.2 Kalman滤波与组合导航原理简介 (7)1.1捷联惯导算法简介在捷联惯导系统〔SINS〕中惯性测量器件〔陀螺和加速度计〕直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进展数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。
一种新的船用捷联惯导系统阻尼算法随着航运工业的不断发展，船舶导航系统也越来越得到了广泛的应用。
捷联惯导系统是现代导航系统中使用最广泛的一种，其具有高精度、长稳定性、无需外界支持等优势。
而在以往的航行过程中，捷联惯导系统在遇到海浪等环境因素时容易出现抖动，对航行稳定性造成了很大影响。
为了解决这个问题，我们提出了一种新的船用捷联惯导系统阻尼算法。
传统的阻尼算法是通过增加阻尼器的负载，使得惯性元件动能逐渐消耗来达到减少捷联惯导系统抖动的目的。
但这种方式会极大地增加系统的成本，同时也会使得系统的响应速度下降。
而我们的新算法则采用了多层次、多维度的动态阻尼控制策略，在保持系统灵敏度的同时，实现了抖动的最佳消除。
我们的新算法的主要特点如下：1.多层次阻尼控制策略我们的算法通过设置多层次的阻尼控制，使得阻尼器对系统的影响逐渐加重。
在海浪等恶劣环境下，我们可以根据船舶状态进行智能控制，选择适当的阻尼级别来抵消环境带来的影响。
2.多维度阻尼控制策略除了多层次的控制外，我们的算法还对系统的多个维度进行了分层控制。
这样可以更好地适应不同的风险、不同的航行模式，同时还可以更加灵活地应对突发状况。
3.动态自适应控制策略我们的算法还利用了机器学习技术，通过对实际航行数据的学习，实现了动态自适应的控制。
这样可以随着航行时不断变化的环境因素，实现对抖动的实时响应。
通过以上特点的结合，我们的新算法可以有效地消除捷联惯导系统的抖动，提升船舶导航系统的稳定性和精度。
与传统算法相比，我们的算法成本更低，响应速度更快，适用范围更广。
总之，我们的船用捷联惯导系统阻尼算法在航运工业中具有非常广泛的应用前景。
希望未来可以得到更多实际应用的机会，并为航运行业的发展作出更多贡献。
为了更好地支持上面提出的船用捷联惯导系统阻尼算法，我们可以列出相关数据并进行分析。
以下是几个可能对算法性能有影响的数据指标：1.海浪高度和周期海浪的高度和周期是船舶航行中最重要的环境因素之一。
1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。
地理坐标系：东、北、天，以下简称地理。
在这个坐标系里有重力永远是（0,0,1g），地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。
机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向量。
换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。
换算公式的系数：以下简称系数，四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。
系数就是描述机体姿态的表达方法的具体数值。
姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示，系数就是横滚xx度俯仰xx度航向xx度。
2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三个来自机体。
3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很独立，直接在公式的老系数上积分，得到新系数。
狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩阵、四元数，他们的积分算法有增量法、数值积分法（X阶龙格-库塔）等等4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g）用公式换算到机体，与机体的加计向量算出误差。
理论上应该没有误差，这误差逆向思维一下，其实就是换算公式的系数误差。
所以这误差可用于纠正公式的系数（横滚、俯仰），也就是姿态。
5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无用。
只留下地理水平面上的向量。
误差可以用来纠正公式的系数（航向）。
6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的转换。
常用的三种公式，它们之间都有转换算法。
再搞个直白一点的例子：机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。
舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。
当然如果舵手很牛逼，也许能估计很准确，维持很长时间。
不过只信任舵手，肯定会迷路，所以一般都有地图和瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。
然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。
舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%*ERR，觉得天气不好、地图误差大，那就听个10%*ERR，根据这个来纠正估算航向。
------------------------------------------------------来点干货，注意以下的欧拉角都是这样的顺序：先航向-再俯仰-然后横滚公式截图来自：袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。
--------------------------------------------------根据加计计算初始欧拉角这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要，因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);imu.euler.y = -asin(imu.accel.x / ACCEL_1G);ACCEL_1G 为9.81米/秒^2，accel.xyz的都为这个单位，算出来的euler.xyz单位是弧度航向imu.euler.z可以用电子罗盘计算--------------------------------------------------欧拉角微分方程如果用欧拉角算法，那么这个公式就够了，不需要来回转换。
矩阵上到下三个角度（希腊字母）是roll pitch和yaw，公式最左边的上面带点的三个是本次更新后的角度，不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度，单位为弧度，计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.--------------------------------------------------以下是四元数--------------------------------------------------四元数初始化q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化--------------------------------------------------四元数微分方程四元数更新算法，一阶龙库法，同样4个量（入、P1-3）也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如上面的0.02秒。
再来一张，另外一本书上的，仔细看和上面是一样的delta角度，就是上面的角速度*周期，单位为弧度--------------------------------------------------四元数微分方程更新后的规范化每个周期更新完四元数，需要对四元数做规范化处理。
因为四元数本来就定义为四维单位向量。
求q0-3的平方和，再开根号算出的向量长度length。
然后每个q0-3除这个length。
--------------------------------------------------四元数转欧拉角公式把四元数转成了方向余弦矩阵中的几个元素，再用这几个元素转成了欧拉角先从四元数q0-3转成方向余弦矩阵：再从方向余弦矩阵转成欧拉角代码：//更新方向余弦矩阵t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;t12=2.0*(q.q1*q.q2+q.q0*q.q3);t13=2.0*(q.q1*q.q3-q.q0*q.q2);t21=2.0*(q.q1*q.q2-q.q0*q.q3);t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;t23=2.0*(q.q2*q.q3+q.q0*q.q1);t31=2.0*(q.q1*q.q3+q.q0*q.q2);t32=2.0*(q.q2*q.q3-q.q0*q.q1);t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;//求出欧拉角imu.euler.roll = atan2(t23,t33);imu.euler.pitch = -asin(t13);imu.euler.yaw = atan2(t12,t11);if (imu.euler.yaw < 0){imu.euler.yaw += ToRad(360);}----------------------------------------------------以下代码摘自网上，很巧妙，附上注释，有四元数微分，有加计耦合。
没电子罗盘，其实耦合原理也一样。
1.//=====================================================================================================2.// IMU.c3.// S.O.H. Madgwick4.// 25th September 20105.//=====================================================================================================6.// Description:7.//8.// Quaternion implementation of the 'DCM filter' [Mayhony et al].9.//10.// User must define 'halfT' as the (sample period / 2), and the filtergains 'Kp' and 'Ki'.11.//12.// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elementsrepresenting the estimated13.// orientation. See my report for an overview of the use of quaternionsin this application.14.//15.// User must call 'IMUupdate()' every sample period and parse calibratedgyroscope ('gx', 'gy', 'gz')16.// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units areradians/second, accelerometer17.// units are irrelevant as the vector is normalised.18.//19.//=====================================================================================================20.21.//----------------------------------------------------------------------------------------------------22.// Header files23.24.#include "IMU.h"25.#include <math.h>26.27.//----------------------------------------------------------------------------------------------------28.// Definitions29.30.#define Kp 2.0f // proportional gain governs rateof convergence to accelerometer/magnetometer31.#define Ki 0.005f // integral gain governs rate ofconvergence of gyroscope biases32.#define halfT 0.5f // half the sample period33.34.//---------------------------------------------------------------------------------------------------35.// Variable definitions36.37.float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elementsrepresenting the estimated orientation38.float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error39.40.//====================================================================================================41.// Function42.//====================================================================================================43.44.void IMUupdate(float gx, float gy, float gz, float ax, float ay, floataz) {45. float norm;46. float vx, vy, vz;47. float ex, ey, ez;48.49. // normalise the measurements50. norm = sqrt(ax*ax + ay*ay + az*az);51. ax = ax / norm;52. ay = ay / norm;53. az = az / norm;54.把加计的三维向量转成单位向量。
