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

捷联惯导算法与组合导航原理讲义严恭敏，翁浚 编著西北工业大学2023-9前 言近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和 ，都有惯性技术存在甚至大显身手的身影。相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。本书在编写和定稿过程中得到以下同行的热心支持，指出了不少错误之处或提出了许多宝贵的修改建议，深表谢意：西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军；网友：Zoro；山东科技大学：王云鹏。书中缺点和错误在所难免，望读者不吝批评指正。作 者 2023年9月目 录第1章概述61.1捷联惯导算法简介61.2 Kalman滤波与组合导航原理简介7第2章捷联惯导姿态解算基础102.1反对称阵及其矩阵指数函数102.1.1 反对称阵102.1.2 反对称阵的矩阵指数函数122.2方向余弦阵与等效旋转矢量132.2.1 方向余弦阵132.2.2 等效旋转矢量142.3方向余弦阵微分方程及其求解172.3.1 方向余弦阵微分方程172.3.2 方向余弦阵微分方程的求解172.4姿态更新的四元数表示202.4.1 四元数的基本概念202.4.2 四元数微分方程232.4.3 四元数微分方程的求解252.5等效旋转矢量微分方程及其泰勒级数解262.5.1 等效旋转矢量微分方程262.5.2 等效旋转矢量微分方程的泰勒级数解292.6圆锥运动条件下的等效旋转矢量算法312.6.1 圆锥运动的描述312.6.2 圆锥误差补偿算法33第3章地球形状与重力场基础403.1地球的形状描述403.2地球的正常重力场463.3地球重力场的球谐函数模型503.3.1 球谐函数的基本概念503.3.2 地球引力位函数583.3.3 重力位及重力计算63第4章捷联惯导更新算法及误差分析694.1捷联惯导数值更新算法694.1.1 姿态更新算法694.1.2 速度更新算法704.1.3 位置更新算法764.2捷联惯导误差方程76惯性传感器测量误差76姿态误差方程78速度误差方程79位置误差方程79误差方程的整理804.3静基座误差特性分析824.3.1 静基座误差方程824.3.2 高度通道834.3.3 水平通道834.3.4 水平通道的简化884.3.5 水平通道误差方程的仿真90第5章卡尔曼滤波基本理论925.1递推最小二乘法925.2 Kalman滤波方程的推导945.3连续时间随机系统的离散化与连续时间Kalman滤波1015.4噪声相关条件下的Kalman滤波1075.5序贯滤波1115.6信息滤波与信息融合1145.7平方根滤波1165.8遗忘滤波1245.9 Sage-Husa自适应滤波1255.10最优平滑算法1275.11非线性系统的EKF滤波、二阶滤波与迭代滤波1305.12间接滤波与滤波校正1365.13联邦滤波（待完善）1365.14滤波的稳定性与可观测度分析141第6章初始对准及组合导航技术1476.1捷联惯导粗对准147矢量定姿原理147解析粗对准方法149间接粗对准方法1526.2捷联惯导精对准1536.3惯性/卫星组合导航157空间杆臂误差157时间不同步误差158状态空间模型1596.4车载惯性/里程仪组合导航159航位推算算法159航位推算误差分析1616.4.3惯性/里程仪组合1646.5低成本姿态航向参考系统（AHRS）167简化的惯导算法及误差方程1686.5.2地磁场测量及误差方程169低成本组合导航系统模型170低成本惯导的姿态初始化1716.5.5捷联式地平仪的工作原理173第7章捷联惯导与组合导航仿真1767.1飞行轨迹和惯性器件信息仿真176飞行轨迹设计1767.1.2 捷联惯导反演算法1777.1.3 仿真1787.2捷联惯导仿真1807.2.1 Matlab子函数180捷联惯导仿真主程序1857.3惯导/卫星组合导航仿真186子函数186组合导航仿真主程序187附录190A一些重要的三维矢量运算关系190B 运载体姿态的欧拉角描述192C 姿态更新的毕卡算法、龙格库塔算法及精确数值解法199D 从非直角坐标系到直角坐标系的矩阵变换207E 线性系统基本理论211F 加权最小二乘估计216G 矩阵求逆引理217H 几种矩阵分解方法（QR、Cholesky与UD）219I 二阶滤波中的引理证明223J 方差阵上界的证明225K 三阶非奇异方阵的奇异值分解226L Matlab仿真程序231M 练习题237参考文献241第1章 概 述第1章概述11.1捷联惯导算法简介11.2 Kalman滤波与组合导航原理简介31.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）以高速回路的处理结果作为输入再使用相对复杂的高阶算法进行姿态矩阵或四元数更新。双速回路算法的结构设计和实现过程都稍显繁琐，它只是在计算机运算能力低下时期所采取的权宜之策，随着通用计算机技术的飞速发展，尤其是80年代中后期之后，导航计算机的运算能力就不再是导航算法研究中需要着重关注的问题。双速回路算法的结构研究已经成为历史，目前的计算机完全能够满足高速高精度姿态更新解算的要求。1998年，P. G. Savage相继发表的两篇论文对整体捷联惯导数值算法进行了比较全面的总结，但相对于普通技术人员而言，其算法描述过于繁杂，给具体实现带来了很大的不便或困惑。1.2 Kalman滤波与组合导航原理简介如果信号受噪声干扰，为了从量测中恢复出有用信号而又要尽量减少干扰的影响，常常采用滤波器进行信号处理。使用经典滤波器时假定信号和干扰的频率分布不同，通过设计特定的滤波器带通和带止频段，实现有用信号和干扰的分离。但是，如果干扰的频段很宽，比如白噪声，在有用信号的频段范围内也必然会存在干扰，这时经典滤波器对滤除这部分干扰噪声无能为力。若有用信号和干扰噪声的频带相互重叠，信号处理时通常不再认为有用信号是确定性的，而是带有一定随机性的。对于随机信号不可能进行准确无误差的恢复，只能根据信号和噪声的统计特性，利用数理统计方法进行估计，并且一般采取某种统计准则使估计误差尽可能小。借用经典滤波器的术语，这种针对随机信号的统计估计方法也常常称为滤波器，或称为现代滤波器以区别于经典滤波器，但须注意经典滤波器和现代滤波器之间是有本质区别的。1 Kalman滤波早在1632年，伽利略（Galileo Galilei）就尝试用各种误差函数最小化的方法提出了估计理论问题。1801年，数学家高斯（Karl Gauss）将最小二乘估计法应用于谷神星的轨道跟踪和预测，取得了良好的效果。最小二乘估计以观测残差平方和最小作为估计准则，它不需要关于量测的任何统计信息，算法简单且实用性强，在参数估计领域获得了广泛的应用。但是，通常情况下最小二乘估计只能应用于静态参数估计，而不适用于动态系统的状态估计。20世纪40年代初期，维纳（NorbertWiener）开始将统计方法应用于通信系统和控制系统的研究中，提出了著名的维纳滤波理论。同一时期，柯尔莫哥洛夫（Andrey Kolmogorow）也进行了类似的研究。维纳滤波是一种从频域角度出发设计滤波器的方法，它根据有用信号和干扰信号的功率谱特性，通过构造和求解维纳霍夫（Wiener-Hopf）方程得到最佳滤波器的传递函数，给出了最小均方误差意义下的稳态解。但是，在一般情况下求解维纳霍夫方程极为困难，甚至是不可能的。此外，维纳滤波仅适用于低维平稳随机过程，人们试图将它推广到高维和非平稳情况，但都因无法突破计算上的困难而难以实用，这严重限制了维纳滤波的普及。维纳滤波在历史上有着非常重要的作用和独特的地位，它首次将数理统计理论和线性系统理论有机结合起来，形成了对随机信号进行估计的新理论，虽然维纳滤波不适合用于状态估计，但是它在信号处理和通信理论中依然十分有用。1960年，Rudolf Kalman将控制系统状态空间的概念引入随机估计理论中，建立了随机状态空间模型，利用了随机状态方程、量测方程以及激励白噪声的统计特性，构造估计算法对随机状态进行滤波估计，后来被称为Kalman（卡尔曼）滤波。在Kalman滤波中，所有利用的信息都是时域内的参量，它不但可以应用于一维平稳的随机过程，还可应用于多维非平稳过程，这就避免了Wiener滤波器设计的困境。Kalman滤波是一套由数字计算机实现的实时递推算法，它以随机系统的量测作为滤波器的输入，滤波器的输出是对系统状态的最优估计，这一特征与确定性控制系统中的状态观测器非常相似。在Kalman滤波器出现以后，估计理论的发展基本上都是以它为基础的一些推广和改进。20世纪60年代，Kalman滤波在美国的太空计划中获得了成功的应用，但是由于当时计算机字长较短，滤波器在实现过程中有时会出现一些问题，即计算机求解均方误差阵容易出现无穷大情况，导致滤波发散。平方根滤波是一种在数学上增加Kalman滤波精度的方法，Potter为“阿波罗”太空计划开发了第一个平方根滤波算法，它推动了后来一些其他平方根滤波方法的研究，比如Bierman提出的U-D分解滤波。平方根滤波精度性能的提升是以增加计算量为代价的，目前，随着计算机硬件技术的发展，普遍采用双精度浮点数进行计算和存储，多数情况下不必再像过去那样过于关注和担心数值问题了。经典Kalman滤波是基于线性系统的估计方法，一般只能适用于线性或者非常接近于线性的非线性问题，对于非线性比较明显的问题，Kalman滤波往往不能给出满意的结果，需要采用非线性估计方法。最为广泛使用的非线性估计方法是EKF（扩展卡尔曼滤波），它通过泰勒级数展开，对非线性函数进行线性化近似。同样，以泰勒级数展开为基础，若保留二阶项则称为二阶卡尔曼滤波方法，理论上二阶滤波降低了EKF的线性化误差，会得到比EKF稍好的估计性能，但这是以高复杂性和计算量为代价的。迭代滤波方法也是一种对EKF滤波的修正。随着系统规模的不断增大，如何有效处理多个传感器测量信息的问题被提出并得到了广泛的研究。传统的方法是采用集中式Kalman滤波，将所有测量信息送到中心处理器进行集中处理，虽然它的处理结果是全局最优的，但是这种处理方式存在通信负担重、计算量大和容错性能差等缺点。Speyer从分散控制的角度提出了多处理器结构思想，每个局部传感器都有自己的分处理器，处理包括自身在内的所有传感器的测量信息，得到的估计结果既是局部最优的也是全局最优的。Willsky对Speyer的方法进行了改进，提出了一个中心处理器（主）加多个局部处理器（子）的结构方式，主处理器完成各个子处理器结果的合成，各子处理器间不要求通信联系，因而是相互独立的。Carlson对分散滤波算法做了重大改进，提出了联邦滤波算法，采用信息分享原理，把全局状态估计信息和系统噪声信息分配给各个子滤波器，且不改变各子滤波器算法的形式，联邦滤波具有实现简单、信息分享方式灵活、容错性能好的诸多优点。2组合导航将运载体从起始点引导到目的地的技术或方法称为导航，导航系统提供的信息主要有姿态、方位、速度和位置，甚至还包括加速度和角速率，这些信息可用于运载体的正确操纵和控制。随着技术的发展，导航系统的种类越来越多，比如惯导系统、卫星导航系统、磁罗盘、里程仪/多普勒测速仪/空速计、气压高度表/雷达高度表、地标点/地图匹配等。这些导航系统各有特色，优缺点并存，比如惯导系统的优点是自主性强、动态性能好、导航信息全面且输出频率高，但其缺点是误差随时间不断累积，长期精度差；卫星导航系统的优点是精度高、误差不随时间增大，缺点是导航信息不够全面、频带窄、信号容易受到干扰、在室内等环境下接收不到卫星信号而无法使用。在许多对导航性能要求苛刻的任务中，无论是精度要求高还是可靠性要求高，任何单一的导航系统可能都无法满足要求，这就需要使用多种导航系统同时对运载体进行导航信息测量，再对所有测量信息作综合处理（包括检测、结合、相关和估计），从而得到更为准确和可靠的导航结果。这种对多种导航信息作综合处理的技术就称为组合导航技术。从上述对惯导和卫星导航的优缺点描述中可以看出，两者性能具有非常强的互补性，因而惯性/卫星组合导航被公认为是最佳的组合导航方案。组合导航系统的设计一般都采用Kalman滤波器，Kalman滤波器最早和最成功的应用实例便是在导航领域。1960年卡尔曼在美国国家航空航天局埃姆斯研究中心（NASAAmes Research Center）访问时，Stanley Schmidt发现Kalman滤波方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗登月飞船的导航系统便使用了Kalman滤波器，通常认为Schmidt首次实现了Kalman滤波器。此外，美国在航天飞机、潜艇和无人航空航天飞行器（比如巡航导弹）上均使用了Kalman滤波器。第2章 捷联惯导姿态解算基础第2章捷联惯导姿态解算基础12.1反对称阵及其矩阵指数函数12.1.1 反对称阵12.1.2 反对称阵的矩阵指数函数32.2方向余弦阵与等效旋转矢量42.2.1 方向余弦阵42.2.2 等效旋转矢量52.3方向余弦阵微分方程及其求解82.3.1 方向余弦阵微分方程82.3.2 方向余弦阵微分方程的求解82.4姿态更新的四元数表示112.4.1 四元数的基本概念112.4.2 四元数微分方程142.4.3 四元数微分方程的求解162.5等效旋转矢量微分方程及其泰勒级数解172.5.1 等效旋转矢量微分方程172.5.2 等效旋转矢量微分方程的泰勒级数解202.6圆锥运动条件下的等效旋转矢量算法222.6.1 圆锥运动的描述222.6.2 圆锥误差补偿算法24在捷联惯导系统的姿态、速度和位置更新算法中，姿态算法对整个系统精度的影响最大，它是算法研究和设计的核心。在非定轴转动情况下，描述姿态运动的微分方程是非线性的，其离散化求解会引起转动不可交换误差。现代高精度的捷联惯导中，陀螺仪往往采用角增量信号输出方式，利用角增量构造等效旋转矢量以补偿和降低不可交换误差，是目前主流姿态算法的基础。本章先介绍一些有关于刚体转动或坐标系变换的数学基础知识，之后重点讨论等效旋转矢量微分方程的推导及其离散化求解方法。2.1反对称阵及其矩阵指数函数2.1.1反对称阵两个三维列向量和之间的叉乘积，可利用行列式计算规则表示为（2.1-1）另一方面，若计算由向量中各元素构造的某种特殊矩阵与向量之间的矩阵乘法，可得（2.1-2）比较式（2.1-1）与式（2.1-2），容易发现它们的右端结果完全相同，因此，可记式（2.1-2）左端的特殊矩阵如下（2.1-3）并将其称为向量的反对称阵（或斜对称阵）。引入反对称阵概念后，两向量之间叉乘运算可等价表示为前一向量的反对称阵与后一向量之间的矩阵乘法运算，亦即（2.1-4）以后会看到，这一简单改写方式在许多场合会带来很大的便利。如果是实向量（以后在涉及反对称阵时未特别说明均作此假设），显然有（2.1-5）其中，右上标“”表示Hermite转置，即共轭转置。不难验证下式成立：（2.1-6）可见，反对称阵是正规矩阵(Normal Matrix)。根据矩阵理论知，正规矩阵可酉相似于对角阵，且不同特征值对应的特征向量两两正交。下面求解与对角阵之间的相似变换关系。首先，计算的特征多项式（2.1-7）其中，是向量的模值。令特征多项式，可解得的三个特征值如下（2.1-8）当时，不难求得与上式三个特征值相对应的单位特征向量，分别为（2.1-9）而当（甚至）时，可选择单位正交特征向量如下（2.1-10）实际上，反对称阵的复单位特征向量是不唯一的（见附录I练习题2），式（2.1-9）和（2.1-10）只给出了其中一组。如记（2.1-11）可验证有，因此是酉矩阵。根据矩阵特征值与特征向量之间的关系，有（2.1-12）上式两边同时左乘，得（2.1-13）至此，验证了可酉相似于对角阵，并求得了相应的相似变换矩阵。最后，给出反对称阵的幂方公式，如下综上，可写出通式（2.1-14）2.1.2 反对称阵的矩阵指数函数根据哈密顿凯莱（Hamilton-Cayley）定理，矩阵指数函数可以展开成的有限项级数形式，即（2.1-15）其中，、和为待定系数。根据式（2.1-13）和（2.1-15），有（2.1-16）上式两边矩阵都展开成元素分量形式，可得（2.1-17）将特征值式（2.1-8）代入式（2.1-17），比较两边对角线元素，可得如下方程组即 （2.1-18）从上式可解得待定系数（2.1-19）再将这些待定系数重新代回式（2.1-18），有反对称阵的矩阵函数求解公式（2.1-20）实际上，若直接将式（2.1-14）代入式（2.1-15）的求和符号中，亦可求得上式，即（2.1-21）此外，在式（2.1-16）中有，据此可得（2.1-22）对比上式与式（2.1-12），可知与反对称阵具有相同的特征向量，它们均为矩阵的列向量；并且矩阵函数与对角阵具有相同的特征值，分别为（2.1-23）根据以上特征值，易知有成立，所以是酉矩阵。由于多个酉矩阵之乘积仍然是酉矩阵，可知也是酉矩阵；此外，式（2.1-20）表明，若是实向量则是实矩阵，所以必定是单位正交阵，这一点亦可证明如下：（2.1-24）值得指出的是，由于，所以，在所有三阶单位正交阵中只有行列式为1者才可以表示成的形式，事实上，行列式为1的单位正交阵可称为右手直角坐标变换矩阵（反之，行列式为-1者可称为左手矩阵）。2.2方向余弦阵与等效旋转矢量2.2.1方向余弦阵若用分别表示直角坐标系（系）坐标轴上的单位矢量，而用表示（系）坐标轴向的单位矢量，则可分别用表示为：（2.2-1）实际上，上式表示的正是两直角坐标系之间的基变换公式，将其改写成矩阵的方式，如下（2.2-2）其中，为从系到系的过渡矩阵（或称从系到系的坐标系/基变换矩阵），即（2.2-3）假设有一个三维矢量，它在系和系下的投影坐标分别为和 若用投影表示法，则有（2.2-4）而若用坐标表示法，则有（2.2-5）将式（2.2-2）代入式（2.2-5）的右端，可得（2.2-6）从而有即 （2.2-7）其中，记为从系到系的坐标变换矩阵，也就是从系到系的坐标系变换矩阵（或过渡矩阵）。从几何含义上，不难验证过渡矩阵是单位正交阵（即有），比如对于式（2.2-3）中的第一行向量，它表示在系的投影，可记为，显然有，而第一行向量与第二行向量点乘为同理，可验证中任一行向量为单位向量，且任意两个不同行向量之间正交。由于矩阵中的每一个元素均表示两套坐标系（系和系）相应坐标轴之间夹角的余弦值，比如表示坐标轴与之间夹角的余弦值，即，因此常称为方向余弦阵（direction cosine matrix, DCM）。2.2.2 等效旋转矢量参见图2.2-1，三维空间中的某矢量绕另一单位矢量转动（设）角度，得矢量，以下求解转动前后两矢量与之间的几何运算关系。图2.2-1 等效旋转矢量不妨假设矢量和单位矢量具有共同的起始点，记的矢端在上的投影为。若以为圆心、为半径作圆，则的矢端也在该圆周上。在圆上取一点使得，则有（2.2-8）转动前的矢量相对于单位矢量可分解为平行于的分量和垂直于的分量，如下即 （2.2-9）其中（2.2-10）（2.2-11）同理，转动后的矢量相对于也可以分解为平行分量和垂直分量，如下即 （2.2-12）其中（2.2-13）（2.2-14）至此，将式（2.2-10）和式（2.2-14）代入式（2.2-12），可详细展开为（2.2-15）此外，由附录A三重矢积公式（A-3），即，可得（2.2-16）将式（2.2-16）代入式（2.2-15），得（2.2-17）其中记（2.2-18）式（2.2-17）称为罗德里格（Rodrigues）旋转公式，它建立了转动前后两矢量与之间的线性变换关系，该变换是转轴及转角的函数。直角坐标系上存在三个坐标轴向单位矢量，也可对它们实施旋转操作。假设有动坐标系系与参考坐标系系，两坐标系在起始时刻重合，接着系相对于系作定轴转动，即绕通过原点的单位矢量转动了角，也就是说，系坐标轴的单位矢量绕转动角得到系坐标轴的单位矢量。根据式（2.2-18），可得两坐标轴单位矢量之间的变换关系（2.2-19）再假设（2.2-20）将式（2.2-19）和（2.2-20）代入过渡矩阵式（2.2-2），得（2.2-21）这表明，矩阵正好是从参考坐标系系到动坐标系系的过渡矩阵，它也是从系到系的坐标变换矩阵，可重新记式（2.2-18）为：（2.2-22）进一步，若记和，则有，将其代入式（2.2-22），可得（2.2-23）这里称为等效旋转矢量（Rotation Vector），根据图2.2-1，等效旋转矢量的矢量方向表示转轴方向，而模值大小表示旋转角度大小。从转动的物理含义上看，表示的是相同的转动，这可通过将其代入式（2.2-23）进行验证，即与的取值无关。如果限定转角的取值范围，则等效旋转矢量和方向余弦阵之间存在一一对应关系。从坐标系的定轴转动中可以看出，等效旋转矢量（或单位转轴）是一种比较特殊的矢量，它在系和系下的坐标值完全相等，即有（或）。有时为了更加明确地显示系相对于系的转动关系，可利用下角标进行标注，比如（或）。将式（2.2-23）与向量反对称阵的矩阵函数（2.1-20）对比，可看出两者形式上完全一致，这说明式（2.1-20）中三维向量具有等效旋转矢量的物理含义。根据式（2.1-30）和（2.1-31）还可以看出，方向余弦阵的一个特征值恒为1（），与其对应的单位特征向量（）表示转轴方向；方向余弦阵的另外两个共轭特征值（和）即为等效旋转矢量模值的幂指函数，特征值的幅角表示等效旋转矢量的转角大小。若将方向余弦阵看作是等效旋转矢量的函数，可简记为（2.2-24）并且有（2.2-25）特别地，若分别取、和，则有（2.2-26a）（2.2-26b）（2.2-26c）上述三式分别称为以坐标轴为旋转轴的基本转动矩阵，或称Givens矩阵或初等旋转矩阵，空间的任意转动都可以由三次基本转动合成，参见附录B。由等效旋转矢量与方向余弦阵之间的一一对应关系可知，方向余弦阵虽然含有9个元素，但它只有3个独立参数，包含了6个约束条件，即行向量之间两两正交（3个）及每个行向量模值均为1（3个）。3个独立参数即为3个自由度，这与三维空间中的转动自由度是一致的。2.3方向余弦阵微分方程及其求解2.3.1 方向余弦阵微分方程假设动坐标系（系）和参考坐标系（系）具有共同的原点，系相对于系转动的角速度为，从系到系的坐标系变换矩阵记为，它是时变矩阵，再假设在系中有一固定矢量，则固定矢量在两坐标系下投影的转换关系（即坐标变换），为（2.3-1）将上式两边同时微分，得（2.3-2）注意到，是系中的固定矢量，则有；由于系相对于系的角速度为，则在系上观察的角速度应为（或写为），并且有，因而式（2.3-2）可化为即（2.3-3）由于上式对于任意系固定矢量都成立，任选三个不共面的非零矢量、和，则有显然矩阵可逆，所以必定有（2.3-4）这便是方向余弦阵微分方程，或称为姿态阵微分方程，它建立了动坐标系相对于参考坐标系之间方向余弦阵与动坐标系运动角速度之间的关系。此外，通过如下矢量运算关系比较上式两边，可得反对称阵的相似变换公式（2.3-5）根据式（2.3-4）和式（2.3-5），并考虑到是单位正交阵，即有，容易证明以下四种方向余弦阵微分方程是相互等价的：（2.3-6a）（2.3-6b）（2.3-6c）（2.3-6d）2.3.2 方向余弦阵微分方程的求解以下讨论微分方程的求解，为了书写简便，略去各量的上下角标，但明确写出时变量的时间参数，并记反对称阵，将姿态阵微分方程表示为（2.3-7）显然，这是一个典型的时变系数齐次微分方程，需采用毕卡（Peano-Baker/Picard?）法求解。首先，对式（2.3-7）在时间段上积分，得（2.3-8）由于上式右边第二项被积函数依然含有待求的，重复使用上式右边整体代入积分号内，第一次代入，得（2.3-9）第二次代入，可得（2.3-10）依此不断代入，便可得到以无限重积分表示的所谓的毕卡级数（2.3-11）上述级数是收敛的，但一般情况下得不到闭合形式的解（初等解），只有在所谓的定轴转动特殊情形下才容易得到闭合解。考虑时间段，对于任意时间参数，假设转动角速度满足如下可交换性条件（2.3-12）则有即（2.3-13）现计算以下微分（2.3-14）注意，上式的最后一个等号是在式（2.3-12）条件下才能成立的。根据式（2.3-14），有如下积分成立（2.3-15）同理，有（2.3-16）等等。至此，在可交换条件（2.3-12）下，毕卡级数式（2.3-11）可简化成闭合解形式（2.3-17）下面说明可交换条件式（2.3-12）的几何含义。设角速度的分量形式为和，则有（2.3-18）（2.3-19）令上述两式相等，可解得（2.3-20）如果式（2.3-20）中所有的角速率分量都不为0，则有（2.3-21）这表示在时间段内，系相对于系的转动角速度方向始终不变，即为定轴转动；如果式（2.3-20）中某些角速率分量为0，也容易得出该转动是定轴转动的结论；如果所有角速度分量均为0，即为静止，它亦可视为定轴转动的特殊情形。综合上述三种情况，说明闭合解式（2.3-17）只有在定轴转动情形下才能严格成立。针对时间段，记角增量且模值，考虑到矩阵指数函数式（2.1-19），则有（2.3-22）因此，式（2.3-17）在时刻的解可简写为（2.3-23）其中（2.3-24）若将时间区间从更改为，且假设已知时刻的方向余弦阵为，时间段的角增量为且记模值，则求解时刻的姿态阵的公式为（2.3-25）（2.3-26）上述两式便是姿态阵离散化更新的递推计算公式。值得注意的是，式（2.3-26）严格成立的前提条件是系在时间内必须是定轴转动，该式与式（2.2-23）相比，两者在形式上完全一致，因而可以认为定轴转动时角增量是以系为参考，系相对于系转动的等效旋转矢量；否则，如果可交换性条件式（2.3-12）不成立，依然简单地利用式（2.3-26）进行计算将会引起姿态求解的不可交换误差，不可交换性是高维时变系统（时变矩阵微分方程）的普遍特性。同理，类似于式（2.3-25）和（2.3-26），可求得另一种姿态阵微分方程表示形式的更新公式，为（2.3-27）（2.3-28）其中，、。显然，有成立。对于非定轴转动下的姿态更新方法，主要是通过等效旋转矢量算法进行不可交换误差补偿，这些内容将在本章后续小节详细介绍。2.4姿态更新的四元数表示四元数（quaternion）的概念最早在1843年由数学家哈密顿（W R Hamilton）提出，它可用于描述刚体转动或姿态变换，与方向余弦阵相比，四元数表示方法虽然比较抽象，但却十分的简洁。2.4.1 四元数的基本概念顾名思义，四元数就是包含四个元的一种数，可表示为（2.4-1）其中，、和都是实数，称为实部、称为虚部。四元数可以看作是复数概念的扩充，有时也称其为超复数，当时四元数即退化为复数。四元数的虚数单位之间满足如下乘法运算规则（2.4-2a）即（哈密顿公式） （2.4-2b）其中，运算符“”表示四元数乘法运算，在不引起歧义的情况下可写成“”号或直接省略。式（2.4-2a）中第一行运算规则与复数中虚数的运算规则完全相同；第二行运算规则与三维向量空间中坐标轴单位矢量的叉乘运算规则相同。四元数可以看作是四维空间中的一种数，但因其虚部单位矢量的叉乘运算特点，也可将四元数的虚数部分看成是在三维空间中的映象（image），反之，一个三维矢量可以看作是一个零标量四元数。假设有如下三个四元数（2.4-3a）（2.4-3b）（2.4-3c）两个四元数相等当且仅当它们的四个元分别对应相等，即（2.4-4）两个四元数之间的加（或减法）定义为（2.4-5a）或者记为（2.4-5b）容易验证，四元数的加法满足交换律和结合律，即有和。考虑到运算规则（2.4-2a），两个四元数的乘法结果为（2.4-6）特别地，两个零标量四元数相乘，可得（2.4-7）这是零标量四元数乘法运算规则与三维矢量运算规则之间的关系，上式右边同时包含了矢量的点乘运算和叉乘运算。实际上，运算规则式（2.4-2a）可视为式（2.4-7）应用于坐标轴单位矢量的特殊情形。若采用三维矢量运算表示法，四元数乘法可表示为（2.4-8）在式（2.4-7）中，由于矢量叉乘不满足交换律，因而四元数乘法也不满足交换律，即一般情况下；当且仅当，即两个四元数的虚部矢量相互平行（包括零矢量）时，才有。容易验证，四元数乘法运算满足结合律，且乘法对加法满足分配律和。可见，四元数乘法运算律与矩阵乘法是完全一致的。若采用矩阵表示法，四元数乘法式（2.4-6）还可写成（2.4-9a）或者（2.4-9b）其中（2.4-10a）（2.4-10b）为了简写方便，可定义三维向量的两种四维反对称阵，分别如下（2.4-11a）（2.4-11b）、分别称为第一和第二反对称阵，如果省略右下标“1”和“2”则默认为第一反对称阵。根据上述反对称阵定义，式（2.4-10）可简写为和 （2.4-12）四元数的共轭（转置）四元数定义为（2.4-13）两个四元数之和（或乘积）的共轭满足如下运算规则（2.4-14a）（2.4-14b）式（2.4-14a）显然成立；而采用乘法式（2.4-9b）容易验证式（2.4-14b）成立，即四元数的模值（2-范数）定义为（2.4-15）模值表示四元数在四维空间中的矢量长度。虽然一般情况下，但可以证明总有成立，即（2.4-16）对于非零四元数，即当时，有（2.4-17）因此，可以定义为非零四元数的逆，记作（2.4-18）两个非零四元数之乘积的逆满足运算规则：，验证如下（2.4-19）该运算规则与两矩阵乘积之逆也完全一致。如果，则称运算为四元数的归一化操作，归一化的四元数也称单位四元数，满足。显然，单位四元数的共轭与其逆相等，即有；两个单位四元数之乘积仍然是单位四元数，即如有且，则必有。类比于复数的三角表示法，四元数也可以表示为三角函数的形式（2.4-20）特别地，当时，即对于单位四元数，有（2.4-21）其中，且；为单位长度的三维矢量，即；表示某种角度值，后面将看到它的含义。通过前面的介绍不难发现，四元数的乘法不满足交换律、共轭及求逆等运算规律与矩阵的相应运算规律几乎完全一致，这似乎暗示着四元数与矩阵之间存在很强的内在联系。以下说明四元数三角表示法的几何意义。根据方向余弦阵式（2.2-22）进行恒等变形，可得（2.4-22）其中，为等效旋转矢量。将式（2.4-21）的实部和虚部代入式（2.4-22），可得（2.4-23）上式建立了单位四元数与方向余弦阵之间的关系，并且表明了单位四元数三角表示法式（2.4-21）的几何意义。为了更明确地表示两坐标系之间的转动变换关系，常在四元数的右边加上上下角标，写成，则式（2.4-21）中表示动坐标系（系）相对于参考坐标系（系）旋转的单位转轴、表示旋转角度大小。使用角标后，共轭四元数可记为，这与矩阵转置的表示方法类似，比如。2.4.2四元数微分方程假设有一个三维矢量，它在动坐标系（系）和参考坐标系（系）中的投影坐标分别为和，现对矢量（视为零标量四元数）实施如下四元数乘法操作（2.4-24）不难发现，上式右边矩阵中的右下角三阶对角分块矩阵恰好与式（2.4-23）一致，因而上式可简写为（2.4-25）这表明，的结果也是一个零标量四元数，其虚部正好对应于姿态阵坐标变换。为了书写简洁，类似于矩阵的坐标变换表达习惯，可定义四元数与三维矢量的乘法运算，即四元数坐标变换公式，如下（2.4-26）式中，乘法算符“”的含义本质上是先进行四元数乘法运算，再提取结果中的虚部（即矢量部分）。由式（2.4-25）两边同时右乘，可得（2.4-27）假设矢量是系中的固定矢量，即为常值矢量，并假设系绕系转动角速度为，则和都是时变量。在系中观察矢量，其相对于系的角速度为。将式（2.4-27）两边同时微分，考虑到，可得（2.4-28）由于，从而有，将其及式（2.4-25）一起代入式（2.4-28），可得（2.4-29）再将上式两边同时左乘，移项得（2.4-30）上式写成矩阵形式，为即 （2.4-31）由于可为任意固定矢量，类似于式（2.3-3），有即 （2.4-32）另一方面，四元数及其微分可分别写成和 （2.4-33）根据上述表达式，直接计算，得（2.4-34）由上式可见，的标量部分恒为零，因此，由式（2.4-32）可得即 （2.4-35）这便是四元数微分方程，它建立了变换四元数与坐标系旋转角速度之间的关系。与矩阵微分方程式（2.3-6）类似，容易证明以下四种四元数微分方程之间是相互等价的最后，比较式（2.4-32）和式（2.4-34）右端的矢量部分，可得（2.4-36）这是推导等效旋转矢量微分方程的基本公式，将在后续小节进一步介绍。实际上，根据方向余弦阵微分方程，并类比于式（2.4-34）将用等效旋转矢量展开，也可推得上式（2.4-36）。2.4.3 四元数微分方程的求解将四元数微分方程（2.4-35）写成矩阵形式，为（2.4-37）为表示简洁，这里暂且省略和角标，但明确给出了时间参数。如果角速度（即系数矩阵）是时变的，类似于方向余弦阵微分方程（2.3-7）的求解，只有在时间段内满足定轴转动条件时，等价于（2.4-38）才能求得闭合解（2.4-39）其中（2.4-40）（2.4-41）为时间段内的角增量，是其模值。为了计算式（2.4-39）中的指数函数，先求解反对称阵的各次幂，有（省略时间参数，和分别简记为和）所以，有（2.4-42）将式（2.4-42）代入式（2.4-39），可得（2.4-43）若将研究时间区间从改为，则根据上式有（2.4-44）（2.4-45）其中，、分别表示和时刻的姿态变换四元数，是从时刻到时刻的姿态四元数变化，且有和。式（2.4-44）和式（2.4-45）便是姿态更新的四元数递推计算公式，但应当注意到，这是在假设系在时间段内为定轴转动时才能严格成立的。2.5等效旋转矢量微分方程及其泰勒级数解方向余弦阵更新算法式（2.3-25）和四元数更新算法式（2.4-44）两种算法完全等价，只是后者计算量稍小一点而已。两种算法都是假设在更新周期内动坐标系作定轴转动才能严格成立，如果不是定轴转动，由角增量直接求解变化矩阵或四元数，容易引入转动不可交换误差。实际捷联惯导系统中的陀螺测量信号输出为角增量形式，比如激光陀螺，或者信号输出为角速率形式，但是为了降低噪声，常常采用了高频采样再滤波平滑处理后降频输出，这种方式也接近于增量输出方式，而非瞬时角速率输出。为了减小不可交换误差的影响，研究者们提出了先通过角增量求解等效旋转矢量、再利用等效旋转矢量更新方向余弦阵或四元数的方法。2.5.1 等效旋转矢量微分方程对于式（2.4-36），为书写简洁暂且省略角标，重写为（2.5-1）根据附录A式（A-6）（A-8），有，将它们代入式（2.5-1），得（2.5-2）上式移项，得（2.5-3）这是旋转矢量微分方程的一种表示形式，但稍显不足的是等式右边依然含有微分项，不利于实际使用。下面根据反对称阵的幂方特性，由式（2.5-2）求解。若记（2.5-4）则式（2.5-2）可简写为（2.5-5a）使用左叉乘上式的两边，可得（2.5-5b）再次使用左叉乘上式的两边，可得（2.5-5c）将上述三式合并在一起写成矩阵形式，有（2.5-6）若将式（2.5-6）视为关于、和的三元线性方程组，则不难求解得（2.5-7）最后，重新将式（2.5-4）中的和表达式代入上式，得（2.5-8）应用三角恒等式，上式还可等价于（2.5-9）这便是常见的等效旋转矢量微分方程，它是利用等效旋转矢量进行转动不可交换误差补偿的数学理论基础，该式最早于1971年由学者J E Bortz推导提出，后来通常称之为Bortz方程。至此，可总结坐标系相对转动的四种数学描述，即角速度、姿态阵、四元数和等效旋转矢量，之间的关系，如图2.5-1所示。更多姿态描述之间的相互转换关系参见附录B。图2.5-1转动的四种数学描述之间的关系Bortz方程（2.5-9）虽然在理论上是严格成立的，但实际应用时略显繁杂。当转动角度为小量时，常常将方程右边三角函数用泰勒级数展开，进行如下近似（2.5-10）如果再忽略上式右端三阶小量的影响，还可进一步近似为（2.5-11）对上式两边同时在时间段内积分，为了表述更加清晰，各符号标明时间变量参数，可得（2.5-12）即（2.5-13）其中，表示从时刻开始由角速度累积的角增量且显然有。将式（2.5-13）右端整体再次代入其第三项的积分号内，可得（2.5-14）在时间段内，如果是小量，式（2.5-14）右边的第5项远小于第4项，即有（2.5-15）因而，式（2.5-14）可近似为（2.5-16）特别地，若假设在时刻的等效旋转矢量，则可表示从时刻开始的等效旋转矢量“增量”，式（2.5-16）可简化为（2.5-17）其中，记（2.5-18）表示等效旋转矢量增量与角增量之间的差异，通常称为转动不可交换误差的修正量。对式（2.5-17）两边同时求导，可得（2.5-19）上式可以看作是等效旋转矢量微分方程（2.5-11）的再近一步近似。需要特别指出的是，上式成立的前提条件是：且需保证为小量，越小近似精度越高。此外，在式（2.5-16）中，若令且设，则有（2.5-20）其中，和分别表示上一时刻（时刻）和当前时刻（时刻）的等效旋转矢量；将、分别更明确地记为和，表示从上一时刻至当前时刻的角增量和修正量。式（2.5-20）可视为等效旋转矢量递推的近似计算公式，运算简单且计算量小，但是在实际算法中并不常用，究其原因，主要是随着递推步数的增加和变大，误差会不断积累。实际应用时，一般总是假设，再根据式（2.5-17）计算等效旋转矢量，相当于只递推计算一步，这样有利于保证等效旋转矢量始终为小量，降低公式推导过程中的近似误差。在获得之后，改等效旋转矢量递推计算为方向余弦阵或四元数递推，即改用方向余弦阵或四元数完成姿态递推更新，以四元数为例（方向余弦阵类似），等效旋转矢量与四元数相配合的姿态更新算法如下（2.5-21）（2.5-22）其中，将简记为，且有。注意，比较式（2.4-45）与式（2.5-22），两者虽然在形式上完全一样，但本质含义上存在重要区别：前者仅简单地使用角增量进行变化四元数计算，理论上只能适用于定轴转动情形；而后者在求解等效旋转矢量过程中考虑了转动不可交换误差的补偿。2.5.2 等效旋转矢量微分方程的泰勒级数解在实际应用中，从高精度捷联惯导陀螺中采样获得的往往是在一定采样间隔内的角增量信息，下文的主要目的就是借助式（2.5-19）由采样角增量求解等效旋转矢量。针对算法式（2.5-21）和（2.5-22），不妨将时刻重新记为0时刻，陀螺在姿态四元数更新时间段（即，）内可进行若干次等间隔角增量采样，暂且假设陀螺角速度输出为线性形式（2.5-23）则陀螺输出角增量为（2.5-24）其中，和均为常数向量。现计算角速度和角增量，以及它们的各阶导数，得（2.5-25）（2.5-26）再记（2.5-27）根据如下求导规则（2.5-28）求及其各阶导数，可得（2.5-29）根据等效旋转矢量微分方程式（2.5-19），可计算得的各阶导数，如下（2.5-30）若将视为光滑函数且在处展开成泰勒级数，并将式（2.5-30）代入，可得（2.5-31）式（2.5-31）中包含两个未知向量参数和，为了消去和并求解出，需在采样时间段内进行两次角增量采样，记为（2.5-32）由上式可求得以角增量表示的常数向量和，再将其代入式（2.5-31）便可求得以角增量表示的等效旋转矢量二子样算法（2.5-33）类似于