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

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

讲义

严恭敏，翁浚 编著

西北工业大学 2016-9

1

前 言

近年来，惯性技术不论在军事上、工业上，还是在民用上，特别是消费电子产品领域，都获得了广泛的应用，大到潜艇、舰船、高铁、客机、导弹和人造卫星，小到医疗器械、电动独轮车、小型四旋翼无人机、空中鼠标和手机，都有惯性技术存在甚至大显身手的身影。相应地，惯性技术的研究和开发也获得前所未有的蓬勃发展，越来越多的高校学生、爱好者和工程技术人员加入到惯性技术的研发队伍中来。

惯性技术涉及面广，涵盖元器件技术、测试设备和测试方法、系统集成技术和应用开发技术等方面，囿于篇幅和作者知识面限制，本书主要讨论捷联惯导系统算法方面的有关问题，包括姿态算法基本理论、捷联惯导更新算法与误差分析、组合导航卡尔曼滤波原理、捷联惯导系统的初始对准技术、组合导航系统建模以及算法仿真等内容。希望读者参阅之后能够对捷联惯导算法有个系统而深入的理解，并能快速而有效地将基本算法应用于解决实际问题。

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

西北工业大学自动化学院：梅春波、赵彦明、刘洋、沈彦超、肖迅、牟夏、郑江涛、刘士明、金竹、冯理成、赵雪华；航天科工第九总体设计部：王亚军；辽宁工程技术大学：丁伟；北京腾盛科技有限公司：刘兴华；东南大学：童金武；中国农业大学：包建华；南京航空航天大学：赵宣懿；武汉大学：董翠军；网友：Zoro；山东科技大学：王云鹏。

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

作 者 2016年9月

2

目 录

第1章 概 述 .................................................................................................................. 6

1.1捷联惯导算法简介 ....................................................................................................................... 6

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

3

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

5.1递推最小二乘法 ........................................................................................................................ 92 5.2 Kalman滤波方程的推导 ........................................................................................................... 94 5.3连续时间随机系统的离散化与连续时间Kalman滤波 ............................................................. 101 5.4噪声相关条件下的Kalman滤波 .............................................................................................. 107 5.5序贯滤波 ................................................................................................................................. 111 5.6信息滤波与信息融合 ............................................................................................................... 114 5.7平方根滤波 ............................................................................................................................. 116 5.8遗忘滤波 ................................................................................................................................. 124 5.9 Sage-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.1 Matlab子函数 ........................................................................................................................... 180 7.2.2捷联惯导仿真主程序 ................................................................................................................ 185 7.3惯导/卫星组合导航仿真 .......................................................................................................... 186

7.3.1Matlab子函数 ............................................................................................................................ 186 7.3.2组合导航仿真主程序 ................................................................................................................ 187

附 录 ............................................................................................................................ 190

A一些重要的三维矢量运算关系 .................................................................................................... 190 B 运载体姿态的欧拉角描述 .......................................................................................................... 192 C 姿态更新的毕卡算法、龙格—库塔算法及精确数值解法 ........................................................... 199

4

D 从非直角坐标系到直角坐标系的矩阵变换 ................................................................................. 207 E 线性系统基本理论 ..................................................................................................................... 211 F 加权最小二乘估计 ..................................................................................................................... 216 G 矩阵求逆引理 ............................................................................................................................ 217 H 几种矩阵分解方法（QR、Cholesky与UD） ............................................................................... 219 I 二阶滤波中的引理证明 .............................................................................................................. 223 J 方差阵上界的证明 ..................................................................................................................... 225 K 三阶非奇异方阵的奇异值分解 ................................................................................................... 226 L Matlab仿真程序 ........................................................................................................................ 231 M 练习题 ....................................................................................................................................... 237

参考文献 ....................................................................................................................... 241

5

第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

矩阵如下

?0?(V?)??Vz??Vy?T?Vz0VxVy???Vx? （2.1-3） 0??并将其称为向量V??。引入反对称阵概念后，两向量之间叉乘?VxVyVz??的反对称阵（或斜对称阵）

运算可等价表示为前一向量的反对称阵与后一向量之间的矩阵乘法运算，亦即

V1?V2?(V1?)V2 （2.1-4）

以后会看到，这一简单改写方式在许多场合会带来很大的便利。

如果V是实向量（以后在涉及反对称阵时未特别说明均作此假设），显然有

(V?)H?(V?)T??(V?) （2.1-5）

其中，右上标“H”表示Hermite转置，即共轭转置。

不难验证下式成立：

?Vy2?Vz2?VxVy?VxVz???(V?)H(V?)?(V?)(V?)H???VxVyVx2?Vz2?VyVz? （2.1-6）

??VxVz?VyVzVx2?Vy2???可见，反对称阵(V?)是正规矩阵(Normal Matrix)。根据矩阵理论知，正规矩阵可酉相似于对角阵，且不同特征值对应的特征向量两两正交。下面求解(V?)与对角阵之间的相似变换关系。

首先，计算(V?)的特征多项式

?f(?)?det[?I?(V?)]??VzVy22xVz??Vx?VyVx? （2.1-7）

??(??V)?Vz(??Vz?VxVy)?Vy(VxVz??Vy)??3?(Vx2?Vy2?Vz2)???3??2?其中，??V?Vx2?Vy2?Vz2是向量V的模值。

令特征多项式f(?)?0，可解得(V?)的三个特征值如下

?1?0,?2,3??j? （2.1-8）

??VxVzj?Vy??? （2.1-9） ?VV?j?Vyzx??22?Vx?Vy???当Vx2?Vy2?0时，不难求得与上式三个特征值相对应的单位特征向量，分别为

?Vx?11?,u?u1??Vy2,322????2(V?V)xy??Vz??而当Vx?Vy?0（甚至Vx?Vy?Vz?0）时，可选择单位正交特征向量如下

?0??1??,u?1?j? （2.1-10） u1??02,3??2?????1???0??实际上，反对称阵(V?)的复单位特征向量是不唯一的（见附录I练习题2），式（2.1-9）和（2.1-10）只给出了其中一组。

如记

U??u1u2u3?,

11

Λ?diag(?1?2?3) （2.1-11）

可验证有UHU?I，因此U是酉矩阵。

根据矩阵特征值与特征向量之间的关系，有

(V?)U?UΛ （2.1-12）

上式两边同时左乘U?1，得

Λ?U?1(V?)U （2.1-13）

至此，验证了(V?)可酉相似于对角阵，并求得了相应的相似变换矩阵U。

最后，给出反对称阵的幂方公式，如下 (V?)1??0(V?)

(V?)2?VVT??2I??0(V?)2

(V?)3?(V?)2(V?)?(VVT??2I)(V?)?VVT(V?)??2(V?)?V?01?3??2(V?)???2(V?)

(V?)4?(V?)3(V?)???2(V?)2

24?(V?)5?(V?)2(V?)3?(VVT??2I)???(V?)??(V?) ??2242(V?)6?(V?)3(V?)3?????(V?)??????(V?)????(V?)

综上，可写出通式

?(?1)(i?1)/2?i?1(V?)(V?)??(i?2)/2i?2?(V?)2?(?1)ii=1,3,5， （2.1-14）

i=2,4,6，2.1.2 反对称阵的矩阵指数函数

根据哈密顿—凯莱（Hamilton-Cayley）定理，矩阵指数函数e(V?)可以展开成(V?)的有限项级数形式，即

e(V?)(V?)i??i?0?k0I?k1(V?)?k2(V?)2 （2.1-15）

i!?其中，k0、k1和k2为待定系数。

根据式（2.1-13）和（2.1-15），有

iU?1(V?)U????(V?)????1???i?0?U??i?0?Ui!i!??ieΛ?eU?1(V?)U2?U?1e(V?)U?U?1??k0I?k1(V?)?k2(V?)??U （2.1-16）

?k0U?1U?k1U?1(V?)U?k2U?1(V?)UU?1(V?)U?k0I?k1Λ?k2Λ2上式两边矩阵都展开成元素分量形式，可得

?e?100??k0?k1?1?k2?12????20e00?????0?0e?3?0????? （2.1-17）

k0?k1?2?k2?22?2?0k0?k1?3?k2?3?将特征值式（2.1-8）代入式（2.1-17），比较两边对角线元素，可得如下方程组

?e0?k0?k0?1?j??22 即 e?k?k(j?)?k(j?)?k0?k1(j?)?k2??cos??jsin? （2.1-18） ?012???j?22e?k?k(?j?)?k(?j?)?k0?k1(j?)?k2??cos??jsin?012?从上式可解得待定系数

sin?1?cos? （2.1-19） k0?1,k1?,k2?2000??再将这些待定系数重新代回式（2.1-18），有反对称阵的矩阵函数求解公式

12

e(V?)?I?sin??(V?)?1?cos??2(V?)2 （2.1-20）

实际上，若直接将式（2.1-14）代入式（2.1-15）的求和符号中，亦可求得上式，即

i?(V?)1111(V?)e??i?0?(V?)0?(V?)1?(V?)2?(V?)3?(V?)4?i!1!2!3!4!1111?1??1??(V?)0??(V?)1?(V?)3?(V?)5????(V?)2?(V?)4?(V?)6??3!5!4!6!?1!??2!? （2.1-21）

?1?2?4?(V?)??(V?)?(V?)?(V?)?3!5!?1!sin?1?cos??I?(V?)?(V?)220??1?2?4222???2!(V?)?4!(V?)?6!(V?)????????此外，在式（2.1-16）中有eΛ?U?1e(V?)U，据此可得

?1?2e(V?)U?UeΛ??eueu21?e?3u3?? （2.1-22）

对比上式与式（2.1-12），可知e(V?)与反对称阵(V?)具有相同的特征向量，它们均为矩阵U的列向量；并且矩阵函数e(V?)与对角阵e具有相同的特征值，分别为 ??1??e?1?e0?1???e?2?ej??cos??jsin? （2.1-23） ??2???3?j???e?e?cos??jsin?3?根据以上特征值，易知有(eΛ)HeΛ?I成立，所以e是酉矩阵。由于多个酉矩阵之乘积仍然是酉矩阵，可知e(V?)?UeΛU?1也是酉矩阵；此外，式（2.1-20）表明，若V是实向量则e(V?)是实矩阵，所以e(V?)必定是单位正交阵，这一点亦可证明如下：

TΛΛ??e(V?)??e(V?)sin?1?cos??2???I?(V?)?(V?)?e(V?)2?????sin?1?cos??2T?(V?)????I?(V?)T?(V?) （2.1-24） 2???e????sin?1?cos??2?(V?)??I?(?V?)T?(?V?)e2??????e(?V?)e(V?)?IT值得指出的是，由于det(e(V?))?etr(V?)?e0?1，所以，在所有三阶单位正交阵中只有行列式为1者才可以表示成e(V?)的形式，事实上，行列式为1的单位正交阵可称为右手直角坐标变换矩阵（反之，行列式为-1者可称为左手矩阵）。

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

2.2.1 方向余弦阵

若用ib,jb,kb分别表示直角坐标系oxbybzb（b系）坐标轴上的单位矢量，而用ii,ji,ki表示oxiyizi（i系）坐标轴向的单位矢量，则ib,jb,kb可分别用ii,ji,ki表示为： ?ib?(ib?ii)ii?(ib?ji)ji?(ib?ki)ki??jb?(jb?ii)ii?(jb?ji)ji?(jb?ki)ki （2.2-1） ?k?(k?i)i?(k?j)j?(k?k)kbiibiibii?b实际上，上式表示的正是两直角坐标系之间的基变换公式，将其改写成矩阵的方式，如下

13

?ibjbkb???iiji?ib?iiki???ib?ji??ib?kijb?iijb?jijb?kikb?ii?kb?ji????iikb?ki??jiki?P （2.2-2）

其中，P为从i系到b系的过渡矩阵（或称从i系到b系的坐标系/基变换矩阵），即

?ib?iijb?iikb?ii?? （2.2-3） P???ib?jijb?jikb?ji???ib?kijb?kikb?ki??假设有一个三维矢量V，它在i系和b系下的投影坐标分别为 ?Vxi??Vxb?????Vi??Vyi? 和 Vb??Vyb?

?Vzb??Vzi?????若用投影表示法，则有

V?Vxiii?Vyiji?Vziki?Vxbib?Vybjb?Vzbkb （2.2-4）

而若用坐标表示法，则有

?iiji?Vxi???ki??Vyi???ib?Vzi???jb?Vxb???kb??Vyb? （2.2-5） ?Vzb????Vxb???ki?P?Vyb? （2.2-6）

?Vzb???将式（2.2-2）代入式（2.2-5）的右端，可得

?Vxi?i?V?iijiki???y???ii?Vzi???从而有

ji?Vxi??Vxb?i?i??b? 即 Vi?PVb?CV b （2.2-7） V?PVb?y??y?i?Vz??Vzb?????i其中，记Cb。 ?P为从b系到i系的坐标变换矩阵，也就是从i系到b系的坐标系变换矩阵（或过渡矩阵）

从几何含义上，不难验证过渡矩阵P是单位正交阵（即有PTP?I），比如对于式（2.2-3）中的第一行向量?ib?iijb?iikb?ii?，它表示ii在b系的投影，可记为(ii)b，显然有(ii)b?ii?1，而第一行向

jb?iikb?ii??ib?jijb?jikb?ji??(ii)b?(ji)b?ii?ji?0

量与第二行向量点乘为

?ib?ii同理，可验证P中任一行向量为单位向量，且任意两个不同行向量之间正交。

i由于矩阵Cb?P中的每一个元素均表示两套坐标系（b系和i系）相应坐标轴之间夹角的余弦值，

os(比如ib?ji表示坐标轴oxb与oyi之间夹角的余弦值，即ccosine matrix, DCM）。

?xb)oyi，因此常称Cbi为方向余弦阵（direction

2.2.2 等效旋转矢量

参见图2.2-1，三维空间中的某矢量r绕另一单位矢量u转动?（设??0）角度，得矢量r?，以下求解转动前后两矢量r与r?之间的几何运算关系。

14

r??(u?r)?uO?AA'r??r?u?rr?Br?r??(r?u)uuO图2.2-1 等效旋转矢量

不妨假设矢量r和单位矢量u具有共同的起始点O，记r的矢端A在u上的投影为O?。若以O?为圆心、O?A为半径作圆，则r?的矢端A?也在该圆周上。在圆上取一点B使得O?B?O?A，则有

O?B?u?r （2.2-8）

转动前的矢量r相对于单位矢量u可分解为平行于u的分量r和垂直于u的分量r?，如下

r?OO??O?A 即 r?r?r? （2.2-9） 其中

r?(r?u)u （2.2-10）

r??O?B?u?(u?r)?u （2.2-11）

?，如下 同理，转动后的矢量r?相对于u也可以分解为平行分量r?和垂直分量r? （2.2-12） r??OO??O?A? 即 r??r??r??其中

r??r （2.2-13）

??O?Acos??O?Bsin??(u?r)?ucos??u?rsin? （2.2-14） r?至此，将式（2.2-10）和式（2.2-14）代入式（2.2-12），可详细展开为

r??(r?u)u?(u?r)?ucos??u?rsin? （2.2-15） 此外，由附录A三重矢积公式（A-3），即(V?V3)V?V?(V?V3)??2V3，可得

2(r?u)u?(u?r)u?u?(u?r)?ur???I?(u?)??r （2.2-16）

2将式（2.2-16）代入式（2.2-15），得

22?r???I?(u?)r?(u?)rcos??u?rsin???2???I?sin?(u?)?(1?cos?)(u?)??r （2.2-17）

?Dr其中记

D?I?sin?(u?)?(1?cos?)(u?)2 （2.2-18）

式（2.2-17）称为罗德里格（Rodrigues）旋转公式，它建立了转动前后两矢量r与r?之间的线性变换关系，该变换是转轴u及转角?的函数。

直角坐标系上存在三个坐标轴向单位矢量，也可对它们实施旋转操作。假设有动坐标系b系与参考坐标系i系，两坐标系在起始时刻重合，接着b系相对于i系作定轴转动，即绕通过原点的单位矢量u转动了?角，也就是说，i系坐标轴的单位矢量ii,ji,ki绕u转动?角得到b系坐标轴的单位矢量ib,jb,kb。根据式（2.2-18），可得两坐标轴单位矢量之间的变换关系

?ib?Dii??jb?Dji （2.2-19） ?k?Dki?b

15

再假设

?D11D12D13??1??0??0??,?0?,?1?,?0? （2.2-20） D??DDDi?j?k?212223iii?????????????D31D32D33???0???0???1??将式（2.2-19）和（2.2-20）代入过渡矩阵式（2.2-2），得

?Dii?iiDji?iiDki?ii??D11D12D13????D??D （2.2-21） P??Di?jDj?jDk?jDDiiiiii212223??????Dii?kiDji?kiDki?ki????D31D32D33??这表明，矩阵D正好是从参考坐标系i系到动坐标系b系的过渡矩阵，它也是从b系到i系的坐标变换矩阵，可重新记式（2.2-18）为：

iCb?I?sin?(u?)?(1?cos?)(u?)2 （2.2-22）

进一步，若记???u和???，则有u????，将其代入式（2.2-22），可得

iCb?I?sin??(??)?1?cos??2(??)2 （2.2-23）

这里?称为等效旋转矢量（Rotation Vector），根据图2.2-1，等效旋转矢量的矢量方向表示转轴方向，而模值大小表示旋转角度大小。从转动的物理含义上看，(??2kπ)u(k?0,1,2,)表示的是相同的转动，这可通过将其代入式（2.2-23）进行验证，即Cbi与k的取值无关。如果限定转角的取值范围0???2π，则等效旋转矢量和方向余弦阵之间存在一一对应关系。从坐标系的定轴转动中可以看出，等效旋转矢量

ib（或单位转轴）是一种比较特殊的矢量，它在i系和b系下的坐标值完全相等，即有?i?Cb???b（或

b。有时为了更加明确地显示b系相对于i系的转动关系，可利用下角标进行标注，比如?ibb（或uib）。 ui?ub）

将式（2.2-23）与向量反对称阵的矩阵函数（2.1-20）对比，可看出两者形式上完全一致，这说明式（2.1-20）中三维向量具有等效旋转矢量的物理含义。根据式（2.1-30）和（2.1-31）还可以看出，方向余弦阵的一个特征值恒为1（?1??1），与其对应的单位特征向量（u1）表示转轴方向；方向余弦阵的另

?和?3?）即为等效旋转矢量模值的幂指函数e外两个共轭特征值（?2量的转角大小。

i若将方向余弦阵Cb看作是等效旋转矢量?的函数，可简记为

?j?，特征值的幅角表示等效旋转矢

iCb?MRV(?) （2.2-24）

并且有

iTCib?(Cb)?MRV(??) （2.2-25）

特别地，若分别取?1???100?、?2???010?和?3???001?，则有

2TTT0?000??000??1??(1?cos?)?00?1???0cos?iCb(?1)?MRV(?1)?I?sin??00?1????????010???010????0sin??001??001??cos???(1?cos?)?000???0iCb(?2)?MRV(?2)?I?sin??000?????????100????100?????sin??0?10??0?10??cos???(1?cos?)?100???sin?iCb(?3)?MRV(?3)?I?sin??100?????????000??000????0意转动都可以由三次基本转动合成，参见附录B。

16

2? （2.2-26a） ?sin???cos???0sin?? （2.2-26b） 10??0cos???02?sin?cos?00? （2.2-26c） 0??1??上述三式分别称为以坐标轴为旋转轴的基本转动矩阵，或称Givens矩阵或初等旋转矩阵，空间的任

由等效旋转矢量与方向余弦阵之间的一一对应关系可知，方向余弦阵虽然含有9个元素，但它只有3个独立参数，包含了6个约束条件，即行向量之间两两正交（3个）及每个行向量模值均为1（3个）。3个独立参数即为3个自由度，这与三维空间中的转动自由度是一致的。

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

2.3.1 方向余弦阵微分方程

假设动坐标系（b系）和参考坐标系（i系）具有共同的原点，b系相对于i系转动的角速度为ωib，

i从i系到b系的坐标系变换矩阵记为Cb，它是时变矩阵，再假设在i系中有一固定矢量r，则固定矢量r在两坐标系下投影的转换关系（即坐标变换），为

ibri?Cbr （2.3-1）

将上式两边同时微分，得

ibibri?Cbr?Cbr （2.3-2）

注意到，r是i系中的固定矢量，则有ri?0；由于b系相对于i系的角速度为ωib，则在b系上观察rb的角速度应为?ωib（或写为ωbi??ωib），并且有rb??ωib?rb，因而式（2.3-2）可化为

ibibibib0?Cb(?ωib?rb)?Cbr 即 Cbr?Cb(ωib?)rb （2.3-3）

由于上式对于任意i系固定矢量r都成立，任选三个不共面的非零矢量r1、r2和r3，则有

ibbbibbbb?Cb?r1r2r3???Cb(ωib?)??r1r2r3??

bbb显然矩阵??rrr123??可逆，所以必定有

iibCb?Cb(ωib?) （2.3-4）

这便是方向余弦阵微分方程，或称为姿态阵微分方程，它建立了动坐标系相对于参考坐标系之间方向余弦阵与动坐标系运动角速度之间的关系。

此外，通过如下矢量运算关系

iibibibωib?ri?Cb(ωib?rb)?Cb(ωib?)rb?Cb(ωib?)Cibri

比较上式两边，可得反对称阵的相似变换公式

iib(ωib?)?Cb(ωib?)Cib （2.3-5）

ii?1iT根据式（2.3-4）和式（2.3-5），并考虑到Cb是单位正交阵，即有(Cb)?(Cb)?Cib，容易证明以

下四种方向余弦阵微分方程是相互等价的：

iibCb?Cb(ωib?) （2.3-6a） iii （2.3-6b） Cb?(ωib?)CbbCib?(ωbi?)Cib （2.3-6c） iCib?Cib(ωbi?) （2.3-6d）

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

iib以下讨论微分方程Cb?Cb(ωib?)的求解，为了书写简便，略去各量的上下角标，但明确写出时变量

b的时间参数，并记反对称阵Ω(t)??ωib?(t)???，将姿态阵微分方程表示为

C(t)?C(t)Ω(t) （2.3-7）

显然，这是一个典型的时变系数齐次微分方程，需采用毕卡（Peano-Baker/Picard?）法求解。

首先，对式（2.3-7）在时间段[0,t]上积分，得

17

C(t)?C(0)??C(?)Ω(?)d? （2.3-8）

0t由于上式右边第二项被积函数依然含有待求的C(t)，重复使用上式右边整体代入积分号内，第一次代入，得

t?C(t)?C(0)???C(0)??C(?1)Ω(?1)d?1?Ω(?)d??0?0??tt??C(0)?C(0)Ω(?)d??C(?)Ω(?)d?Ω(?)d? （2.3-9）

?0??00111tt??C(0)?I??Ω(?)d?????C(?1)Ω(?1)d?1Ω(?)d???000??第二次代入，可得

tt?C(t)?C(0)?I??Ω(?)d????Ω(?1)d?1Ω(?)d????000?? （2.3-10）

??t00????10C(?2)Ω(?2)d?2Ω(?1)d?1Ω(?)d?依此不断代入，便可得到以无限重积分表示的所谓的毕卡级数

tt?C(t)?C(0)?I??Ω(?)d????Ω(?1)d?1Ω(?)d??000? （2.3-11）

??才容易得到闭合解。

t00????10Ω(?2)d?2Ω(?1)d?1Ω(?)d?????上述级数是收敛的，但一般情况下得不到闭合形式的解（初等解），只有在所谓的定轴转动特殊情形下

考虑时间段[0,T]，对于任意时间参数t,??[0,T]，假设转动角速度满足如下可交换性条件

Ω(t)Ω(?)?Ω(?)Ω(t) （2.3-12）

则有

?t0Ω(t)Ω(?)d???Ω(?)Ω(t)d? 即 Ω(t)?Ω(?)d???Ω(?)d?Ω(t) （2.3-13）

000ttt现计算以下微分

2d?td?t????tΩ(?)d??Ω(?)d??Ω(?)d??????0???0????0?dt?dt?ttt?? （2.3-14）

?Ω(t)?Ω(?)d???Ω(?)d?Ω(t)?2?Ω(?)d?Ω(t)000注意，上式的最后一个等号是在式（2.3-12）条件下才能成立的。根据式（2.3-14），有如下积分成立

2t?1?t?Ω(?)d? （2.3-15） ?0?0Ω(?1)d?1Ω(?)d??2????0?同理，有

23t?2?1t1?1?t???Ω(?1)d?1Ω(?)d???Ω(?)d? （2.3-16） ?0?0?0Ω(?2)d?2Ω(?1)d?1Ω(?)d???02?????0??0?6?等等。

至此，在可交换条件（2.3-12）下，毕卡级数式（2.3-11）可简化成闭合解形式

23t?1?t1?t???C(t)?C(0)?I??Ω(?)d??Ω(?)d??Ω(?)d?????0?3!???0?0??2!??? （2.3-17）

Ω(?)d??C(0)e?0t下面说明可交换条件式（2.3-12）的几何含义。 设角速度的分量形式为ω(t)????1x?1y?1z??和ω(?)????2x?2y?2z??，则有

???1y?2y??1z?2z??1y?2x?1z?2x?? （2.3-18）

Ω(t)Ω(?)??ω(t)???ω(?)?????1x?2y??1x?2x??1z?2z?1z?2y???1x?2z?1y?2z??1x?2x??1y?2y???18

TT

???1y?2y??1z?2z??2y?1x?2z?1x?? （2.3-19）

Ω(?)Ω(t)??ω(?)???ω(t)?????2x?1y??1x?2x??1z?2z?2z?1y???2x?1z?2y?1z??1x?2x??1y?2y???令上述两式相等，可解得

??1x?2y??2x?1y???1x?2z??2x?1z （2.3-20） ??????2y1z?1y2z如果式（2.3-20）中所有的角速率分量都不为0，则有

?1x?1y?1z （2.3-21） ???2x?2y?2z这表示在时间段[0,T]内，b系相对于i系的转动角速度方向始终不变，即为定轴转动；如果式（2.3-20）中某些角速率分量为0，也容易得出该转动是定轴转动的结论；如果所有角速度分量均为0，即为静止，它亦可视为定轴转动的特殊情形。综合上述三种情况，说明闭合解式（2.3-17）只有在定轴转动情形下才能严格成立。

针对时间段[0,T]，记角增量θ(T)??ω(?)d?且模值?(T)?θ(T)，考虑到矩阵指数函数式（2.1-19），

0T则有

Ω(?)d?sin?(T)1?cos?(T)2e?0?e[θ(T)?]?I??θ(T)???2?θ(T)?? （2.3-22）

?(T)?(T)因此，式（2.3-17）在T时刻的解可简写为

0 （2.3-23） C(T)?C(0)CTT其中

sin?(T)1?cos?(T)2?θ(T)???2?θ(T)?? （2.3-24） ?(T)?(T)i若将时间区间从[0,T]更改为[tm?1,tm]，且假设已知tm?1时刻的方向余弦阵为Cb，[tm?1,tm]时间段(m?1)0CT?I?的角增量为Δθm??tmtm?1ib的公式为 ωib(t)dt且记模值??m?Δθm，则求解tm时刻的姿态阵Cb(m)iib(m?1) （2.3-25） Cb(m)?Cb(m?1)Cb(m)sin??m1?cos??m?I?(Δθm?)?(Δθm?)2 （2.3-26） 2??m??mb(m?1)Cb(m)上述两式便是姿态阵离散化更新的递推计算公式。值得注意的是，式（2.3-26）严格成立的前提条件是b系在时间[tm?1,tm]内必须是定轴转动，该式与式（2.2-23）相比，两者在形式上完全一致，因而可以认为定轴转动时角增量Δθm是以b(tm?1)系为参考，b(tm)系相对于b(tm?1)系转动的等效旋转矢量；否则，如果可交换性条件式（2.3-12）不成立，依然简单地利用式（2.3-26）进行计算将会引起姿态求解的不可交换误差，不可交换性是高维时变系统（时变矩阵微分方程）的普遍特性。

b同理，类似于式（2.3-25）和（2.3-26），可求得另一种姿态阵微分方程表示形式Cib?(ωbi?)Cib的更

新公式，为

b(m)b(m?1) （2.3-27） Cib(m)?Cb(m?1)Ci??sin??m1?cos??m??)???)2?I?(Δθm(Δθm2??)??m(??m （2.3-28）

sin??m1?cos??m?I?(Δθm?)?(Δθm?)22??m(??m)Tb(m)Cb(m?1)其中，Δθm???

mb(m)bb??Δθm?。显然，有Cb?b(m?1)?成立。 ωbi(t)dt???ωib(t)dt??Δθm、??m(m?1)??Cb(m)?tm?1tm?1tmt19

对于非定轴转动下的姿态更新方法，主要是通过等效旋转矢量算法进行不可交换误差补偿，这些内容将在本章后续小节详细介绍。

2.4姿态更新的四元数表示

四元数（quaternion）的概念最早在1843年由数学家哈密顿（W R Hamilton）提出，它可用于描述刚体转动或姿态变换，与方向余弦阵相比，四元数表示方法虽然比较抽象，但却十分的简洁。

2.4.1 四元数的基本概念

顾名思义，四元数就是包含四个元的一种数，可表示为

Q?q0?qv?q0?q1i?q2j?q3k （2.4-1）

其中，q0、q1、q2和q3都是实数，q0称为实部、qv?q1i?q2j?q3k称为虚部。四元数可以看作是复数概念的扩充，有时也称其为超复数，当q2?q3?0时四元数即退化为复数。四元数的虚数单位i,j,k之间满足如下乘法运算规则

?ii?jj?kk??1 （2.4-2a） ??ij?k,jk?i,ki?j,ji??k,kj??i,ik??j即

i2?j2?k2?ijk??1 （哈密顿公式） （2.4-2b） 其中，运算符“”表示四元数乘法运算，在不引起歧义的情况下可写成“?”号或直接省略。式（2.4-2a）中第一行运算规则与复数中虚数的运算规则完全相同；第二行运算规则与三维向量空间中坐标轴单位矢量的叉乘运算规则相同。四元数可以看作是四维空间中的一种数，但因其虚部单位矢量的叉乘运算特点，也可将四元数的虚数部分qv?q1i?q2j?q3k看成是在三维空间中的映象（image），反之，一个三维矢量可以看作是一个零标量四元数。

假设有如下三个四元数

P?p0?pv?p0?p1i?p2j?p3k （2.4-3a） Q?q0?qv?q0?q1i?q2j?q3k （2.4-3b） S?s0?sv?s0?s1i?s2j?s3k （2.4-3c）

两个四元数相等当且仅当它们的四个元分别对应相等，即

?p0?q0?p?q?11P?Q???p2?q2??p3?q3 （2.4-4）

两个四元数之间的加（或减法）定义为

P?Q?(p0?p1i?p2j?p3k)?(q0?q1i?q2j?q3k)?(p0?q0)?(p1?q1)i?(p2?q2)j?(p3?q3)k或者记为

（2.4-5a）

P?Q?(p0?pv)?(q0?qv)?(p0?q0)?(pv?qv) （2.4-5b）

容易验证，四元数的加法满足交换律和结合律，即有P?Q?Q?P和(P?Q)?S?P?(Q?S)。

考虑到运算规则（2.4-2a），两个四元数的乘法结果为

20

PQ?(p0?p1i?p2j?p3k)(q0?q1i?q2j?q3k)?(p0q0?p1q1?p2q2?p3q3)?(p0q1?p1q0?p2q3?p3q2)i （2.4-6）

?(p0q2?p2q0?p3q1?p1q3)j?(p0q3?p3q0?p1q2?p2q1)k特别地，两个零标量四元数相乘，可得

pvqv?(?p1q1?p2q2?p3q3)?(p2q3?p3q2)i?(p3q1?p1q3)j?(p1q2?p2q1)k??pq?pv?qvTvv （2.4-7）

这是零标量四元数乘法运算规则与三维矢量运算规则之间的关系，上式右边同时包含了矢量的点乘运算和叉乘运算。实际上，运算规则式（2.4-2a）可视为式（2.4-7）应用于坐标轴单位矢量的特殊情形。

若采用三维矢量运算表示法，四元数乘法可表示为

PQ?(p0?pv)(q0?qv)?p0q0?p0qv?q0pv?pvqv?(p0q0?pq)?(p0qv?q0pv?pv?qv)Tvv （2.4-8）

在式（2.4-7）中，由于矢量叉乘pv?qv不满足交换律，因而四元数乘法也不满足交换律，即一般情况下PQ?QP；当且仅当pv?qv?qv?pv，即两个四元数的虚部矢量相互平行（包括零矢量）时，才有PQ?QP。容易验证，四元数乘法运算满足结合律(PQ)S?P(QS)，且乘法对加法满足分配律(P?Q)S?PS?QS和S(P?Q)?SP?SQ。可见，四元数乘法运算律与矩阵乘法是完全一致的。

若采用矩阵表示法，四元数乘法式（2.4-6）还可写成

?p0?p1?p2?p3??q0??q0?q1?q2?q3??p0??p?q??q?p?p0?p3p2?q0q3?q2?11?1??????1??M?P （2.4-9a） PQ??MPQ?Q?p2p3?q2?q3q0p0?p1??q2?q1??p2?????????p?pppqqq?qq210??3?210??p3??3?3或者

TTT?p0??q0??q0??p0????pv?qvp0q0?pvqvPQ????????????? （2.4-9b）

qp?pvp0I?(pv?)??v??qvq0I?(qv?)??v??p0qv?q0pv?pv?qv?其中

?p3?Tp2?? （2.4-10a） p0?pv?????p3p0?p1??pvp0I?(pv?)???p2p1p0??q0?q1?q2?q3??q??Tqq?q? （2.4-10b） q0?qv1032????MQ????q2?q3q0q1??qvq0I?(qv?)????q3q2?q1q0?为了简写方便，可定义三维向量的两种四维反对称阵，分别如下

?0?p1?p2?p3??p??T0?pp? （2.4-11a） 0?pv132??(pv?)1?????p2p30?p1??pv(pv?)???p10??p3?p2?0?p1?p2?p3??p??T0p?p? （2.4-11b） 0?pv132??(pv?)2?????p2?p30p1??pv?(pv?)???0??p3p2?p1?p0?pMP??1?p2??p3?p1p0?p2?p3(pv?)1、(pv?)2分别称为第一和第二反对称阵，如果省略右下标“1”和“2”则默认为第一反对称阵。根

21

据上述反对称阵定义，式（2.4-10）可简写为

??q0I?(qv?)2 （2.4-12） MP?p0I?(pv?)1 和 MQ四元数Q的共轭（转置）四元数定义为

Q*?q0?qv?q0?q1i?q2j?q3k （2.4-13）

两个四元数之和（或乘积）的共轭满足如下运算规则

(P?Q)*?P*?Q* （2.4-14a） (PQ)*?Q*P* （2.4-14b）

式（2.4-14a）显然成立；而采用乘法式（2.4-9b）容易验证式（2.4-14b）成立，即

TT??p0??q0?????pvp0q0?pvqv*(PQ)??????? ??ppI?(p?)???0v??qv????(q0pv?p0qv?pv?qv)???vTT?q0??p0???qvp0q0?pvqv**QP??????? ???qvq0I?(qv?)???pv???p0qv?q0pv?pv?qv?四元数Q的模值（2-范数）定义为

*222 （2.4-15） Q?Q*Q?QQ*?q0?q12?q2?q3模值表示四元数在四维空间中的矢量长度。虽然一般情况下PQ?QP，但可以证明总有

PQ?QP?P?Q成立，即

PQ?(PQ)(PQ)*?(PQ)(Q*P*)?P(QQ*)P*?(PP*)(QQ*)?P?Q对于非零四元数，即当Q?0时，有

（2.4-16）

Q*Q22Q?QQ*Q2?1 （2.4-17）

因此，可以定义Q*/Q为非零四元数Q的逆，记作

Q* （2.4-18） Q?2Q?1两个非零四元数之乘积的逆满足运算规则：(PQ)?1?Q?1P?1，验证如下

(PQ)??1(PQ)*PQ2?(Q)*(P)*P2Q2?(Q)*(P)*Q2P2?Q?1P?1 （2.4-19）

该运算规则与两矩阵乘积之逆也完全一致。

?/Q?为四元数的归一化操作，归一化的四元数也称单位四元数，满足??0，则称运算Q?Q如果QQ?1。显然，单位四元数的共轭与其逆相等，即有Q?1?Q*；两个单位四元数之乘积仍然是单位四元

数，即如有P?1且Q?1，则必有PQ?P?Q?1。

类比于复数的三角表示法，四元数也可以表示为三角函数的形式

Q?Q(cos?usin) （2.4-20）

22特别地，当Q?1时，即对于单位四元数，有

Q?q0?qv?cos?usin （2.4-21）

222T其中，q0?cos，qv?usin且q0?qvqv?1；u为单位长度的三维矢量，即uTu?1；?表示某种角度

22值，后面将看到它的含义。

??????通过前面的介绍不难发现，四元数的乘法不满足交换律、共轭及求逆等运算规律与矩阵的相应运算

22

规律几乎完全一致，这似乎暗示着四元数与矩阵之间存在很强的内在联系。以下说明四元数三角表示法的几何意义。

根据方向余弦阵式（2.2-22）进行恒等变形，可得

iCb?I?sin?(u?)?(1?cos?)(u?)2?I?2sincos(u?)?2sin2(u?)2 （2.4-22）

222?I?2cos(sinu?)?2(sinu?)2222其中，?u为等效旋转矢量。将式（2.4-21）的实部q0和虚部qv代入式（2.4-22），可得

iCb?I?2q0(qv?)?2(qv?)2???????0?q3q2??0?q3q2???2?q??I?2q0?q0?q0?q3131??????0?0???q2q1???q2q1?22?1?2(q2?q3)2(q1q2?q0q3)2(q1q3?q0q2)???2??2(q1q2?q0q3)1?2(q12?q3)2(q2q3?q0q1)?2??2(q1q3?q0q2)2(q2q3?q0q1)1?2(q12?q2)??222?q0?q12?q2?q3???2(q1q2?q0q3)?2(q1q3?q0q2)?2 （2.4-23）

2(q1q2?q0q3)222q0?q12?q2?q32(q2q3?q0q1)2(q1q3?q0q2)??2(q2q3?q0q1)?222?q0?q12?q2?q3?上式建立了单位四元数与方向余弦阵之间的关系，并且表明了单位四元数三角表示法式（2.4-21）的几

i何意义。为了更明确地表示两坐标系之间的转动变换关系，常在四元数的右边加上上下角标，写成Qb，

则式（2.4-21）中u表示动坐标系（b系）相对于参考坐标系（i系）旋转的单位转轴、?表示旋转角度

i*iT大小。使用角标后，共轭四元数可记为Qib?(Qb)，这与矩阵转置的表示方法类似，比如Cib?(Cb)。

2.4.2 四元数微分方程

假设有一个三维矢量r，它在动坐标系（b系）和参考坐标系（i系）中的投影坐标分别为

brb???rxrybiirzb??和r???rxTryiiQbrbQib?MQi(rbbb，现对矢量（视为零标量四元数）实施如下四元数乘法操作 rrzi???0??0??b?b?)?MQiMQ?b?b?Qib)?MQi(MQbibi?r??r?T?q0?q??1?q2??q3?q1q0q3?q2?q2?q3q0q1?q3??q0??qq2???1?q1???q2??q0???q3q1q0q3?q2q2?q3q0q1q3??0??b?q2???rx??q1??ryb????q0??rzb?0??0??b?2(q1q3?q0q2)???rx?2(q2q3?q0q1)??ryb?222??b?q0?q12?q2?q3??rz? （2.4-24）

0?1?0q2?q2?q2?q20123???02(q1q2?q0q3)??02(q1q3?q0q2)02(q1q2?q0q3)222q0?q12?q2?q32(q2q3?q0q1)不难发现，上式右边矩阵中的右下角三阶对角分块矩阵恰好与式（2.4-23）一致，因而上式可简写为

?10??0??0??0?iQbrbQib????ib???i? （2.4-25） i??b?0Cb??r???Cbr??r?ibi这表明，Qbr。为了rbQib的结果也是一个零标量四元数，其虚部正好对应于姿态阵坐标变换ri?Cb书写简洁，类似于矩阵的坐标变换表达习惯，可定义四元数与三维矢量的乘法运算，即四元数坐标变换公式，如下

23

iri?Qb?rb （2.4-26）

i式中，乘法算符“?”的含义本质上是先进行四元数乘法运算QbrbQib，再提取结果中的虚部（即矢量

部分）。

i由式（2.4-25）两边同时右乘Qb，可得

ii （2.4-27） Qbrb?riQbb假设矢量r是i系中的固定矢量，即ri为常值矢量，并假设b系绕i系转动角速度为ωib，则Qbi和rb都是b时变量。在b系中观察矢量r，其相对于b系的角速度为?ωib。将式（2.4-27）两边同时微分，考虑到ri?0，

可得

iii （2.4-28） Qbrb?Qbrb?riQbbbb由于ωib，可得 ?rb??ωibrb，将其及式（2.4-25）一起代入式（2.4-28）?rb，从而有rb??ωibiibii （2.4-29） Qbrb?Qb(ωibrb)?(QbrbQib)Qb再将上式两边同时左乘Qib，移项得

iib(QibQb)rb?rb(QibQb)?ωibrb （2.4-30）

上式写成矩阵形式，为

0?0??0??00??0??0? 即 （2.4-31） ?????bi?b??rb?b?b????r??0(ωib?)??r?????02?(QiQb)v????由于r可为任意固定矢量，类似于式（2.3-3），有

1bbibbi 即 ?2?(QQ)??(ω?)(QQ)?ωib （2.4-32） bv?ibibv?i2i另一方面，四元数Qb及其微分可分别写成

?Mb?(Qi?0????M?MωbiiQb)(QibQb)??b?ibr?????????cos?sin????222ii 和 ?? （2.4-33） Qb??Qb???b????ubsin??b?usin?ucosibibib????2??222??i根据上述表达式，直接计算QibQb，得 Qib???cos?2?iQb??????ubsin?ib??2???????sin??22???b???b?usin?ucosibib??222????????Tb??bb??sincos?(usin)(usin?ucos)ibibib??（2.4-34）

2222222?????b?????????b?bbbb?cos(usin?ucos)?usin?sin?(usin)?(usin?ucos)ibibibibibib??22222222222??0??0???1???b?bbbb?ucos?sin??ub??ubsin??ubsin??2?u??usin??u?u(1?cos?)ibibibib??ibibibib??22222??i由上式可见，QibQb的标量部分恒为零，因此，由式（2.4-32）可得

iQibQb?1?0? 即 i1ib （2.4-35） Qb?Qbωib?b?22?ωib?这便是四元数微分方程，它建立了变换四元数与坐标系旋转角速度之间的关系。与矩阵微分方程式（2.3-6）类似，容易证明以下四种四元数微分方程之间是相互等价的

1i1i1bibiiQb?Qbωib,Qb?ωibQb,Qib?ωbiQib,222

24

1i Qib?Qibωib2最后，比较式（2.4-32）和式（2.4-34）右端的矢量部分，可得

bbbbbωib?uib??uibsin??uib?uib(1?cos?) （2.4-36）

这是推导等效旋转矢量微分方程的基本公式，将在后续2.5.1小节进一步介绍。实际上，根据方向余弦

iiibb阵微分方程Cb用等效旋转矢量?uib展开，也可推得上式?Cb(ωib?)，并类比于式（2.4-34）将CibCb（2.4-36）。

2.4.3 四元数微分方程的求解

将四元数微分方程（2.4-35）写成矩阵形式，为

1?(t)Q(t) （2.4-37） Q(t)?Mω2为表示简洁，这里暂且省略Q和ω角标，但明确给出了时间参数。如果角速度ω(t)（即系数矩阵

?(t)）是时变的，类似于方向余弦阵微分方程（2.3-7）的求解，只有在时间段t,??[0,T]内满足定轴转Mω动条件?ω(t)???ω(?)????ω(?)???ω(t)??时，等价于

?(t)Mω?(?)?Mω?(?)Mω?(t) （2.4-38） Mω才能求得闭合解

Q(T)?e其中

1Θ(T)2Q(0) （2.4-39）

??x(T)??y(T)??z(T)??0??(T)?0?(T)??(T)Txzy???θ(T)?? （2.4-40） ?(t)dt??Θ(T)??Mω20??y(T)??z(T)0?x(T)????(T)?(T)??(T)0??yx?z?θ(T)????x(T)?y(T)?z(T)????ω(t)dt （2.4-41）

0TTθ(T)为时间段[0,T]内的角增量，?(T)?θ(T)是其模值。

为了计算式（2.4-39）中的指数函数e和?(T)分别简记为Θ和?）

1Θ(T)2，先求解反对称阵Θ(T)的各次幂，有（省略时间参数，Θ(T)Θ2???2IΘ3?Θ2Θ???2ΘΘ4?Θ3Θ???2ΘΘ??4I Θ5?Θ4Θ??4Θ所以，有

25

e1Θ(T)2?Θ(T)??Θ(T)??2?2??Θ(T)????????I?????2!3!?2?2223?Θ(T)??2?????n!44n??(T)???(T)?Θ(T)??(T)???(T)?Θ(T)II???2?2?22?2??Θ(T)??????????2??I??????2!3!4!5!?2??35???(T)?2??(T)?4????(T)???(T)?????????2??Θ(T)???(T)??2?2?2??????????I?1??????????2!4!3!5!???(T)??2????????(T)Θ(T)?(T)?Icos?sin2?(T)2将式（2.4-42）代入式（2.4-39），可得

??(T)Θ(T)?(T)?Q(T)??Icos?sin?Q(0)2?(T)2?? （2.4-42）

????????(T)? （2.4-43） ?cos????(T)??θ(T)?(T)???2???Icos???sin????Q(0)?Q(0)?θ(T)?(T)2?(T)2????2??????sin?2???(T)?若将研究时间区间从[0,T]改为[tm?1,tm]，则根据上式有

iib(m?1) （2.4-44） Qb(m)?Qb(m?1)Qb(m)??m??cos??2b(m?1)? （2.4-45） Qb(m)??Δθ???msinm??2????m?iib(m?1)其中，Qb、Qb分别表示tm?1和tm时刻的姿态变换四元数，Qb是从tm?1时刻到tm时刻的姿态四(m?1)(m)(m)b元数变化，且有Δθm??ωibdt和??m?Δθm。式（2.4-44）和式（2.4-45）便是姿态更新的四元数递

tm?1tm推计算公式，但应当注意到，这是在假设b系在时间段[tm?1,tm]内为定轴转动时才能严格成立的。

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

方向余弦阵更新算法式（2.3-25）和四元数更新算法式（2.4-44）两种算法完全等价，只是后者计算量稍小一点而已。两种算法都是假设在更新周期内动坐标系作定轴转动才能严格成立，如果不是定轴转动，由角增量直接求解变化矩阵或四元数，容易引入转动不可交换误差。实际捷联惯导系统中的陀螺测量信号输出为角增量形式，比如激光陀螺，或者信号输出为角速率形式，但是为了降低噪声，常常采用了高频采样再滤波平滑处理后降频输出，这种方式也接近于增量输出方式，而非瞬时角速率输出。为了减小不可交换误差的影响，研究者们提出了先通过角增量求解等效旋转矢量、再利用等效旋转矢量更新方向余弦阵或四元数的方法。

2.5.1 等效旋转矢量微分方程

对于式（2.4-36），为书写简洁暂且省略角标，重写为

ω?u??usin??u?u(1?cos?) （2.5-1）

根据附录A式（A-6）～（A-8），有

26

u????(???)??? ??(???)，

， ??u?u?u???2?2?3将它们代入式（2.5-1），得

???(???)???(???)???ω?????sin??(1?cos?)?232????? （2.5-2）

????sin????(???)???(1?cos?)2??1?????2??上式移项，得

??ω?(1?cos?)????sin????(???) （2.5-3）

?1??2????2??这是旋转矢量微分方程的一种表示形式，但稍显不足的是等式右边依然含有微分项?，不利于实际使用。下面根据反对称阵的幂方特性，由式（2.5-2）求解?。

若记

a?则式（2.5-2）可简写为

(1?cos?)?2?sin??2 （2.5-4） ,b??1??/????ω???a????b(??)2? （2.5-5a）

使用?左叉乘上式的两边，可得

??ω?????a(??)2??b(??)3??????a(??)2??b?2??? （2.5-5b） ?(1?b?2)????a(??)2?再次使用?左叉乘上式的两边，可得

(??)2ω?(1?b?2)(??)2??a(??)3???a?????(1?b?)(??)?将上述三式合并在一起写成矩阵形式，有

222 （2.5-5c）

ab?????ω??1?01?b?2??????????ω? （2.5-6） a??????22?2?2?1?b???0?a???(??)????(??)ω??若将式（2.5-6）视为关于?、???和(??)2?的三元线性方程组，则不难求解得 ??ω?a????b(??)2??ω?a?(1?b?2)??ω?a(??)2ω?2222??(1?b?)?a?b?a?2??ω?(1?b?2)(??)2ω??2222??a??(1?b?) （2.5-7）

aa2?b(1?b?2)?ω???ω?(??)2ω22222222(1?b?)?a?(1?b?)?a?最后，重新将式（2.5-4）中的a和b表达式代入上式，得 1(1?cos?)2?(??sin?)sin?2??ω???ω?(??)ω22?2(1?cos?)12(1?cos?)??sin??ω???ω?(??)2ω222?(1?cos?)11?ω???ω?22?

（2.5-8）

??sin??21??2(1?cos?)?(??)ω??27

应用三角恒等式，上式还可等价于

??????2sincos11?22?(??)2ω??ω???ω?2?1??2??2? （2.5-9） ?2?2sin?2?11?????ω???ω?2?1?cot?(??)2ω2??22?这便是常见的等效旋转矢量微分方程，它是利用等效旋转矢量进行转动不可交换误差补偿的数学理论基础，该式最早于1971年由学者J E Bortz推导提出，后来通常称之为Bortz方程。

bii至此，可总结坐标系相对转动的四种数学描述，即角速度ωib、姿态阵Cb、四元数Qb和等效旋转矢

量?ibb，之间的关系，如图2.5-1所示。更多姿态描述之间的相互转换关系参见附录B。

b ωib ?ibbCbiQbi

图2.5-1 转动的四种数学描述之间的关系

Bortz方程（2.5-9）虽然在理论上是严格成立的，但实际应用时略显繁杂。当转动角度???为小量时，常常将方程右边三角函数cot(?/2)用泰勒级数展开，进行如下近似

??11???21???ω???ω?2?1???????(??)2ω2??2??32?? （2.5-10） 11?ω???ω?(??)2ω212如果再忽略上式右端三阶小量的影响，还可进一步近似为

1??ω???ω （2.5-11）

2对上式两边同时在时间段[tm?1,t]内积分，为了表述更加清晰，各符号标明时间变量参数，可得

tt11t?(t)??(tm?1)??ω(?)??(?)?ω(?)d???ω(?)d????(?)?ω(?)d?tm?1tm?122tm?1 （2.5-12）

1t?Δθ(t)???(?)?ω(?)d?2tm?1即

1t?(t)??(tm?1)?Δθ(t)???(?)?ω(?)d? （2.5-13）

2tm?1其中，Δθ(t)??ω(?)?d?表示从tm?1时刻开始由角速度累积的角增量且显然有Δθ(tm?1)?0。

tm?1t将式（2.5-13）右端整体再次代入其第三项的积分号内，可得

1t?1???(t)??(tm?1)?Δθ(t)????(tm?1)?Δθ(?)???(?1)?ω(?1)d?1??ω(?)d?2tm?1?2tm?1?11t （2.5-14） ??(tm?1)?Δθ(t)??(tm?1)?Δθ(t)??Δθ(?)?ω(?)d?22tm?11t?????(?1)?ω(?1)d?1?ω(?)d?4tm?1tm?1在时间段[tm?1,?]内，如果?(?1)是小量，式（2.5-14）右边的第5项远小于第4项，即有

28

??t?tm?1tm?1?(?1)?ω(?1)d?1?ω(?)d???12??t?tm?1tm?1ω(?1)d?1?ω(?)d???ttm?1Δθ(?)?ω(?)d? （2.5-15）

因而，式（2.5-14）可近似为

?(t)??(tm?1)?Δθ(t)??(tm?1)?Δθ(t)?1tΔθ(?)?ω(?)d? （2.5-16） ?tm?12特别地，若假设在tm?1时刻的等效旋转矢量?(tm?1)?0，则?(t)可表示从tm?1时刻开始的等效旋转矢量“增量”，式（2.5-16）可简化为

?(t)?Δθ(t)?其中，记

1tΔθ(?)?ω(?)d? （2.5-17） 2?tm?1?Δθ(t)?σ(t)σ(t)?1tΔθ(?)?ω(?)d? （2.5-18） ?tm?12表示等效旋转矢量增量?(t)与角增量Δθ(t)之间的差异，通常称为转动不可交换误差的修正量。

对式（2.5-17）两边同时求导，可得

?(t)?ω(t)?Δθ(t)?ω(t) （2.5-19）

上式可以看作是等效旋转矢量微分方程（2.5-11）的再近一步近似。需要特别指出的是，上式成立的前提条件是：?(tm?1)?Δθ(tm?1)??且需保证?(t)为小量，?(t)越小近似精度越高。

此外，在式（2.5-16）中，若令t?tm且设?(tm?1)?0，则有 1?(tm)??(tm?1)?Δθ(tm,tm?1)??(tm?1)?Δθ(tm,tm?1)?σ(tm,tm?1) （2.5-20）

2其中，?(tm?1)和?(tm)分别表示上一时刻（tm?1时刻）和当前时刻（tm时刻）的等效旋转矢量；将Δθ(tm)、

12σ(tm)分别更明确地记为Δθ(tm,tm?1)和σ(tm,tm?1)，表示从上一时刻至当前时刻的角增量和修正量。式（2.5-20）可视为等效旋转矢量递推的近似计算公式，运算简单且计算量小，但是在实际算法中并不常用，究其原因，主要是随着递推步数的增加和?(tm)变大，误差会不断积累。实际应用时，一般总是假设

?(tm?1)?0，再根据式（2.5-17）计算等效旋转矢量?(tm)，相当于只递推计算一步，这样有利于保证等

效旋转矢量始终为小量，降低公式推导过程中的近似误差。在获得?(tm)之后，改等效旋转矢量递推计算为方向余弦阵或四元数递推，即改用方向余弦阵或四元数完成姿态递推更新，以四元数为例（方向余弦阵类似），等效旋转矢量与四元数相配合的姿态更新算法如下

iib(m?1) （2.5-21） Qb(m)?Qb(m?1)Qb(m)b(m?1)Qb(m)?m??cos?2? （2.5-22）

??????msinm??2???m?其中，将?(tm)简记为?m，且有?m??m。注意，比较式（2.4-45）与式（2.5-22），两者虽然在形式上完全一样，但本质含义上存在重要区别：前者仅简单地使用角增量进行变化四元数计算，理论上只能适用于定轴转动情形；而后者在求解等效旋转矢量过程中考虑了转动不可交换误差的补偿。

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

在实际应用中，从高精度捷联惯导陀螺中采样获得的往往是在一定采样间隔内的角增量信息，下文的主要目的就是借助式（2.5-19）由采样角增量求解等效旋转矢量。

针对算法式（2.5-21）和（2.5-22），不妨将时刻tm?1重新记为0时刻，陀螺在姿态四元数更新时间段[tm?1,tm]（即[0,T]，T?tm?tm?1）内可进行若干次等间隔角增量采样，暂且假设陀螺角速度输出为线

29

性形式

ω(?)?a?2b? 0???T （2.5-23）

则陀螺输出角增量为

Δθ(?)??ω(??)d???a??b?2 （2.5-24）

0?其中，a和b均为常数向量。

现计算角速度ω(0)和角增量Δθ(0)，以及它们的各阶导数，得

?ω(0)?a? （2.5-25） ?ω(0)?2b?ω(i)(0)?0i?2,3,4,??Δθ(0)?0??Δθ(0)?ω(0)?a （2.5-26） ??Δθ(0)?ω(0)?2b?Δθ(i)(0)?ω(i?1)(0)?0i?3,4,5,?再记

β(?)?Δθ(?)?ω(?) （2.5-27）

根据如下求导规则

0(n)1(n?1)(1)2(n?2)(2)(xy)(n)?Cnxy?Cnxy?Cnxy?n?Cnxy(n) （2.5-28）

求β(0)及其各阶导数，可得

?β(0)?0?01?β(0)?C1Δθ(0)?ω(tk)?C1?0?a?a?0 （2.5-29） ?012?β(0)?C2Δθ(0)?ω(tk)?C2Δθ(0)?ω(tk)?C2?0?2b?a?2?a?2b?2a?b?β(i)(0)?0i?3,4,5,?根据等效旋转矢量微分方程式（2.5-19），可计算得?(0)的各阶导数，如下

1??(0)?ω(0)?β(0)?ω(0)?a?2???(0)?ω(0)?1β(0)?ω(0)?2b? （2.5-30） 2??11??(0)?ω(0)?β(0)?β(0)?a?b22?(i)???(0)?0i?4,5,6,若将?(t)视为光滑函数且在t?0处展开成泰勒级数，并将式（2.5-30）代入，可得

T2T3?(T)??(0)?T?(0)??(0)??(0)?2!3!T32?0?Ta?Tb?a?b6T32?Ta?Tb?a?b6 （2.5-31）

式（2.5-31）中包含两个未知向量参数a和b，为了消去a和b并求解出?(T)，需在采样时间段[0,T]内进行两次角增量采样，记为

T/2?TT22T/2Δθ?ω(?)d??a??b??a?b?0?1?024 （2.5-32） ?2?Δθ?Tω(?)d??a??b?2T?Ta?3Tb2?T/2T/2??24由上式可求得以角增量表示的常数向量a?(3Δθ1?Δθ2)/T和b?2(Δθ2?Δθ1)/T2，再将其代入式

30

（2.5-31）便可求得以角增量表示的等效旋转矢量二子样算法

2?(T)?(Δθ1?Δθ2)?Δθ1?Δθ2 （2.5-33）

3类似于前述二子样算法的推导思路，若设陀螺角速度输出为如下抛物线形式

ω(?)?a?2b??3c?2 0???T

并且在时间段[0,T]内进行三次角增量采样，分别记为

Δθ1??T/30ω(?)d?,Δθ2??2T/3T/3ω(?)d?,Δθ3??T2T/3ω(?)d?

则可求得等效旋转矢量三子样算法（过程不复杂但稍显繁琐，从略）

3357?(T)?(Δθ1?Δθ2?Δθ3)?Δθ1?Δθ3?(Δθ1?Δθ2?Δθ2?Δθ3) （2.5-34）

8080值得说明的是，基于泰勒级数展开的等效旋转矢量多子样算法是在式（2.5-19）的基础上推导的，式（2.5-19）又是式（2.5-9）在一定的近似条件下获得的；此外，高阶泰勒级数展开原则上要求函数足够光滑，而实际陀螺输出总会或多或少包含电气噪声，噪声并不反映载体的真实角运动，同时对角速度函数的光滑性也造成不良影响。因此，多子样算法的精度有限，并非子样数越多算法的实用精度就越高。

显然，若假设陀螺角速度输出为常值形式，此即简单的单轴旋转情形，则有等效旋转矢量单子样算法

?(T)?Δθ1??ω(?)d? （2.5-35）

0T特别地，还有一种称为“单子样+前一周期”的等效旋转矢量算法，它假设角速度输出为如下线性形式 ω(?)?a?2b? ?T???T 在时间段[0,T]内仅进行一次角增量采样，记Δθ1??ω(?)d?，但该算法在计算等效旋转矢量?(T)是还

0T会充分利用前一次的角增量信息，记Δθ0??ω(?)d?，通过如下方程组

?T0?T32??(T)?Ta?Tb?6a?b?T?2TΔθ?ω(?)d??a??b??Ta?T2b （2.5-36） ?1?00?00?Δθ0?ω(?)d??a??b?2?Ta?T2b??T?T??消去中间参量a和b，可求解得

?(T)?Δθ1?1Δθ0?Δθ1 （2.5-37） 12与单子样算法（2.5-35）相比，“单子样+前一周期”算法在陀螺采样频率相同的情况下提高了不可交换误差补偿精度；在相同采用频率下，“单子样+前一周期”算法与二子样算法（2.5-33）的精度量级相当，但前者提高了姿态输出频率。

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

2.6.1 圆锥运动的描述

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

ω(t)??aΩsinΩtbΩcosΩtc? （2.6-1）

31

T其中，a、b和c均为常数，在x和y轴表现为同频但相位差90°的正弦角振动，振动频率为Ω，而在

z轴上表现为常值角速率。虽然输入轴z轴有角速率输入，但从长时间来看陀螺仪整体上并不绕着输入

轴产生明显偏转，这就是圆锥运动的神奇之处，曾颇令研究者们费解。

下面采用四元数描述来研究圆锥运动。假设动坐标系（b系）相对于参考坐标系（i系）的变换四元数为

?cos(?/2)??sin(?/2)cosΩt?? （2.6-2） Q(t)???sin(?/2)sinΩt???0??i式中，角度值?和频率Ω均为常值，为书写简便省略角标，Q(t)应理解为Qb(t)。对上式两边同时微分，

得

0???0???Ωsin(?/2)sinΩt???sinΩt????Ωsin??? （2.6-3） Q(t)???Ωsin(?/2)cosΩt?2?cosΩt?????00????根据四元数微分方程（2.4-35），可得角速度的四元数表示如下

?(t)Q*(t)ωq(t)?2Q*(t)Q(t)?2MQsinΩt?0?0???sinΩt?2Ωsin?02?cosΩt?cosΩt?0所以有角速度

?cosΩt00sinΩt0??cos(?/2)0?????sin(?/2)cosΩt???Ωsin?sinΩt?（2.6-4） ?cosΩt????????sinΩt???sin(?/2)sinΩt??Ωsin?cosΩt??????20??0???2Ωsin(?/2)???Ωsin?sinΩt???sinΩt???Ωsin??cosΩt? （2.6-5） ω(t)??Ωsin?cosΩt????2????2Ωsin(?/2)????tan(?/2)??这恰好与式（2.6-1）的角运动表现形式一致，可取a??sin?、b?sin?和c??2sin2(?/2)。

??(t)?，并对比式（2.6-2）

根据四元数与旋转矢量之间的关系Q(t)?cos?，可得 sin2?2?cosΩt?? （2.6-6） ?(t)???sinΩt????0??这说明在任意t时刻，动坐标系绕oxbyb平面上的转轴u(t)??cosΩtsinΩt0?相对于参考坐标系转动了?角度，转轴时刻在变化而转角恒定不变，动坐标系的zb轴在空间画出一个圆锥面（半锥角为?，z轴称为锥轴），参见图2.6-1，这正是该角运动称为圆锥运动的原因。

T

图2.6-1 圆锥运动

式（2.6-2）、（2.6-5）和（2.6-6）分别是圆锥运动的四元数、角速度和等效旋转矢量描述，它们形

32

式上都比较简单，这是除定轴转动之外的比较简单的角运动解析描述。与之相反，比如线性角速度运动式（2.5-23），其角速度表示虽然简单，但是很难得到相应的简单的等效旋转矢量或四元数描述。

不难验证圆锥运动的角速度ω(t)和等效旋转矢量?(t)满足Bortz方程，过程如下。 由式（2.6-5）和（2.6-6），可得

?cosΩt????sinΩt????sinΩttan(?/2)????Ωsin??cosΩt????Ωsin??cosΩttan(?/2)? （2.6-7） ??ω???sinΩt??????????????1?0?????tan(?/2)??????cosΩt????sinΩttan(?/2)???sinΩt?????Ωsin??cosΩttan(?/2)????2Ωsin???cosΩt????2ω （2.6-8） ??(??ω)???sinΩt??????????????1?0???????tan(?/2)??再将式（2.6-7）和（2.6-8）代入Bortz方程式（2.5-9）的右端，得

11????1????ω???ω?2?1?cot?(??2ω)???ω?cotω2??22?222??sinΩttan(?/2)???sinΩt?1???cot??Ωsin??cosΩt? （2.6-9） ??Ωsin??cosΩttan(?/2)??2??22???1????tan(?/2)????sinΩt??tan(?/2)?cot(?/2)?sin????sinΩt?1?????Ω?cosΩt??tan(?/2)?cot(?/2)?sin????Ω?cosΩt??2???0?0????上式正好等于式（2.6-6）直接微分的结果，验证完毕。

2.6.2 圆锥误差补偿算法

若记圆锥运动的四元数更新方程为

Q(tm)?Q(tm?1)Q(T) （2.6-10）

其中，T?tm?tm?1表示更新周期，Q(T)为该周期内的变化四元数（四元数增量）。上式两边同时左乘

Q*(tm?1)，可得

Q(T)?Q*(tm?1)Q(tm)?MQ*(tm?1)Q(tm)??????cossincosΩtsinsinΩt0?m?1m?1???cos222????2? ????sin?cosΩt???cos0?sinsinΩtm?1m?1???sincosΩtm?222?????2???????sinsinΩtm?10cossincosΩtm?1???222??sinsinΩtm?2????????00sinsinΩtm?1?sincosΩtm?1cos??????222? 33

?2??2?2?cos?sincosΩtcosΩt?sinsinΩtsinΩtm?1mm?1m??222?????????sincoscosΩtm?1?sincoscosΩtm??2222??????????sincossinΩtm?1?sincossinΩtm2222????2?2?sinsinΩtm?1cosΩtm?sincosΩtm?1sinΩtm???22?2???ΩT??????22?1?2?sincos???cos2?sin2cosΩT??22????????1sin??(cosΩt?cosΩt)???sin?sinΩTsinΩ?t?T??mm?1?m??2??22??????????1sin??(sinΩtm?sinΩtm?1)??sin?sinΩTcosΩ?t?T???m??2??22?????????sin2sinΩT???2???sinsinΩT?2??2? （2.6-11）

假设与变化四元数Q(T)对应的在时间段[tm?1,tm]内变化的等效旋转矢量为?(T)，即

Q(T)?cos?(T)?(T)?(T) （2.6-12）

?sin2?(T)2其中，?(T)??(T)为模值。比较式（2.6-11）和（2.6-12）的四元数矢量部分，可得

?ΩTT????sin?sinsinΩt??m??22??????(T)?(T)?ΩTT???sin??sin?sincosΩ?tm??? （2.6-13） ?(T)222????????sin2sinΩT??2??对上式两边同时取模，得

2当半锥角?和ΩT均为小量时，近似有

sin?(T)?sin2?sin2ΩT??sin4sin2ΩT （2.6-14） 222这说明等效旋转矢量?(T)也是小量，进一步近似有

sin?(T)?sin2?sin2ΩT?ΩT （2.6-15） ?22?(T)??ΩT （2.6-16）

因此，根据式（2.6-13）可知，在时间段[tm?1,tm]内的等效旋转矢量（旋转矢量增量）可近似为

?ΩTT???ΩTT?????sin?sinsinΩt??2sin?sinsinΩt??m??m??22???22?????????(T)?ΩTT???ΩTT?? （2.6-17） ???(T)?sin?sincosΩt??2sin?sincosΩt??m????m???(T)?2222????????sin2???????sin2sinΩT?2sin2sinΩT????22????再根据角速度式（2.6-5）积分，可得在等效旋转矢量计算时间段[tm?1,tm]内的角增量

34

??Ωsin?sinΩt??sin??(cosΩtm?cosΩtm?1)??dt??sin??(sinΩt?sinΩt)?Δθm??ω(t)dt???Ωsin?cosΩtmm?1???tm?1tm?1?22????2sin(?/2)?ΩT??2Ωsin(?/2)????tmtm?ΩTT????2sin?sinsinΩt??m??22??????ΩTT?????2sin?sincosΩ?tm???22??????2??2sin?ΩT??2?? （2.6-18）

比较式（2.6-17）和（2.6-18），它们在x轴和y轴上完全相同，而z轴上存在差异，这一差异使得

使用角增量代替旋转矢量进行姿态更新时会产生误差，并且误差随时间会不断累积。考虑到半锥角?为小量，定义如下误差

??(T)??(T)?Δθm???????? （2.6-19） 00??????00????????2?2?2???2sinsinΩT?(?2sin?ΩT)??2sin(ΩT?sinΩT)??22??2?为了补偿该误差，通常采用多子样补偿算法，在时间段[tm?1,tm]内进行N次采样，记采样间隔为h?T/N，参照式（2.6-18），可计算得每个采样间隔的内角增量（子样，sub-sample），为

??h????2sin?sinsinΩt?ih??m?1??22?????tm?1?ih??h???, （2.6-20） Δθm(i)??ω(t)dt??2sin?sincosΩ?tm?1?ih??? i?1,2,Ntm?1?(i?1)h22??????2??2sin????2??其中简记??Ωh，显然有时间段[tm?1,tm]内的总角增量

Δθm??tmtm?1ω(t)dt??i?1Δθm(i) （2.6-21）

N将式（2.6-20）中不同子样的角增量之间进行叉乘，可得

??h????h?????2sin?sinsinΩt?ih??2sin?sinsinΩt?jh?m?1m?1?????22???22????????????h??h????Δθm(i)?Δθm(j)??2sin?sincosΩ?tm?1?ih?????2sin?sincosΩ?tm?1?jh???

22???22?????????2?2??2?sin?2?sin????22???????h?h?????2??4?sinsin?sin?cosΩt?ih??cosΩt?jh???m?1??m?1????2222??????????h?h??? ?? ?2????4?sinsin?sin??sinΩ?tm?1?ih???sinΩ?tm?1?jh????22?2?2????????2???????2sin?sin?sin(i?j)???2???? 35

??(i?j)?i?j?1???2?8?sinsin?sinsinsinΩt?h???m?1?2222???????(i?j)?i?j?1?? ????8?sin2sin?sinsincosΩ?tm?1?h??2222????????4sin2?sin2sin(i?j)???2??由于假设?和?都是小量，对上式的x和y轴分量作近似，有

?(i?j)(??)3i?j?1???sinΩt?h???m?1?22?????(i?j)(??)3i?j?1?? （2.6-22） ?Δθm(i)?Δθm(j)???cosΩ?tm?1?h??22??????22??4sin?sinsin(i?j)???2????上式中x和y轴分量是随时间tm?1呈正弦波动的，而z轴分量是与子样数间隔(i?j)相关的小量常值。可见，在圆锥运动条件下，不同子样间的叉乘积在z轴（锥轴）方向可提供一定的角增量补偿作用，所以一般使用时间段[tm?1,tm]内所有子样之间的叉乘积之和来对式（2.6-19）作估计和补偿，记为 j?1*?(T)?N??kΔθ(i)?Δθ(j) （2.6-23）

??j?2i?1ijmm*其中kij为待定系数，共有N(N?1)/2个系数，常称为圆锥误差补差系数。

注意到，式（2.6-22）中z轴分量与绝对时间tm?1无关，只与子样数间隔(i?j)有关，比如有

Δθm(1)?Δθm(2)?Δθm(2)?Δθm(3)??Δθm(N?2)?Δθm(N)、

?Δθm(N?1)?Δθm(N)、Δθm(1)?Δθm(3)?Δθm(2)?Δθm(4)?

、Δθm(1)?Δθm(N?1)?Δθm(2)?Δθm(N)等等，因而式（2.6-23）中所有子

样叉乘积的项数可由N(N?1)/2项降低为N?1项，即式（2.6-23）可简化为

?(T)?N?1kΔθ(i)?Δθ(N) （2.6-24） ???i?1N?imm且有kN?i??i**kk，系数与之间的关系参见图2.6-1。 kN?ij(j?N?i)ijj?1ji

12*123kk?*13*234kkk?*14*24*34Nkkk?*1N*2N*3NkN?1?k1*N

**kN?2?k1(N?1)?k2N123N?1?k?

***k2?k13?k24?k35?***k1?k12?k23?k34?k(*N?2)Nk(*N?1)Nk(*N?1)N*图2.6-1 系数ki与kij之间的关系

根据式（2.6-22）可知，以式（2.6-24）估计式（2.6-19），在x轴和y轴分量上是高阶的微幅振荡（(??)3量级），但这些误差是可忽略的，不会引起姿态累积漂移，因而后续主要考虑z轴分量的影响。

由??Ωh和T?Nh，可得ΩT?N?，将式（2.6-19）中的z轴分量用泰勒级数展开，得

????z(T)?2sin2(ΩT?sinΩT)?2sin2(N??sinN?)22?N3?3N5?5?2sin?N???N????2?3!5!?2???????? （2.6-25）

N5?5?4sin???2?2?3!2?5!2??N3?3??2??4sin(?1)i?1ci?2i?1??i?12?36

其中

N2i?1 （2.6-26） ci?2?(2i?1)!而将式（2.6-22）代入（2.6-24），得z轴分量估计值为

?(T)??4sin2????i?1kN?isin2zN?1?2sin(i?N)??4sin2??i?1kN?isin2?4sin2??j?1kjsin2N?1N?1?2sin(N?i)? （2.6-27）

?2sinj?利用三角函数的三重积化和差公式

1sinx?siny?sinz??sin(?x?y?z)?sin(x?y?z)?sin(x?y?z)?sin(x?y?z)? （2.6-28）

4则在式（2.6-27）的求和项中有

??1sinsinsinj???sinj??sinj??sin(1?j)??sin(1?j)??224 （2.6-29）

1??2sinj??sin(j?1)??sin(j?1)??4将式（2.6-29）代入式（2.6-27），并进行泰勒级数展开，可得

?(T)?sin2?N?1k?2sinj??sin(1?j)??sin(1?j)????z?j?1j2i?1?(j?1)2i?1?(j?1)2i?12i?12i?12j?sin??j?1kj?i?1(?1)?(2i?1)!注意到，当i?1时有2j2i?1?(j?1)2i?1?(j?1)2i?1?0，因而上式可改为

N?1? （2.6-30）

?(T)?sin2???z22j2i?1?(j?1)2i?1?(j?1)2i?12i?1??j?1kj?i?1(?1)(2i?1)!N?1?iN?1?i?1?sin??j?1kj?i?1(?1)?N?1(j?1)2i?1?(j?1)2i?1?2j2i?12i?1 （2.6-31）

?(2i?1)!?sin2??i?1(?1)i?1?j?1Aijkj?2i?1其中

(j?1)2i?1?(j?1)2i?1?2j2i?1 （2.6-32） Aij?(2i?1)!由于半锥角?是小量，在式（2.6-25）中可进行近似4sin2（2.6-31），令两式中关于?,?,其中，A??Aij?、k??kj?35?,?2N?12项的对应系数相等，则可建立矩阵方程

?sin2???2，再对比式（2.6-25）和

Ak?c （2.6-33）

(N?1)?(N?1)(N?1)?1和c??ci?(N?1)?1，通过求解方程（2.6-33）便可确定出待定误差补

偿系数kj。在式（2.6-25）中关于?未补偿的最低次幂项为?2N?1?(ΩT/N)2N?1，可见，在圆锥运动条件下算法的误差量级为O(T2N?1)。若以漂移角速率（rad/s）表示圆锥补偿的剩余误差，定义如下

1?12??N????(T)???(T)??(ANk?2N?1?cN?2N?1)zz??TT12?2(ΩT)2N?1 (N?1)2N?1 （2.6-34） ?(ANk?cN)?(Ωh)?(ANk?cN)2N?1TNT?2(ΩT)2N?1??NT2N?1其中，AN??，称为误差漂移系数。 ??(Ak?c)/N?AAANNNN1N2N(N?1)??表2.6-1给出了N?1~10子样算法的误差补偿系数以及对应的误差漂移系数，由表中误差系数可知，只要?(ΩT)

22N?1/T?1°/h则二子样算法能够满足绝大多数惯性级导航系统的算法精度要求。例如，当

37

2525、N?2时，有?(ΩT)/T= 0.0628°/h，此时?2?(ΩT)/T的影响可忽略Ω=10Hz、T=0.01、?=1°

2323不计；而当N?1时，有?(ΩT)/T= 6.28°/h，这时?1?(ΩT)/T的影响不可忽略，或者说，单子样

算法不能达到惯性级系统的要求。

N

1

2 3 4 5 6 7 8 9 10

k1

- 0.667 1.350 2.038 2.728 3.419 4.111 4.083 5.495 6.178

表2.6-1 1~10子样的圆锥误差补偿系数及误差漂移系数

k3 k5 k6 k7 k8 k9 k2 k4

0.450 0.876 1.290 1.696 2.097 2.495 2.891 3.285

0.514 1.042 1.579 2.124 2.676 3.231 3.790

0.496 0.987 1.471 1.951 2.426 2.898

0.501 1.004 1.510 2.018 2.529

0.500 0.999 0.500 1.497 1.000 0.500 1.993 1.501 1.000 0.500

?N

8.333E-02 1.042E-03 4.899E-06 1.211E-08 1.847E-11 1.912E-14 1.432E-17 8.119E-21 3.606E-24 1.289E-27

C G Park（1996年）经过仔细推导，给出了以分数形式表示的圆锥误差补偿系数精确解，如表2.6-2所列。同时，Park还给出剩余误差系数的解析表达式如下

N! （2.6-35） ?N?2NN?1N?1N2?k?1(2k?1)N

1 2 3 4 5 6

表2.6-2 1~6子样的圆锥误差补偿系数及误差漂移系数的分数解

k3 k5 ?N k1 k2 k4 - 2/3 27/20 214/105

9/20 92/105

54/105

1/12 1/960 1/204120 1/82575360

1375/504 650/504 525/504 250/504 1/54140625000

15797/4620 7834/4620 7296/4620 4558/4620 2315/4620 1/52295018840064

在表2.6-1和表2.6-2中，误差补偿系数ki(i?1,2,子样算法为例，等效旋转矢量计算公式为

?(T)?(T)?Δθ???m,N?1)表示间隔为i的两子样叉乘的系数，以四

??Δθm(1)?Δθm(2)?Δθm(3)?Δθm(4)???k3Δθm(1)?k2Δθm(2)?k1Δθm(3)??Δθm(4)92214?54???Δθm(1)?Δθm(2)?Δθm(3)?Δθm(4)???Δθm(1)?Δθm(2)?Δθm(3)??Δθm(4)105105?105?（2.6-36）

值得注意的是，在前述圆锥误差补偿系数的推导过程中进行了如下几点近似：①式（2.6-15）在假设?和ΩT为小量时对理论等效旋转矢量进行近似；②式（2.6-22）忽略了非圆锥轴振荡对圆锥误差补偿的影响；③在式（2.6-25）中再次假设?为小量。因此，当圆锥运动的锥角比较大时，表2.6-1中的误差漂移系数可能变得不准确。在实际系统中，陀螺仪的测量分辨率或噪声、幅相特性不理想及数据间不同步都会影响到理论上的圆锥误差补偿效果，此外，实际载体的剧烈角运动还会激励出陀螺仪的动态误差，动态误差可能远远大于算法引起的误差，致使多子样圆锥误差补偿往往达不到预期的效果，所以实际应用时子样数并非越多越好，建议最多选用3～4子样就足够了。

对比本节圆锥误差补偿多子样算法与2.5节基于泰勒级数展开的多子样算法，理论上，前者比后者更适合应用于圆锥运动环境，而后者比前者更适合应用于多项式角运动环境。对于实际系统，在角运动过程中，通常认为多项式角运动只会短暂出现，而更容易激发的是较长时间的周期性振动，它可近似为

38

圆锥运动，因此实际中一般优先考虑采用基于圆锥误差补偿的多子样算法。相对于2.5节而言，本节在圆锥运动假设条件下获得的圆锥误差补偿算法也常常称为多子样优化算法。

最后指出的是，有些文献将圆锥运动的角速度定义为

?aΩsinΩt?? （2.6-37） ω(t)???bΩcosΩt???0??这相当于在式（2.6-1）中取c?0，此时就不能够得到相应的等效旋转矢量和四元数的简单解析表达式了。

与式（2.6-37）对应的角增量为

??a(cosΩt?cosΩtm?1)?? （2.6-38） Δθ(t,tm?1)??ω(?)d???b(sinΩt?sinΩt)m?1??tm?1??0??将式（2.6-37）和（2.6-38）代入不可交换误差式（2.5-18），可得

??a(cosΩ??cosΩtm?1)??aΩsinΩ??1tm1tm??bΩcosΩ??d? σ(tm,tm?1)??Δθ(?,tm?1)?ω(?)d????b(sinΩ??sinΩtm?1)?????2tm?12tm?1???00?????t??01?? ???0?d?2tm?1??abΩ?(cosΩ??cosΩtm?1)cosΩ??(sinΩ??sinΩtm?1)sinΩ????tm????00abtm?????0d???0????2tm?1???Ω?1?cosΩ(??tm?1)????ab/2??Ω(tm?tm?1)?sinΩ(tm?tm?1)??? （2.6-39）

0?????0????ab/2?(ΩT?sinΩT)??当角度幅值a?b??且为小量时，上式与式（2.6-19）的结果完全相同，后续圆锥误差补偿系数的求解方法和结果也与前文完全一致，无需赘述。

39

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

第?章??地球形状与重力场基础 ......................................................................................... 40

3.1地球的形状描述 ........................................................................................................................ 40

3.2地球的正常重力场 ..................................................................................................................... 46 3.3地球重力场的球谐函数展开 ...................................................................................................... 50

3.3.1 球谐函数 .................................................................................................................................... 50 3.3.2 地球引力位函数......................................................................................................................... 58 3.3.3 重力位及重力计算..................................................................................................................... 63

3.1地球的形状描述

实际的地球表面是一个凹凸不平、形状十分复杂的物理面，难以准确量化描述。为了研究方便，假想海洋表面静止，并将其向陆地延伸，所得到的封闭曲面称为大地水准面，大地水准面包围的形体称为大地水准体。由于地球内部密度分布不均匀和表面形状起伏的影响，大地水准体也是一个不规则的几何体。实用中希望使用比较简单的数学方程来拟合地球几何形状，按精度从低到高有如下三种近似：①近似成圆球体，中心选择在地球质心上，半径约6371km，该描述比较粗略，适用于对精度要求不高的场合；②近似成旋转椭球体，地球自转轴（极轴）与一椭圆短半轴重合，椭圆的椭圆度约1/300（长短半轴相差约21km），椭圆绕其短半轴旋转构成旋转椭球体的表面，该描述中地球赤道是圆形的；③近似成三轴椭球体，其中赤道是椭圆的，赤道椭圆度约1/100000（长短半轴相差约60m）。三轴椭球体描述虽然比旋转椭球体更精确，但前者的相关计算比后者复杂许多，考虑到三轴椭球体的赤道椭圆度不大，可将其似成圆形的，因此旋转椭球体应用最为广泛。

下面先简要介绍一下地球旋转椭球上的一些基本概念。

参见图3.1-1，地球自转轴的南端点和北端点分别称为南极（S）和北极（N），包含南北极点的平面称为子午面，子午面与旋转椭球面的交线称为子午圈（或经圈）。通过英国格林尼治的经线称为本初子午线（或零度经线）。任一经线所在子午面与本初子午面之间的夹角，定义为经度（记为?），夹角方向与地球自转轴同方向，取值范围-180°～180°。包含旋转椭球中心且垂直于自转轴的平面称为赤道面，赤道面与旋转椭球面的交线称为赤道，平行于赤道面的平面与椭球面的交线称为纬圈。

NZeRpZezPπ?L2O?XeYeO?SLxReBXeQ

图3.1-1 旋转椭球基本概念 图3.1-2 子午圈椭圆 对于地球旋转椭球体而言，确定其三维形状参数的关键在于确定二维子午圈椭圆。 1 子午圈椭圆

S

参见图3.1-1，建立地心右手直角坐标系，常称为地心地固坐标系（Earth Center Earth Fixed，ECEF），

40

坐标原点选在地心，OZe轴为自转轴且指向北极，OXe轴指向赤道与本初子午线的交点，OYe轴在赤道平面内且指向90°经线，ECEF系与地球固联，即跟随地球自转一起相对惯性空间转动。对子午圈椭圆，不失一般性，选择本初子午线椭圆作为研究对象，如图3.1-2所示。椭圆上任一P点与地心连线PO与OXe轴的夹角称为地心纬度，记为?，取值范围-90°～90°，南纬为负北纬为正。过P点的椭圆法线PQ与OXe轴的夹角称为地理纬度，简称纬度，记为L，取值范围-90°～90°。此外，与地心纬度对应的方向PO称为地心垂线，而与地理纬度对应的方向PQ称为地理垂线。

椭圆形状完全由其长半轴和短半轴确定，但在涉及椭圆的计算中，为了方便常引入扁率和偏心率概念。

椭圆方程为

x2z2?2?1 （3.1-1） Re2Rp其中，Re和Rp分别为椭圆长半轴和短半轴。

椭圆扁率（或称椭圆度，flattening）定义为

f?椭圆偏心率（eccentricity）定义为

Re?RpRe （3.1-2）

e?第二偏心率定义为

2Re2?RpRe （3.1-3）

e??2Re2?RpRpe2 且有 e?? （3.1-4） 21?e2相对于第二偏心率而言，式（3.1-3）有时也称e为第一偏心率。

由式（3.1-2）和（3.1-3）可分别得

Rp?(1?f)Re （3.1-5）

Rp?Re1?e2 （3.1-6）

比较上述两式，可得

f?1?1?e2 和 e2?2f?f2 （3.1-7）

将椭圆方程（3.1-1）两边同时对x求导，并考虑到式（3.1-6），得

2x2z?dz/dx?2?0 （3.1-8） 22ReRe(1?e)上式移项整理得

dzx??(1?e2) （3.1-9） dxzdz表示椭圆在P点的切线PB的斜率，显然，切线PB与法线PQ之间是相互垂直的，法线PQ的斜dx率为tanL，则有

dzxtanL??(1?e2)?tanL??1 （3.1-10） dxz从上式可解得

式中

z?x(1?e2)tanL （3.1-11）

将式（3.1-6）和式（3.1-11）代入椭圆方程（3.1-1），可求得以地理纬度L为参数的椭圆参数方程

41

Re?x?cosL?221?esinL? （3.1-12） ?2?z?Re(1?e)sinL?1?e2sin2L?参见图3.1-2，记线段长度PQ?RN，则有

x?RNsin?SQO?RNcosL （3.1-13）

比较式（3.1-13）和式（3.1-12）中的第一式，可得

Re （3.1-14） RN?221?esinL因而参数方程（3.1-12）可简写为

??x?RNcosL （3.1-15） ?2??z?RN(1?e)sinL最后，比较一下地球表面上同一点的地理纬度与地心纬度之间的差别，或者说，地理垂线与地心垂线之间的偏差。

对于地心纬度，注意到tan??z/x，根据式（3.1-11），有

tan??(1?e2)tanL （3.1-16）

记地理纬度与地心纬度之间的偏差量?L?L??，则有

tanL?tan?tan?L?tan(L??)?1?tanLtan??将?L和e视为小量，上式可近似为

tanL?(1?e)tanLesinLcosL?1?tanL(1?e2)tanL1?e2sin2L2222 （3.1-17）

e2(1?e2sin2L)?L?esinLcosL(1?esinL)?sin2L （3.1-18）

22或者

e2?L?sin2L?fsin2L （3.1-19）

22其中，根据式（3.1-7）近似取f?e/2。例如，若取椭球扁率f?1/298.257，则在纬度L=45°处可求得地理纬度与地心纬度的最大偏差值约为?L?11.5′。

2 旋转椭球表面的曲率半径

导航过程中，运载体在地球椭球表面附近移动，为了在合适的坐标系（通常指地理坐标系）下进行三维定位解算，旋转椭球表面的几何曲率半径是一个非常重要的参数。

首先给出法截线的概念。参见图3.1-3，包含椭球面上某P点法线PQ的平面称为法截面，法截面与子午面之间的夹角记为A，法截面与椭球的交线称为法截线。显然，当法截面包含南北极点时，法截线即为子午圈；当法截面垂直于子午面时，法截线称为卯酉圈。

ZeX'PAZ'Y'OYeQXe图3.1-3 法截线及其局部坐标系

42

在图3.1-3中，不失一般性，假设P点在本初子午线上，以P为坐标原点建立局部直角坐标系

PX'Y'Z'，其中PX'轴沿法线向外，PZ'轴沿法截线切线方向。不难看出，若坐标系PX'Y'Z'先绕PX'轴转动A角度，然后绕PY'轴转动L角度，再作相应平移，则可得OXeYeZe坐标系（即地心直角坐标系）。根据椭圆参数方程（3.1-12）知P点在OXeYeZe坐标系下的坐标为??RNcosL0RN(1?e2)sinL??，此

T即前述平移的坐标值，所以PX'Y'Z'和OXeYeZe两坐标系之间的坐标变换关系为

00??x'??RNcosL??x??cosL0?sinL??1?y???0??0cosAsinA??y'???? （3.1-20） 100??????????2??z????sinL0cosL????0?sinAcosA????z'????RN(1?e)sinL??x2?y2z2将上式代入旋转椭球方程?2?1，移项整理得 2ReRpx?2?z?2?2RNz??e?2(1?e?2)(x?cosAcosL?z?sinL)2?0 （3.1-21）

由于在PX'Y'Z'局部坐标系下表示的法截线方程必有y??0，因而上式中不含y?项。

将式（3.1-21）对坐标x?求一次导和二次导，代入平面曲线的曲率计算公式，可得法截线在P点的曲率

1RA?2dz?/dx?2dz?2??1?()???dx??3/2?P?(x?,z?)?(0,0)RN （3.1-22）

1?e?2cos2Acos2L特别地，当角度A?0或π/2时，分别有

RNRN(1?e2)Re(1?e2) （3.1-23） RM?RA?0???2222223/21?e?cosL1?esinL(1?esinL)RRe （3.1-24） RA?π/2?N?221?01?esinL通常称RM?RA?0为子午圈主曲率半径；而称RN?RA?π/2为卯酉圈主曲率半径。在图3.1-2中卯酉圈曲

率半径即对应于线段PQ的长度。除在地理纬度L??π/2处有RM?RN外，其它纬度处总有RM?RN。 3 大地坐标及其变化率

在旋转椭球表面上P点处，纬圈切线与经圈切线相互垂直，且两切线同时垂直于椭球面的法线。在椭球表面上定义直角坐标系o0x0y0z0：P点为坐标原点（重记为o0），纬圈切线指东、经圈切线指北、椭球面法线指天分别为o0x0轴、o0y0轴和o0z0轴的正向。参见图3.1-4，若某点og在坐标系o0x0y0z0中的直角坐标为og(0,0,h)，显然og在椭球面P点的法线上，h称为og点的地理高度。以og为原点建立坐标系ogxgygzg，其三轴分别平行于o0x0y0z0的同名坐标轴，称ogxgygzg为当地地理坐标系，简记为g系。

og点相对于地球椭球的空间位置可用所谓的大地坐标（地理坐标）表示，记为og(?,L,h)。

vyogvxhxvy0vx0RMogP(o0)?LP(o0)LLRNQ（a）经度变化率 （b）纬度变化率

图3.1-4 速度引起的经纬度变化

43

在图3.1-4中，如果o0点对地球坐标系OXeYeZe的速度在o0x0y0z0系的投影记为

o0ve??o0?v化，有

x0vy00??。注意到，o0x0轴与纬圈相切，两者在同一个平面内，因而vx0仅会引起经度的变

T??vx0x?vx0RNcosL （3.1-25）

同理，o0y0轴与经圈相切，两者在同一平面内，因而速度vy0仅会引起纬度的变化，考虑到P点所在子午圈的曲率半径为RM，则有

L?vy0RM （3.1-26）

g对于地理高度为h的og点，假设其速度为veg???vxvyvz??，根据图3.1-4中几何关系，有

Tvx （3.1-27）

RNRN?hvy0vy （3.1-28） ?RMRM?h?g上述两式分别代入式（3.1-25）和（3.1-26），并考虑到天向速度vz仅引起地理高度h变化，得速度veg与

vx0大地坐标(?,L,h)之间的关系，分别为

??vx （3.1-29）

(RN?h)cosLL?vyRM?h （3.1-30）

h?vz （3.1-31）

地理坐标系ogxgygzg与地球坐标系OXeYeZe之间的转动关系可以用方向余弦阵表示，常称之为

e位置矩阵，记为Cg。参见图3.1-5，g系先绕Z轴转动?π/2，接着绕Y轴转动?(π/2?L)，再绕Z轴转动??，这时g系三轴可与e系相应坐标轴平行。据此，可计算得位置矩阵

ZeYgZgg?e(1):Rot(Zg,?π/2)(2):Rot(Yg',?(π/2?L))\(3):Rot(Zg,??)PLXgYe?XeO图3.1-5 g系至e系的三次转动

?cos(??)sin(??)0??cos(?(π/2?L))0?sin(?(π/2?L))????eCg???sin(??)cos(??)0010?????001?????sin(?(π/2?L))0cos(?(π/2?L))?? ?cos(?π/2)sin(?π/2)0?????sin(?π/2)cos(?π/2)0???001??? 44

?cos??sin?0??sinL0cosL??0?10???0??100???sin?cos?010???????01??0????cosL0sinL????001?? （3.1-32） ??sin??sinLcos?cosLcos?????cos??sinLsin?cosLsin????cosLsinL??0?对上式两边同时微分，得

???cos??(LcosLcos???sinLsin?)?LsinLcos???cosLsin????eCg????sin??(LcosLsin???sinLcos?)?LsinLsin???cosLcos???0??LsinLLcosL?? （3.1-33）

???L??cosLcos???0??sinL?cosL?????e???sinLsin?cosLsin???sinL0L?C?cosLg???????????sinL??cosLsinL??L0????????cosL???eeg上式与方向余弦阵微分公式Cg并将经纬度公式（3.1-29）和（3.1-30）代入，分别记vx,vy?Cg(ωeg?)对比，

??sin????cos???0?sinLcos?为vE,vN，可得

?vN ω???L?cosL?sinL???????RM?hgegTvERN?h?vEtanL? （3.1-34） RN?h?T这便是载体运动线速度引起导航系旋转角速度的计算公式。 4 大地坐标与地心直角坐标之间转换

下面给出同一地点的地理坐标(?,L,h)与地心直角坐标(x,y,z)之间的相互转换关系。 （1）由(?,L,h)求解(x,y,z)

根据式（3.1-15）和图3.1-2，不难求得

?x?(R?h)cosLcos?N???y?(RN?h)cosLsin? （3.1-35） ?2?z???RN(1?e)?h??sinL?其中，子午圈曲率半径RN的计算见式（3.1-14）或重写如下

RN?Re1?esinL22 （3.1-36）

（2）由(x,y,z)求解(?,L,h)

首先，由式（3.1-35）中第二式除第一式，得

ysin? （3.1-37） ?xcos?由上式可直接解得经度

??atan2(y,x) （3.1-38）

其次，对于纬度，不能求得显式表示，通常采用迭代算法，推导过程如下： 由式（3.1-35）中第一式和第二式实施如下运算

(RN?h)cosL?x2?y2 （3.1-39）

由式（3.1-35）中第三式移项整理，可得

(RN?h)sinL?z?RNe2sinL （3.1-40）

式（3.1-40）除以式（3.1-39），得

z?RNe2sinL （3.1-41）

tanL?x2?y2

45

将式（3.1-36）改写成RN?RecosL1?(1?e)tanL122，再代入上式，得

??Ree2tanLtanL??z?? （3.1-42）

2222x?y??1?(1?e)tanL??如记t?tanL，则由上式可构造出求解纬度正切值的迭代公式，如下

??Ree2ti1?z?? （3.1-43） ti?1?2222x?y?1?(1?e)ti???令迭代初值t0?0，一般经过5～6次迭代便可达到足够的数值计算精度，再求反正切即可获得纬度L。

最后，根据（3.1-39）求解高度，得

x2?y2h??RN （3.1-44）

cosL式（3.1-38）、（3.1-43）和（3.1-44）即为由(x,y,z)求解(?,L,h)的算法。

3.2地球的正常重力场

在地球的大地水准体描述中，水准体表面是地球实际重力场的一个等位面，每一点的重力方向均与该点所在等位面相垂直，实际的重力方向一般称为天文垂线，或称真垂线。由于实际地球内部密度分布不均匀，并且表面凹凸不平，大地水准面不规则，因而实际重力的大小和方向也不规则。与地球的几何形状描述类似，也希望使用一个简单的数学函数来拟合地球重力场，这个简单函数表示的重力场就称为正常重力场。

重力是地球万有引力和离心力共同作用的结果，参见图3.2-1，在P点处，重力矢量G是引力矢量F和离心力矢量F?的合力。

ωieP图3.2-1 重力矢量

1．圆球假设下的地球重力

若将地球视为圆球体并且认为密度均匀分布，那么地球引力指向地心，根据牛顿万有引力定律，地球对其表面或外部单位质点的引力大小为

F?GM??2 （3.2-1） r2r其中，G为万有引力常数，M为地球质量，记?=GM为地心引力常数，r为质点至地心的距离。

由于地球绕其极轴存在自转角速率?ie，使得与地球表面固连的单位质点受到离心力的作用，其大小为

2F???ieRcosL （3.2-2）

其中，R为圆球半径，L是地理纬度（在圆球假设中即为地心纬度）。重力是引力与离心力的合力，引力与离心力之间的夹角为π?L，根据余弦定理，在纬度为L的地方上P点的重力大小为

46

gL?F2?F?2?2FF?cos(π?L)22?F2?(?ieRcosL)2?2F?ieRcos2L?(F??R)?2F?RsinL?(?R)sinL22?(F?1/2?ieR)?ieR2??(F??R)?1?2sinL?22(F??R)ie??2ie22??ieR2??ge1?2sinL?ge?1?sinL?gege??22?ieR2ie22ie22ie22 （3.2-3）

22其中，ge?F??ieR为赤道上的重力大小，?ieR/ge为赤道上的离心力与重力加速度的比值。

实践表明，基于圆球假设的重力公式（3.2-3）与实际椭球地球相比，在高纬度地区偏小将近2mg，部分原因归结于实际椭球地球在高纬度地区半径缩小，实际引力增大。为了更精确地计算正常重力值，需要在椭球条件下进行重力推导。 2．旋转椭球假设下的地球重力

对于地球旋转椭球体，假设在椭球表面上重力矢量处处垂直于表面，也就是说，旋转椭球表面为重力的一个等位面，意大利人索密里安（Somigliana）于1929年经过严密推导（过程比较复杂，从略），获得了旋转椭球体的正常重力公式，如下

Regecos2L?Rpgpsin2L （3.2-4） gL?2222RecosL?RpsinL其中，Re和Rp分别为旋转椭球的赤道长半轴和极轴短半轴，ge和gp分别为赤道重力和极点重力，gL为地理纬度L处椭球表面的重力大小。 对于赤道重力ge和极点重力gp，近似有

?33?g?(1?m?mf)e?RR27?ep （3.2-5） ??g??(1?m?6mf)p?Re27?其中，f为旋转椭球几何形状扁率，约等于1/298；并且

m?2?ieRe?/(ReRp)?2?ieRe （3.2-6）

ge为赤道上的离心力与重力加速度的比值，约等于1/288。

记地球重力扁率为

??gp?gege （3.2-7）

其实际值约等于1/189。将式（3.2-5）代入上式，并展开成关于m和f级数形式，可得

66R(1?m?mf)(1?f)(1?m?mf)pgp?gegp77????1??1??13333gegeRe(1?m?mf)1?m?mf2727163333???(1?m?f?mf?mf2)?1?(m?mf)?(m?mf)2???1 （3.2-8）

772727??51715?m?f?mf?m2?2144式（3.2-8）建立了重力扁率?与椭球形状扁率f之间的关系，若忽略高阶小量，近似有

5??m?f （3.2-9）

2

47

上式是利用重力测量方法确定地球形状参数f的基本公式，它最早在1743年由法国数学家克莱罗（A.C. Clairaut）求得，通常称为克莱罗公式。克莱罗在推导公式（3.2-9）时作了如下前提假设：地球是由密度不同的均匀物质层圈组成的椭球体，各椭球面都是重力等位面，且各层密度由地心向外有规律的减小。需要说明的是，克莱罗公式是近似成立的公式，而索密里安公式却是理论上严格成立的，并且后者的前提条件更加宽松，对椭球体密度分布不做任何限制。

若将gp?(1??)ge和Rp?(1?f)Re代入索密里安公式（3.2-4），展开为关于?和f的级数形式，可得

gL?gecos2L?(1?f)(1??)sin2LcosL?(1?f)sinL22222?1?(2f?f)sinL???23?122222????ge?1?(??f??f)sinL1?(2f?f)sinL?(2f?f)sinL???2???8??ge1?(??f??f)sin2L1/2??? （3.2-10）

??1??ge?1??(??f??f)?(2f?f2)?sin2L2???1?3???(2f?f2)2?(??f??f)(2f?f2)?sin4L?2?8?11??ge?1?(???f?f2)sin2L?(?f?f2?22?考虑到如下三角函数恒等式

将上式移项，有

??????)sin4L?sin22L?(2sinLcosL)2?4sin2L(1?sin2L)?4sin2L?4sin4L （3.2-11）

1sin4L?sin2L?sin22L （3.2-12）

4再将式（3.2-12）代入式（3.2-10），忽略高阶小量，可得

111??gL?ge?1?(???f?f2)sin2L?(?f?f2)(sin2L?sin22L)?224??1?? （3.2-13） ?ge?1??sin2L?(2?f?f2)sin22L?8???ge(1??sin2L??1sin22L)其中

?1?(2?f?f2) （3.2-14）

若将克莱罗公式（3.2-9）代入上式，还可得 1?5?1?1??2(m?f)f?f2??(5mf?f2) （3.2-15）

8?2?8式（3.2-13）为索密里安公式的良好近似（最大误差约为12μg），实用中的正常重力模型常常是以该形式给出的，它比式（3.2-4）的计算量稍小些。历史上曾给出过以下一些重要的正常重力模型。

（1）1901年，德国人赫尔默（Helmert）根据当时波斯坦系统的几千个重力测量结果，给出正常重力公式为：

18gL?9.7803?(1?0.005302?sin2L?0.000007?sin22L)相应的参考椭球的扁率f?1/298.3。

(m/s2) （3.2-16）

上式称为赫尔默正常重力公式，其中重力扁率??0.005302?1/188.6，利用克雷诺定理，可以计算出

（2）1909年，美国人海福特（Hayford）根据美国当时的大地测量结果给出了一个参考椭球，它的赤道半径Re?6378388m和几何扁率f?1/297.0；1928年，芬兰人海斯卡宁（Heiskanen）根据当

48

2时的重力测量结果计算出正常场地球模型在赤道上的重力为ge?9.78049m/s。若取地球自转角速率

?ie?7.2921151?10?5rad/s，根据上述四个独立参数，可计算得

gL?9.78049?(1?0.0052884?sin2L?0.0000059?sin22L)(m/s2) （3.2-17）

1930年，国际大地测量与地球物理联合会（IUGG）将上式定为国际正常重力公式。

（3）利用现代卫星测量技术，IUGG于1979年通过了1980大地参考系，与其对应的正常重力公式为

gL?9.780327?(1?0.00530244?sin2L?0.00000585?sin22L)(m/s2) （3.2-18）

（4）1987年，WGS-84（World Geodetic System 1984）大地坐标系给出的地球参数为：半长轴

Re?6378137m，扁率f?1/298.257223563，地心引力常数（含大气层）??3.986004418?1014m3/s2，地球自转角速率?ie?7.2921151467?10?5rad/s。如不考虑大气层影响，可推导得正常重力公式

gL?9.780325?(1?0.00530240?sin2L?0.00000582?sin22L)3．重力与海拔高度的关系

(m/s2) （3.2-19）

在地球表面附近的重力场中，引力与离心力相比前者占主要成分，重力随海拔高度增加而减小，其变化规律与引力随距离增加而减小的规律近似相同。分析高度影响时，不妨将地球近似成圆球且质量集中在地心，地球对高度为h的单位质点的引力为

F?对上式两边同时微分，得

?(R?h)2 （3.2-20）

dF??2其中

?(R?h)3dh??2?R3dh???2dh （3.2-21）

?2?2R? （3.2-22） 3若设地心引力常数??3.986005?1014m3/s2和地球平均半径R?6371km，则有?2?3.08?10?6s?2，这表示在地球表面附近高度每升高1m，引力值（或重力值）将减小3.08?10?6m/s2（约0.3μg）。

综合式（3.2-13）和式（3.2-21），可求得地球表面附近正常重力随纬度和高度变化的实用计算公式，即在大地坐标og(L,?,h)处的重力值，记为

gLh?ge(1??sin2L??1sin22L)??2h （3.2-23）

值得注意的是，上式只给出了重力的大小，其方向应该是og点处的铅垂向（真垂线），而不是该点的地理法向（地理垂线），如图3.2-2所示。 1967年，Heiskanen给出了真垂线和地理垂线之间夹角的近似公式

???3hsin2L （3.2-24） gLh其中，?3?8.08?10?9s?2。据上式计算，在纬度L?45°处，高度每上升1km，?约增加

?98.08?10?1000/?9.8。显然，除了赤道和极点外，只有在旋转椭球表面上（0.17\h?0）真垂线与地

理垂线才能严格重合。

49

?zPoLx

图3.2-2 正常重力场的垂线偏差

当以地理坐标系（g系）作为惯性导航参考坐标系时，为了扣除重力的影响，需将重力矢量投影到地理坐标系，表示为

00?????????hsin2L? （3.2-25） gg???gsin?Lh???3????gLhcos??????gLh??上式的三个分量依次表示重力矢量在东、北和天向的投影值，同样在纬度L?45°处，高度每上升1km，北向重力分量将变化8.08?10?9?1000?0.8ug，这一影响在高空高精度惯性导航中是需要考虑和补偿的。更高精度的且与实际地球相关的重力场计算和补偿详见3.3节介绍。

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

在3.2节中正常重力场描述的是规则地球产生的重力场，但实际地球并不规则，正常重力场只是实际重力场的一个较好的近似，为了更加细致地刻画实际地球的重力场，需引入球谐函数和重力位等概念，这在高精度惯性导航系统的重力场建模和补偿中十分有用。

3.3.1 球谐函数的基本概念

1. 拉普拉斯方程

如果三元函数u(x,y,z)在空间区域?上满足如下偏微分方程 ?2u?2u?2u???0 （3.3-1） ?x2?y2?z2则称u是区域?上的调和函数，或称谐函数，上述方程称为拉普拉斯方程（Laplace equation）。

方程（3.3-1）可简记为

?u?0 （3.3-2）

其中，算符

?2?2?2??2?2?2 （3.3-3）

?x?y?z称为拉普拉斯算子。

参见图3.3-1，球面上一点u，其直角坐标(x,y,z)与极坐标(r,?,?)之间的坐标转换关系为

50

本文来源：https://www.bwwdw.com/article/tv6o.html