第1章 概 述
第1章 概 述 .................................................................................................................. 6
1.1捷联惯导算法简介 ....................................................................................................................... 6
1.2 Kalman滤波与组合导航原理简介 .............................................................................................. 7
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积分理论获得了不可
6
交换性误差的近似公式。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）将最小二乘估计法应用于谷神星的轨道跟踪和预测，取得了良好的效果。最小二乘估计以观测残差平方和最小作为估计准则，它不需要关于量测的任何统计信息，算法简单且实用性强，在参数估计领域获得了广泛的应用。但是，通常情况下最小二乘估计只能应用于静态参
7
数估计，而不适用于动态系统的状态估计。
20世纪40年代初期，维纳（Norbert Wiener）开始将统计方法应用于通信系统和控制系统的研究中，提出了著名的维纳滤波理论。同一时期，柯尔莫哥洛夫（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 组合导航
8
将运载体从起始点引导到目的地的技术或方法称为导航，导航系统提供的信息主要有姿态、方位、速度和位置，甚至还包括加速度和角速率，这些信息可用于运载体的正确操纵和控制。随着技术的发展，导航系统的种类越来越多，比如惯导系统、卫星导航系统、磁罗盘、里程仪/多普勒测速仪/空速计、气压高度表/雷达高度表、地标点/地图匹配等。这些导航系统各有特色，优缺点并存，比如惯导系统的优点是自主性强、动态性能好、导航信息全面且输出频率高，但其缺点是误差随时间不断累积，长期精度差；卫星导航系统的优点是精度高、误差不随时间增大，缺点是导航信息不够全面、频带窄、信号容易受到干扰、在室内等环境下接收不到卫星信号而无法使用。在许多对导航性能要求苛刻的任务中，无论是精度要求高还是可靠性要求高，任何单一的导航系统可能都无法满足要求，这就需要使用多种导航系统同时对运载体进行导航信息测量，再对所有测量信息作综合处理（包括检测、结合、相关和估计），从而得到更为准确和可靠的导航结果。这种对多种导航信息作综合处理的技术就称为组合导航技术。从上述对惯导和卫星导航的优缺点描述中可以看出，两者性能具有非常强的互补性，因而惯性/卫星组合导航被公认为是最佳的组合导航方案。
组合导航系统的设计一般都采用Kalman滤波器，Kalman滤波器最早和最成功的应用实例便是在导航领域。1960年卡尔曼在美国国家航空航天局埃姆斯研究中心（NASA Ames Research Center）访问时，Stanley Schmidt发现Kalman滤波方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗登月飞船的导航系统便使用了Kalman滤波器，通常认为Schmidt首次实现了Kalman滤波器。此外，美国在航天飞机、潜艇和无人航空航天飞行器（比如巡航导弹）上均使用了Kalman滤波器。
9
第2章 捷联惯导姿态解算基础
第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
在捷联惯导系统的姿态、速度和位置更新算法中，姿态算法对整个系统精度的影响最大，它是算法研究和设计的核心。在非定轴转动情况下，描述姿态运动的微分方程是非线性的，其离散化求解会引起转动不可交换误差。现代高精度的捷联惯导中，陀螺仪往往采用角增量信号输出方式，利用角增量构造等效旋转矢量以补偿和降低不可交换误差，是目前主流姿态算法的基础。
本章先介绍一些有关于刚体转动或坐标系变换的数学基础知识，之后重点讨论等效旋转矢量微分方程的推导及其离散化求解方法。
2.1反对称阵及其矩阵指数函数
2.1.1 反对称阵
两个三维列向量V1??可利用行列式计算规则?V1xV1yV1z??和V2???V2xV2yV2z??之间的叉乘积，
表示为
?V1yV2z?V1zV2y?ijk??V1?V2?V1xV1yV1z???(V1xV2z?V1zV2x)? （2.1-1）
?V2xV2yV2z??V1xV2y?V1yV2x?另一方面，若计算由向量V1中各元素构造的某种特殊矩阵与向量V2之间的矩阵乘法，可得
TT?0??V1z??V1y??V1z0V1xV1y??V2x??V1yV2z?V1zV2y??????(VV?VV)? （2.1-2） ?V1x??V2y???1x2z1z2x??0??V2z??????V1xV2y?V1yV2x?比较式（2.1-1）与式（2.1-2），容易发现它们的右端结果完全相同，因此，可记式（2.1-2）左端的特殊
10